Main MRPT website > C++ reference for MRPT 1.5.6
ba_internals.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #ifndef ba_internals_H
11 #define ba_internals_H
12 
14 #include <mrpt/utils/CArray.h>
16 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/vision/types.h>
19 
20 // Declarations shared between ba_*.cpp files, but which are private to MRPT
21 // not to be seen by an MRPT API user.
22 
23 namespace mrpt
24 {
25  namespace vision
26  {
27  using mrpt::math::CArrayDouble; // Allow these "using"s since these headers are internal to mrpt
29  using std::vector;
30 
31  #define VERBOSE_COUT if (verbose) cout
32 
33  // Auxiliary struct for keeping the list of all Jacobians in a sparse, efficient way.
34  template <int FrameDof, int PointDof, int ObsDim>
35  struct JacData
36  {
37  inline JacData() :
38  J_frame(mrpt::math::UNINITIALIZED_MATRIX),
39  J_point(mrpt::math::UNINITIALIZED_MATRIX),
40  J_frame_valid(false),
41  J_point_valid(false)
42  {}
43 
46 
47  // Jacobians of the observation wrt the camera pose & the point:
51 
52  MRPT_MAKE_ALIGNED_OPERATOR_NEW // Needed by any struct having Eigen::Matrix<> fields
53  };
54 
55 
56  /** The projective camera 2x6 Jacobian \f$ \frac{\partial h(f,p)}{\partial p} \f$ (wrt the 6D camera pose)
57  * \note Jacobians as described in http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty (Appendix A)
58  */
59  template <bool POSES_ARE_INVERSE>
60  void frameJac(
61  const mrpt::utils::TCamera & camera_params,
62  const mrpt::poses::CPose3D & cam_pose,
63  const mrpt::math::TPoint3D & landmark_global,
65  {
66  using mrpt::math::square;
67 
68  double x,y,z; // wrt cam (local coords)
69  if (POSES_ARE_INVERSE)
70  cam_pose.composePoint(
71  landmark_global.x,landmark_global.y,landmark_global.z,
72  x,y,z);
73  else
74  cam_pose.inverseComposePoint(
75  landmark_global.x,landmark_global.y,landmark_global.z,
76  x,y,z);
77 
78  ASSERT_(z!=0)
79 
80  const double _z = 1.0/z;
81  const double fx_z = camera_params.fx() *_z;
82  const double fy_z = camera_params.fy() *_z;
83  const double _z2 = square(_z);
84  const double fx_z2 = camera_params.fx()*_z2;
85  const double fy_z2 = camera_params.fy()*_z2;
86 
87  if (POSES_ARE_INVERSE)
88  {
89  const double xy = x*y;
90 
91  const double J_vals[] = {
92  fx_z, 0, -fx_z2*x, -fx_z2*xy, camera_params.fx()*(1+square(x*_z)), -fx_z*y,
93  0, fy_z, -fy_z2*y, -camera_params.fy()*(1+square(y*_z)), fy_z2*xy, fy_z*x };
94  out_J.loadFromArray(J_vals);
95  }
96  else
97  {
98  const mrpt::math::CMatrixDouble33 & R = cam_pose.getRotationMatrix();
99 
100  const double jac_proj_vals[] = {
101  fx_z, 0, -fx_z2*x,
102  0, fy_z, -fy_z2*y };
103  const mrpt::math::CMatrixFixedNumeric<double,2,3> jac_proj(jac_proj_vals);
104 
105  const double p_x = cam_pose.x();
106  const double p_y = cam_pose.y();
107  const double p_z = cam_pose.z();
108  const double tx = landmark_global.x - p_x;
109  const double ty = landmark_global.y - p_y;
110  const double tz = landmark_global.z - p_z;
111 
112  const double aux_vals[] = {
113  -R(0,0), -R(1,0), -R(2,0), tz * R(1,0) - ty * R(2,0) + R(1,0) * p_z - R(2,0) * p_y, tx * R(2,0) - tz * R(0,0) - R(0,0) * p_z + R(2,0) * p_x, ty * R(0,0) - tx * R(1,0) + R(0,0) * p_y - R(1,0) * p_x,
114  -R(0,1), -R(1,1), -R(2,1), tz * R(1,1) - ty * R(2,1) + R(1,1) * p_z - R(2,1) * p_y, tx * R(2,1) - tz * R(0,1) - R(0,1) * p_z + R(2,1) * p_x, ty * R(0,1) - tx * R(1,1) + R(0,1) * p_y - R(1,1) * p_x,
115  -R(0,2), -R(1,2), -R(2,2), tz * R(1,2) - ty * R(2,2) + R(1,2) * p_z - R(2,2) * p_y, tx * R(2,2) - tz * R(0,2) - R(0,2) * p_z + R(2,2) * p_x, ty * R(0,2) - tx * R(1,2) + R(0,2) * p_y - R(1,2) * p_x
116  };
117  const mrpt::math::CMatrixFixedNumeric<double,3,6> vals(aux_vals);
118  out_J.multiply_AB(jac_proj, vals);
119  }
120  }
121 
122  /** Jacobians wrt the point
123  * \note Jacobians as described in http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty (Appendix A)
124  */
125  template <bool POSES_ARE_INVERSE>
126  void pointJac(
127  const mrpt::utils::TCamera & camera_params,
128  const mrpt::poses::CPose3D & cam_pose,
129  const mrpt::math::TPoint3D & landmark_global,
131  {
132  using namespace mrpt::math;
133 
134  mrpt::math::TPoint3D l; // Local point, wrt camera
135 
137 
138  if (POSES_ARE_INVERSE)
139  cam_pose.composePoint(
140  landmark_global.x,landmark_global.y,landmark_global.z,
141  l.x,l.y,l.z,
142  &dp_point );
143  else
144  cam_pose.inverseComposePoint(
145  landmark_global.x,landmark_global.y,landmark_global.z,
146  l.x,l.y,l.z,
147  &dp_point );
148 
149  ASSERT_(l.z!=0)
150 
151  const double _z = 1.0/l.z;
152  const double _z2 = square(_z);
153 
154  const double tmp_vals[] = {
155  camera_params.fx()*_z, 0, -camera_params.fx()*l.x*_z2,
156  0 , camera_params.fy()*_z, -camera_params.fy()*l.y*_z2 };
158 
159  out_J.multiply_AB(tmp, dp_point);
160  }
161 
162  // === Compute sparse Jacobians ====
163  // Case: 6D poses + 3D points + 2D (x,y) observations
164  // For the case of *inverse* or *normal* frame poses being estimated.
165  // Made inline so immediate values in "poses_are_inverses" are propragated by the compiler
166  template <bool POSES_ARE_INVERSE>
168  const TFramePosesVec & frame_poses,
169  const TLandmarkLocationsVec & landmark_points,
170  const mrpt::utils::TCamera & camera_params,
171  mrpt::aligned_containers<JacData<6,3,2> >::vector_t & jac_data_vec,
172  const size_t num_fix_frames,
173  const size_t num_fix_points)
174  {
175  MRPT_START
176 
177  // num_fix_frames & num_fix_points: Are relative to the order in frame_poses & landmark_points
178  ASSERT_(!frame_poses.empty() && !landmark_points.empty())
179 
180  const size_t N = jac_data_vec.size();
181 
182  for (size_t i=0;i<N;i++)
183  {
184  JacData<6,3,2> &D = jac_data_vec[i];
185 
186  const TCameraPoseID i_f = D.frame_id;
187  const TLandmarkID i_p = D.point_id;
188 
189  ASSERTDEB_(i_f<frame_poses.size())
190  ASSERTDEB_(i_p<landmark_points.size())
191 
192  if (i_f>=num_fix_frames)
193  {
194  frameJac<POSES_ARE_INVERSE>(camera_params, frame_poses[i_f], landmark_points[i_p], D.J_frame);
195  D.J_frame_valid = true;
196  }
197 
198  if (i_p>=num_fix_points)
199  {
200  pointJac<POSES_ARE_INVERSE>(camera_params, frame_poses[i_f], landmark_points[i_p], D.J_point);
201  D.J_point_valid = true;
202  }
203  }
204  MRPT_END
205  }
206 
207  /** Construct the BA linear system.
208  * Set kernel_1st_deriv!=NULL if using robust kernel.
209  */
211  const TSequenceFeatureObservations & observations,
212  const std::vector<mrpt::utils::CArray<double,2> > & residual_vec,
213  const mrpt::aligned_containers<JacData<6,3,2> >::vector_t & jac_data_vec,
215  mrpt::aligned_containers<CArrayDouble<6> >::vector_t & eps_frame,
217  mrpt::aligned_containers<CArrayDouble<3> >::vector_t & eps_point,
218  const size_t num_fix_frames,
219  const size_t num_fix_points,
220  const vector<double> * kernel_1st_deriv
221  );
222 
223 
224  }
225 }
226 
227 #endif
void pointJac(const mrpt::utils::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:126
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
double z
X,Y,Z coordinates.
TCameraPoseID frame_id
Definition: ba_internals.h:44
Helper types for STL containers with Eigen memory allocators.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
Definition: CPose3D.h:176
void frameJac(const mrpt::utils::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:60
GLdouble l
Definition: glew.h:5092
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
void ba_compute_Jacobians(const TFramePosesVec &frame_poses, const TLandmarkLocationsVec &landmark_points, const mrpt::utils::TCamera &camera_params, mrpt::aligned_containers< JacData< 6, 3, 2 > >::vector_t &jac_data_vec, const size_t num_fix_frames, const size_t num_fix_points)
Definition: ba_internals.h:167
mrpt::aligned_containers< mrpt::poses::CPose3D >::vector_t TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:160
A numeric matrix of compile-time fixed size.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
#define MRPT_END
A STL container (as wrapper) for arrays of constant size defined at compile time. ...
Definition: CArray.h:53
GLbyte ty
Definition: glext.h:5441
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
void ba_build_gradient_Hessians(const TSequenceFeatureObservations &observations, const std::vector< mrpt::utils::CArray< double, 2 > > &residual_vec, const mrpt::aligned_containers< JacData< 6, 3, 2 > >::vector_t &jac_data_vec, mrpt::aligned_containers< mrpt::math::CMatrixFixedNumeric< double, 6, 6 > >::vector_t &U, mrpt::aligned_containers< CArrayDouble< 6 > >::vector_t &eps_frame, mrpt::aligned_containers< mrpt::math::CMatrixFixedNumeric< double, 3, 3 > >::vector_t &V, mrpt::aligned_containers< CArrayDouble< 3 > >::vector_t &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.
Definition: ba_common.cpp:235
#define MRPT_START
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, 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:427
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
uint64_t TLandmarkID
Unique IDs for landmarks.
GLdouble GLdouble z
Definition: glew.h:1464
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
const float R
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::math::CMatrixFixedNumeric< double, ObsDim, FrameDof > J_frame
Definition: ba_internals.h:48
mrpt::math::CMatrixFixedNumeric< double, ObsDim, PointDof > J_point
Definition: ba_internals.h:49
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
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=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL) const
Computes the 3D point L such as .
Definition: CPose3D.cpp:671
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
GLbyte GLbyte tz
Definition: glext.h:5441
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:158
Lightweight 3D point.
TLandmarkID point_id
Definition: ba_internals.h:45
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:31



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018