MRPT  2.0.4
ba_internals.h
Go to the documentation of this file.
1 /**
2  * Copyright (C) 2010 Hauke Strasdat
3  * Imperial College London
4  * Copyright (c) 2005-2020, Individual contributors, see AUTHORS file
5  * See: https://www.mrpt.org/Authors - All rights reserved.
6  *
7  * bundle_adjuster.h is part of RobotVision.
8  *
9  * RobotVision is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU Lesser General Public License as
11  * published by the Free Software Foundation, either version 3 of the
12  * License, or any later version.
13  *
14  * RobotVision is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Lesser General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * and the GNU Lesser General Public License along with this program.
21  * If not, see <http://www.gnu.org/licenses/>.
22  */
23 #pragma once
24 
25 #include <mrpt/math/CMatrixFixed.h>
26 #include <mrpt/math/CVectorFixed.h>
27 #include <mrpt/poses/CPose3D.h>
28 #include <mrpt/vision/types.h>
29 #include <Eigen/Dense>
30 #include <array>
31 #include <vector>
32 
33 // Declarations shared between ba_*.cpp files, but which are private to MRPT
34 // not to be seen by an MRPT API user.
35 
36 namespace mrpt::vision
37 {
38 using mrpt::math::CVectorFixedDouble; // Allow these "using"s since these
39  // headers are
40 // internal to mrpt
42 using std::vector;
43 
44 #define VERBOSE_COUT \
45  if (verbose) cout
46 
47 // Auxiliary struct for keeping the list of all Jacobians in a sparse, efficient
48 // way.
49 template <int FrameDof, int PointDof, int ObsDim>
50 struct JacData
51 {
52  inline JacData()
55  J_frame_valid(false),
56  J_point_valid(false)
57  {
58  }
59 
62 
63  // Jacobians of the observation wrt the camera pose & the point:
67 
68  // Needed by any struct having
69  // Eigen::Matrix<> fields
70 };
71 
72 /** The projective camera 2x6 Jacobian \f$ \frac{\partial h(f,p)}{\partial p}
73  * \f$ (wrt the 6D camera pose)
74  * \note Jacobians as described in
75  * https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
76  * (Appendix A)
77  */
78 template <bool POSES_ARE_INVERSE>
79 void frameJac(
80  const mrpt::img::TCamera& camera_params,
81  const mrpt::poses::CPose3D& cam_pose,
82  const mrpt::math::TPoint3D& landmark_global,
84 {
85  using mrpt::square;
86 
87  double x, y, z; // wrt cam (local coords)
88  if (POSES_ARE_INVERSE)
89  cam_pose.composePoint(
90  landmark_global.x, landmark_global.y, landmark_global.z, x, y, z);
91  else
92  cam_pose.inverseComposePoint(
93  landmark_global.x, landmark_global.y, landmark_global.z, x, y, z);
94 
95  ASSERT_(z != 0);
96  const double _z = 1.0 / z;
97  const double fx_z = camera_params.fx() * _z;
98  const double fy_z = camera_params.fy() * _z;
99  const double _z2 = square(_z);
100  const double fx_z2 = camera_params.fx() * _z2;
101  const double fy_z2 = camera_params.fy() * _z2;
102 
103  if (POSES_ARE_INVERSE)
104  {
105  const double xy = x * y;
106 
107  const double J_vals[] = {fx_z,
108  0,
109  -fx_z2 * x,
110  -fx_z2 * xy,
111  camera_params.fx() * (1 + square(x * _z)),
112  -fx_z * y,
113  0,
114  fy_z,
115  -fy_z2 * y,
116  -camera_params.fy() * (1 + square(y * _z)),
117  fy_z2 * xy,
118  fy_z * x};
119  out_J.loadFromArray(J_vals);
120  }
121  else
122  {
123  const mrpt::math::CMatrixDouble33& R = cam_pose.getRotationMatrix();
124 
125  const double jac_proj_vals[] = {fx_z, 0, -fx_z2 * x,
126  0, fy_z, -fy_z2 * y};
127  const mrpt::math::CMatrixFixed<double, 2, 3> jac_proj(jac_proj_vals);
128 
129  const double p_x = cam_pose.x();
130  const double p_y = cam_pose.y();
131  const double p_z = cam_pose.z();
132  const double tx = landmark_global.x - p_x;
133  const double ty = landmark_global.y - p_y;
134  const double tz = landmark_global.z - p_z;
135 
136  const double aux_vals[] = {
137  -R(0, 0),
138  -R(1, 0),
139  -R(2, 0),
140  tz * R(1, 0) - ty * R(2, 0) + R(1, 0) * p_z - R(2, 0) * p_y,
141  tx * R(2, 0) - tz * R(0, 0) - R(0, 0) * p_z + R(2, 0) * p_x,
142  ty * R(0, 0) - tx * R(1, 0) + R(0, 0) * p_y - R(1, 0) * p_x,
143  -R(0, 1),
144  -R(1, 1),
145  -R(2, 1),
146  tz * R(1, 1) - ty * R(2, 1) + R(1, 1) * p_z - R(2, 1) * p_y,
147  tx * R(2, 1) - tz * R(0, 1) - R(0, 1) * p_z + R(2, 1) * p_x,
148  ty * R(0, 1) - tx * R(1, 1) + R(0, 1) * p_y - R(1, 1) * p_x,
149  -R(0, 2),
150  -R(1, 2),
151  -R(2, 2),
152  tz * R(1, 2) - ty * R(2, 2) + R(1, 2) * p_z - R(2, 2) * p_y,
153  tx * R(2, 2) - tz * R(0, 2) - R(0, 2) * p_z + R(2, 2) * p_x,
154  ty * R(0, 2) - tx * R(1, 2) + R(0, 2) * p_y - R(1, 2) * p_x};
155  const mrpt::math::CMatrixFixed<double, 3, 6> vals(aux_vals);
156  out_J = jac_proj * vals;
157  }
158 }
159 
160 /** Jacobians wrt the point
161  * \note Jacobians as described in
162  * https://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
163  * (Appendix A)
164  */
165 template <bool POSES_ARE_INVERSE>
166 void pointJac(
167  const mrpt::img::TCamera& camera_params,
168  const mrpt::poses::CPose3D& cam_pose,
169  const mrpt::math::TPoint3D& landmark_global,
171 {
172  using namespace mrpt::math;
173 
174  mrpt::math::TPoint3D l; // Local point, wrt camera
175 
177 
178  if (POSES_ARE_INVERSE)
179  cam_pose.composePoint(
180  landmark_global.x, landmark_global.y, landmark_global.z, l.x, l.y,
181  l.z, dp_point);
182  else
183  cam_pose.inverseComposePoint(
184  landmark_global.x, landmark_global.y, landmark_global.z, l.x, l.y,
185  l.z, dp_point);
186 
187  ASSERT_(l.z != 0);
188  const double _z = 1.0 / l.z;
189  const double _z2 = square(_z);
190 
191  const double tmp_vals[] = {
192  camera_params.fx() * _z, 0,
193  -camera_params.fx() * l.x * _z2, 0,
194  camera_params.fy() * _z, -camera_params.fy() * l.y * _z2};
195  const mrpt::math::CMatrixFixed<double, 2, 3> tmp(tmp_vals);
196 
197  out_J = tmp * dp_point;
198 }
199 
200 // === Compute sparse Jacobians ====
201 // Case: 6D poses + 3D points + 2D (x,y) observations
202 // For the case of *inverse* or *normal* frame poses being estimated.
203 // Made inline so immediate values in "poses_are_inverses" are propragated by
204 // the compiler
205 template <bool POSES_ARE_INVERSE>
207  const TFramePosesVec& frame_poses,
208  const TLandmarkLocationsVec& landmark_points,
209  const mrpt::img::TCamera& camera_params,
210  std::vector<JacData<6, 3, 2>>& jac_data_vec, const size_t num_fix_frames,
211  const size_t num_fix_points)
212 {
213  MRPT_START
214 
215  // num_fix_frames & num_fix_points: Are relative to the order in frame_poses
216  // & landmark_points
217  ASSERT_(!frame_poses.empty() && !landmark_points.empty());
218  const size_t N = jac_data_vec.size();
219 
220  for (size_t i = 0; i < N; i++)
221  {
222  JacData<6, 3, 2>& D = jac_data_vec[i];
223 
224  const TCameraPoseID i_f = D.frame_id;
225  const TLandmarkID i_p = D.point_id;
226 
227  ASSERTDEB_(i_f < frame_poses.size());
228  ASSERTDEB_(i_p < landmark_points.size());
229 
230  if (i_f >= num_fix_frames)
231  {
232  frameJac<POSES_ARE_INVERSE>(
233  camera_params, frame_poses[i_f], landmark_points[i_p],
234  D.J_frame);
235  D.J_frame_valid = true;
236  }
237 
238  if (i_p >= num_fix_points)
239  {
240  pointJac<POSES_ARE_INVERSE>(
241  camera_params, frame_poses[i_f], landmark_points[i_p],
242  D.J_point);
243  D.J_point_valid = true;
244  }
245  }
246  MRPT_END
247 }
248 
249 /** Construct the BA linear system.
250  * Set kernel_1st_deriv!=nullptr if using robust kernel.
251  */
253  const TSequenceFeatureObservations& observations,
254  const std::vector<std::array<double, 2>>& residual_vec,
255  const std::vector<JacData<6, 3, 2>>& jac_data_vec,
257  std::vector<CVectorFixedDouble<6>>& eps_frame,
259  std::vector<CVectorFixedDouble<3>>& eps_point, const size_t num_fix_frames,
260  const size_t num_fix_points, const vector<double>* kernel_1st_deriv);
261 } // namespace mrpt::vision
void frameJac(const mrpt::img::TCamera &camera_params, const mrpt::poses::CPose3D &cam_pose, const mrpt::math::TPoint3D &landmark_global, mrpt::math::CMatrixFixed< double, 2, 6 > &out_J)
The projective camera 2x6 Jacobian (wrt the 6D camera pose)
Definition: ba_internals.h:79
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
#define MRPT_START
Definition: exceptions.h:241
uint64_t TLandmarkID
Unique IDs for landmarks.
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:175
mrpt::math::CMatrixFixed< double, ObsDim, PointDof > J_point
Definition: ba_internals.h:65
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:177
TCameraPoseID frame_id
Definition: ba_internals.h:60
void ba_compute_Jacobians(const TFramePosesVec &frame_poses, const TLandmarkLocationsVec &landmark_points, const mrpt::img::TCamera &camera_params, std::vector< JacData< 6, 3, 2 >> &jac_data_vec, const size_t num_fix_frames, const size_t num_fix_points)
Definition: ba_internals.h:206
A complete sequence of observations of features from different camera frames (poses).
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void loadFromArray(const VECTOR &vals)
Definition: CMatrixFixed.h:171
std::vector< mrpt::poses::CPose3D > TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
This base provides a set of functions for maths stuff.
void pointJac(const mrpt::img::TCamera &camera_params, const mrpt::poses::CPose3D &cam_pose, const mrpt::math::TPoint3D &landmark_global, mrpt::math::CMatrixFixed< double, 2, 3 > &out_J)
Jacobians wrt the point.
Definition: ba_internals.h:166
Classes for computer vision, detectors, features, etc.
Definition: CDifodo.h:17
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:190
#define MRPT_END
Definition: exceptions.h:245
void ba_build_gradient_Hessians(const TSequenceFeatureObservations &observations, const std::vector< std::array< double, 2 >> &residual_vec, const std::vector< JacData< 6, 3, 2 >> &jac_data_vec, std::vector< mrpt::math::CMatrixFixed< double, 6, 6 >> &U, std::vector< CVectorFixedDouble< 6 >> &eps_frame, std::vector< mrpt::math::CMatrixFixed< double, 3, 3 >> &V, std::vector< CVectorFixedDouble< 3 >> &eps_point, const size_t num_fix_frames, const size_t num_fix_points, const vector< double > *kernel_1st_deriv)
Construct the BA linear system.
CVectorFixed< double, N > CVectorFixedDouble
Specialization of CVectorFixed for double numbers.
Definition: CVectorFixed.h:32
mrpt::math::CMatrixFixed< double, ObsDim, FrameDof > J_frame
Definition: ba_internals.h:64
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:225
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
TLandmarkID point_id
Definition: ba_internals.h:61



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020