Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, Individual contributors, see AUTHORS file
5  * See: http://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 #ifndef ba_internals_H
24 #define ba_internals_H
25 
28 #include <mrpt/poses/CPose3D.h>
30 #include <mrpt/vision/types.h>
31 
32 #include <array>
33 
34 // Declarations shared between ba_*.cpp files, but which are private to MRPT
35 // not to be seen by an MRPT API user.
36 
37 namespace mrpt
38 {
39 namespace vision
40 {
41 using mrpt::math::CArrayDouble; // Allow these "using"s since these headers are
42 // internal to mrpt
44 using std::vector;
45 
46 #define VERBOSE_COUT \
47  if (verbose) cout
48 
49 // Auxiliary struct for keeping the list of all Jacobians in a sparse, efficient
50 // way.
51 template <int FrameDof, int PointDof, int ObsDim>
52 struct JacData
53 {
54  inline JacData()
57  J_frame_valid(false),
58  J_point_valid(false)
59  {
60  }
61 
64 
65  // Jacobians of the observation wrt the camera pose & the point:
69 
70  MRPT_MAKE_ALIGNED_OPERATOR_NEW // Needed by any struct having
71  // Eigen::Matrix<> fields
72 };
73 
74 /** The projective camera 2x6 Jacobian \f$ \frac{\partial h(f,p)}{\partial p}
75  * \f$ (wrt the 6D camera pose)
76  * \note Jacobians as described in
77  * http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
78  * (Appendix A)
79  */
80 template <bool POSES_ARE_INVERSE>
81 void frameJac(
82  const mrpt::img::TCamera& camera_params,
83  const mrpt::poses::CPose3D& cam_pose,
84  const mrpt::math::TPoint3D& landmark_global,
86 {
87  using mrpt::square;
88 
89  double x, y, z; // wrt cam (local coords)
90  if (POSES_ARE_INVERSE)
91  cam_pose.composePoint(
92  landmark_global.x, landmark_global.y, landmark_global.z, x, y, z);
93  else
94  cam_pose.inverseComposePoint(
95  landmark_global.x, landmark_global.y, landmark_global.z, x, y, z);
96 
97  ASSERT_(z != 0);
98  const double _z = 1.0 / z;
99  const double fx_z = camera_params.fx() * _z;
100  const double fy_z = camera_params.fy() * _z;
101  const double _z2 = square(_z);
102  const double fx_z2 = camera_params.fx() * _z2;
103  const double fy_z2 = camera_params.fy() * _z2;
104 
105  if (POSES_ARE_INVERSE)
106  {
107  const double xy = x * y;
108 
109  const double J_vals[] = {fx_z,
110  0,
111  -fx_z2 * x,
112  -fx_z2 * xy,
113  camera_params.fx() * (1 + square(x * _z)),
114  -fx_z * y,
115  0,
116  fy_z,
117  -fy_z2 * y,
118  -camera_params.fy() * (1 + square(y * _z)),
119  fy_z2 * xy,
120  fy_z * x};
121  out_J.loadFromArray(J_vals);
122  }
123  else
124  {
125  const mrpt::math::CMatrixDouble33& R = cam_pose.getRotationMatrix();
126 
127  const double jac_proj_vals[] = {fx_z, 0, -fx_z2 * x,
128  0, fy_z, -fy_z2 * y};
130  jac_proj_vals);
131 
132  const double p_x = cam_pose.x();
133  const double p_y = cam_pose.y();
134  const double p_z = cam_pose.z();
135  const double tx = landmark_global.x - p_x;
136  const double ty = landmark_global.y - p_y;
137  const double tz = landmark_global.z - p_z;
138 
139  const double aux_vals[] = {
140  -R(0, 0),
141  -R(1, 0),
142  -R(2, 0),
143  tz * R(1, 0) - ty * R(2, 0) + R(1, 0) * p_z - R(2, 0) * p_y,
144  tx * R(2, 0) - tz * R(0, 0) - R(0, 0) * p_z + R(2, 0) * p_x,
145  ty * R(0, 0) - tx * R(1, 0) + R(0, 0) * p_y - R(1, 0) * p_x,
146  -R(0, 1),
147  -R(1, 1),
148  -R(2, 1),
149  tz * R(1, 1) - ty * R(2, 1) + R(1, 1) * p_z - R(2, 1) * p_y,
150  tx * R(2, 1) - tz * R(0, 1) - R(0, 1) * p_z + R(2, 1) * p_x,
151  ty * R(0, 1) - tx * R(1, 1) + R(0, 1) * p_y - R(1, 1) * p_x,
152  -R(0, 2),
153  -R(1, 2),
154  -R(2, 2),
155  tz * R(1, 2) - ty * R(2, 2) + R(1, 2) * p_z - R(2, 2) * p_y,
156  tx * R(2, 2) - tz * R(0, 2) - R(0, 2) * p_z + R(2, 2) * p_x,
157  ty * R(0, 2) - tx * R(1, 2) + R(0, 2) * p_y - R(1, 2) * p_x};
159  out_J.multiply_AB(jac_proj, vals);
160  }
161 }
162 
163 /** Jacobians wrt the point
164  * \note Jacobians as described in
165  * http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
166  * (Appendix A)
167  */
168 template <bool POSES_ARE_INVERSE>
169 void pointJac(
170  const mrpt::img::TCamera& camera_params,
171  const mrpt::poses::CPose3D& cam_pose,
172  const mrpt::math::TPoint3D& landmark_global,
174 {
175  using namespace mrpt::math;
176 
177  mrpt::math::TPoint3D l; // Local point, wrt camera
178 
180 
181  if (POSES_ARE_INVERSE)
182  cam_pose.composePoint(
183  landmark_global.x, landmark_global.y, landmark_global.z, l.x, l.y,
184  l.z, &dp_point);
185  else
186  cam_pose.inverseComposePoint(
187  landmark_global.x, landmark_global.y, landmark_global.z, l.x, l.y,
188  l.z, &dp_point);
189 
190  ASSERT_(l.z != 0);
191  const double _z = 1.0 / l.z;
192  const double _z2 = square(_z);
193 
194  const double tmp_vals[] = {
195  camera_params.fx() * _z, 0,
196  -camera_params.fx() * l.x * _z2, 0,
197  camera_params.fy() * _z, -camera_params.fy() * l.y * _z2};
199 
200  out_J.multiply_AB(tmp, dp_point);
201 }
202 
203 // === Compute sparse Jacobians ====
204 // Case: 6D poses + 3D points + 2D (x,y) observations
205 // For the case of *inverse* or *normal* frame poses being estimated.
206 // Made inline so immediate values in "poses_are_inverses" are propragated by
207 // the compiler
208 template <bool POSES_ARE_INVERSE>
210  const TFramePosesVec& frame_poses,
211  const TLandmarkLocationsVec& landmark_points,
212  const mrpt::img::TCamera& camera_params,
214  const size_t num_fix_frames, const size_t num_fix_points)
215 {
216  MRPT_START
217 
218  // num_fix_frames & num_fix_points: Are relative to the order in frame_poses
219  // & landmark_points
220  ASSERT_(!frame_poses.empty() && !landmark_points.empty());
221  const size_t N = jac_data_vec.size();
222 
223  for (size_t i = 0; i < N; i++)
224  {
225  JacData<6, 3, 2>& D = jac_data_vec[i];
226 
227  const TCameraPoseID i_f = D.frame_id;
228  const TLandmarkID i_p = D.point_id;
229 
230  ASSERTDEB_(i_f < frame_poses.size());
231  ASSERTDEB_(i_p < landmark_points.size());
232 
233  if (i_f >= num_fix_frames)
234  {
235  frameJac<POSES_ARE_INVERSE>(
236  camera_params, frame_poses[i_f], landmark_points[i_p],
237  D.J_frame);
238  D.J_frame_valid = true;
239  }
240 
241  if (i_p >= num_fix_points)
242  {
243  pointJac<POSES_ARE_INVERSE>(
244  camera_params, frame_poses[i_f], landmark_points[i_p],
245  D.J_point);
246  D.J_point_valid = true;
247  }
248  }
249  MRPT_END
250 }
251 
252 /** Construct the BA linear system.
253  * Set kernel_1st_deriv!=nullptr if using robust kernel.
254  */
256  const TSequenceFeatureObservations& observations,
257  const std::vector<std::array<double, 2>>& residual_vec,
258  const mrpt::aligned_std_vector<JacData<6, 3, 2>>& jac_data_vec,
263  const size_t num_fix_frames, const size_t num_fix_points,
264  const vector<double>* kernel_1st_deriv);
265 } // namespace vision
266 } // namespace mrpt
267 
268 #endif
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
mrpt::math::CMatrixFixedNumeric< double, ObsDim, PointDof > J_point
Definition: ba_internals.h:67
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble z
Definition: glext.h:3872
uint64_t TLandmarkID
Unique IDs for landmarks.
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:165
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:167
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
void pointJac(const mrpt::img::TCamera &camera_params, const mrpt::poses::CPose3D &cam_pose, const mrpt::math::TPoint3D &landmark_global, mrpt::math::CMatrixFixedNumeric< double, 2, 3 > &out_J)
Jacobians wrt the point.
Definition: ba_internals.h:169
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Put this macro inside any class with members that require {16,32,64}-byte memory alignment (e...
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
TCameraPoseID frame_id
Definition: ba_internals.h:62
A complete sequence of observations of features from different camera frames (poses).
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
CArrayNumeric< double, N > CArrayDouble
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:64
GLbyte ty
Definition: glext.h:6092
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:29
double x
X,Y,Z coordinates.
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:88
void ba_compute_Jacobians(const TFramePosesVec &frame_poses, const TLandmarkLocationsVec &landmark_points, const mrpt::img::TCamera &camera_params, mrpt::aligned_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:209
void ba_build_gradient_Hessians(const TSequenceFeatureObservations &observations, const std::vector< std::array< double, 2 >> &residual_vec, const mrpt::aligned_std_vector< JacData< 6, 3, 2 >> &jac_data_vec, mrpt::aligned_std_vector< mrpt::math::CMatrixFixedNumeric< double, 6, 6 >> &U, mrpt::aligned_std_vector< CArrayDouble< 6 >> &eps_frame, mrpt::aligned_std_vector< mrpt::math::CMatrixFixedNumeric< double, 3, 3 >> &V, mrpt::aligned_std_vector< CArrayDouble< 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.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr, 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...
Definition: CPose3D.cpp:379
#define MRPT_END
Definition: exceptions.h:266
mrpt::aligned_std_vector< mrpt::poses::CPose3D > TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
mrpt::math::CMatrixFixedNumeric< double, ObsDim, FrameDof > J_frame
Definition: ba_internals.h:66
GLbyte GLbyte tz
Definition: glext.h:6092
GLenum GLint GLint y
Definition: glext.h:3538
void frameJac(const mrpt::img::TCamera &camera_params, const mrpt::poses::CPose3D &cam_pose, const mrpt::math::TPoint3D &landmark_global, mrpt::math::CMatrixFixedNumeric< double, 2, 6 > &out_J)
The projective camera 2x6 Jacobian (wrt the 6D camera pose)
Definition: ba_internals.h:81
GLenum GLint x
Definition: glext.h:3538
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:232
Lightweight 3D point.
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:63
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=nullptr) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:649



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019