Main MRPT website > C++ reference for MRPT 1.5.6
ba_common.cpp
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 #include "vision-precomp.h" // Precompiled headers
11 
13 #include <mrpt/vision/pinhole.h>
15 #include "ba_internals.h"
16 
17 using namespace std;
18 using namespace mrpt;
19 using namespace mrpt::vision;
20 using namespace mrpt::utils;
21 using namespace mrpt::math;
22 using namespace mrpt::poses;
23 
24 
25 /* -------------------------------------------------------------------------------------
26  ba_initial_estimate
27 
28  Fills the frames & landmark points maps with an initial gross estimate
29  from the sequence \a observations, so they can be fed to bundle adjustment methods.
30  ------------------------------------------------------------------------------------ */
32  const TSequenceFeatureObservations & observations,
33  const TCamera & camera_params,
34  TFramePosesMap & frame_poses,
35  TLandmarkLocationsMap & landmark_points )
36 {
37  MRPT_UNUSED_PARAM(camera_params);
39  // VERY CRUDE APPROACH: All camera poses at the origin, all points at (0,0,1)
40  // TODO: Improve with the data in "observations"...
41 
42  // Clear previous contents:
43  frame_poses.clear();
44  landmark_points.clear();
45 
46  // Go thru the obs list and insert poses:
47  for (TSequenceFeatureObservations::const_iterator it=observations.begin();it!=observations.end();++it)
48  {
49  const TFeatureID feat_ID = it->id_feature;
50  const TCameraPoseID frame_ID = it->id_frame;
51 
52  frame_poses[frame_ID] = CPose3D(0,0,0,0,0,0);
53  landmark_points[feat_ID] = TPoint3D(0,0,1);
54  }
55  MRPT_END
56 }
57 
59  const TSequenceFeatureObservations & observations,
60  const TCamera & camera_params,
61  TFramePosesVec & frame_poses,
62  TLandmarkLocationsVec & landmark_points )
63 {
64  MRPT_UNUSED_PARAM(camera_params);
66  // VERY CRUDE APPROACH: All camera poses at the origin, all points at (0,0,1)
67  // TODO: Improve with the data in "observations"...
68 
69  // Clear previous contents:
70  frame_poses.clear();
71  landmark_points.clear();
72 
73  if (observations.empty()) return;
74 
75  // Go thru the obs list and insert poses:
76 
77  TFeatureID max_pt_id=0;
78  TCameraPoseID max_fr_id=0;
79 
80  for (TSequenceFeatureObservations::const_iterator it=observations.begin();it!=observations.end();++it)
81  {
82  const TFeatureID feat_ID = it->id_feature;
83  const TCameraPoseID frame_ID = it->id_frame;
84  keep_max(max_fr_id, frame_ID);
85  keep_max(max_pt_id, feat_ID);
86  }
87 
88  // Insert N copies of the same values:
89  frame_poses.assign(max_fr_id+1, CPose3D(0,0,0,0,0,0) );
90  landmark_points.assign(max_pt_id+1, TPoint3D(0,0,1) );
91 
92  MRPT_END
93 }
94 
95 // This function is what to do for each feature in the reprojection loops below.
96 // -> residual: the raw residual, even if using robust kernel
97 // -> sum+= scaled squared norm of the residual (which != squared norm if using robust kernel)
98 template <bool POSES_INVERSE>
100  const TCamera & camera_params,
101  const TFeatureObservation & OBS,
102  CArray<double,2> & out_residual,
103  const TFramePosesVec::value_type & frame,
104  const TLandmarkLocationsVec::value_type & point,
105  double &sum,
106  const bool use_robust_kernel,
107  const double kernel_param,
108  double * out_kernel_1st_deriv
109  )
110 {
111  const TPixelCoordf z_pred = mrpt::vision::pinhole::projectPoint_no_distortion<POSES_INVERSE>(camera_params, frame, point);
112  const TPixelCoordf &z_meas = OBS.px;
113 
114  out_residual[0] = z_meas.x-z_pred.x;
115  out_residual[1] = z_meas.y-z_pred.y;
116 
117  const double sum_2= square(out_residual[0])+square(out_residual[1]);
118 
119  if (use_robust_kernel)
120  {
122  kernel.param_sq = square(kernel_param);
123  double kernel_1st_deriv, kernel_2nd_deriv;
124 
125  sum += kernel.eval(sum_2, kernel_1st_deriv,kernel_2nd_deriv);
126  if (out_kernel_1st_deriv) *out_kernel_1st_deriv = kernel_1st_deriv;
127  }
128  else
129  {
130  sum += sum_2;
131  }
132 }
133 
134 
135 /** Compute reprojection error vector (used from within Bundle Adjustment methods, but can be used in general)
136  * See mrpt::vision::bundle_adj_full for a description of most parameters.
137  *
138  * \return Overall squared reprojection error.
139  */
141  const TSequenceFeatureObservations & observations,
142  const TCamera & camera_params,
143  const TFramePosesMap & frame_poses,
144  const TLandmarkLocationsMap & landmark_points,
145  std::vector<CArray<double,2> > & out_residuals,
146  const bool frame_poses_are_inverse,
147  const bool use_robust_kernel,
148  const double kernel_param,
149  std::vector<double> * out_kernel_1st_deriv
150  )
151 {
152  MRPT_START
153 
154  double sum = 0;
155 
156  const size_t N = observations.size();
157  out_residuals.resize(N);
158  if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
159 
160  for (size_t i=0;i<N;i++)
161  {
162  const TFeatureObservation & OBS = observations[i];
163 
164  const TFeatureID i_p = OBS.id_feature;
165  const TCameraPoseID i_f = OBS.id_frame;
166 
167  TFramePosesMap::const_iterator itF = frame_poses.find(i_f);
168  TLandmarkLocationsMap::const_iterator itP = landmark_points.find(i_p);
169  ASSERTMSG_(itF!=frame_poses.end(), "Frame ID is not in list!")
170  ASSERTMSG_(itP!=landmark_points.end(), "Landmark ID is not in list!")
171 
172  const TFramePosesMap::mapped_type & frame = itF->second;
173  const TLandmarkLocationsMap::mapped_type & point = itP->second;
174 
175  double *ptr_1st_deriv = out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) : NULL;
176 
177  if (frame_poses_are_inverse)
178  reprojectionResidualsElement<true>(camera_params, OBS, out_residuals[i], frame, point, sum, use_robust_kernel,kernel_param,ptr_1st_deriv);
179  else
180  reprojectionResidualsElement<false>(camera_params, OBS, out_residuals[i], frame, point, sum, use_robust_kernel,kernel_param,ptr_1st_deriv);
181  }
182 
183  return sum;
184 
185  MRPT_END
186 }
187 
189  const TSequenceFeatureObservations & observations,
190  const TCamera & camera_params,
191  const TFramePosesVec & frame_poses,
192  const TLandmarkLocationsVec & landmark_points,
193  std::vector<CArray<double,2> > & out_residuals,
194  const bool frame_poses_are_inverse,
195  const bool use_robust_kernel,
196  const double kernel_param,
197  std::vector<double> * out_kernel_1st_deriv
198  )
199 {
200  MRPT_START
201 
202  double sum = 0;
203 
204  const size_t N = observations.size();
205  out_residuals.resize(N);
206  if (out_kernel_1st_deriv) out_kernel_1st_deriv->resize(N);
207 
208  for (size_t i=0;i<N;i++)
209  {
210  const TFeatureObservation & OBS = observations[i];
211 
212  const TFeatureID i_p = OBS.id_feature;
213  const TCameraPoseID i_f = OBS.id_frame;
214 
215  ASSERT_BELOW_(i_p,landmark_points.size())
216  ASSERT_BELOW_(i_f,frame_poses.size())
217 
218  const TFramePosesVec::value_type & frame = frame_poses[i_f];
219  const TLandmarkLocationsVec::value_type & point = landmark_points[i_p];
220 
221  double *ptr_1st_deriv = out_kernel_1st_deriv ? &((*out_kernel_1st_deriv)[i]) : NULL;
222 
223  if (frame_poses_are_inverse)
224  reprojectionResidualsElement<true>(camera_params, OBS, out_residuals[i], frame, point, sum, use_robust_kernel,kernel_param,ptr_1st_deriv);
225  else
226  reprojectionResidualsElement<false>(camera_params, OBS, out_residuals[i], frame, point, sum, use_robust_kernel,kernel_param,ptr_1st_deriv);
227  }
228 
229  return sum;
230  MRPT_END
231 }
232 
233 
234 // Compute gradients & Hessian blocks
236  const TSequenceFeatureObservations & observations,
237  const vector<CArray<double,2> > & residual_vec,
238  const mrpt::aligned_containers<JacData<6,3,2> >::vector_t & jac_data_vec,
240  mrpt::aligned_containers<CArrayDouble<6> >::vector_t & eps_frame,
242  mrpt::aligned_containers<CArrayDouble<3> >::vector_t & eps_point,
243  const size_t num_fix_frames,
244  const size_t num_fix_points,
245  const vector<double> * kernel_1st_deriv
246  )
247 {
248  MRPT_START
249 
250  const size_t N = observations.size();
251  const bool use_robust_kernel = (kernel_1st_deriv!=NULL);
252 
253  for (size_t i=0;i<N;i++)
254  {
255  const TFeatureObservation & OBS = observations[i];
256 
257  const TFeatureID i_p = OBS.id_feature;
258  const TCameraPoseID i_f = OBS.id_frame;
259 
260  const Eigen::Matrix<double,2,1> RESID( &residual_vec[i][0] );
261  const JacData<6,3,2> & JACOB = jac_data_vec[i];
262  ASSERTDEB_(JACOB.frame_id==i_f && JACOB.point_id==i_p)
263 
264  if (i_f >= num_fix_frames)
265  {
266  const size_t frame_id = i_f - num_fix_frames;
268  ASSERT_BELOW_(frame_id,U.size())
269 
271  JtJ.multiply_AtA(JACOB.J_frame);
272 
273  CArrayDouble<6> eps_delta;
274  JACOB.J_frame.multiply_Atb(RESID, eps_delta); // eps_delta = J^t * RESID
275  if (!use_robust_kernel)
276  {
277  eps_frame[frame_id] += eps_delta;
278  }
279  else
280  {
281  const double rho_1st_der = (*kernel_1st_deriv)[i];
282  eps_frame[frame_id] += eps_delta * rho_1st_der;
283  }
284  U[frame_id]+=JtJ;
285  }
286  if (i_p >= num_fix_points)
287  {
288  const size_t point_id = i_p - num_fix_points;
290  ASSERT_BELOW_(point_id,V.size())
291 
293  JtJ.multiply_AtA(JACOB.J_point);
294 
295  CArrayDouble<3> eps_delta;
296  JACOB.J_point.multiply_Atb(RESID, eps_delta); // eps_delta = J^t * RESID
297  if (!use_robust_kernel)
298  {
299  eps_point[point_id] += eps_delta;
300  }
301  else
302  {
303  const double rho_1st_der = (*kernel_1st_deriv)[i];
304  eps_point[point_id] += eps_delta * rho_1st_der;
305  }
306 
307  V[point_id]+=JtJ;
308  }
309  }
310 
311  MRPT_END
312 }
313 
315  const TFramePosesVec & frame_poses,
316  const CVectorDouble &delta,
317  const size_t delta_first_idx,
318  const size_t delta_num_vals,
319  TFramePosesVec & new_frame_poses,
320  const size_t num_fix_frames )
321 {
322  MRPT_START
323 
324  new_frame_poses.resize(frame_poses.size());
325 
326  for (size_t i=0;i<num_fix_frames;++i)
327  new_frame_poses[i] = frame_poses[i];
328 
329  size_t delta_used_vals = 0;
330  const double *delta_val = &delta[delta_first_idx];
331 
332  for (size_t i=num_fix_frames;i<frame_poses.size();i++)
333  {
334  const CPose3D &old_pose = frame_poses[i];
335  CPose3D &new_pose = new_frame_poses[i];
336 
337  // Use the Lie Algebra methods for the increment:
338  const CArrayDouble<6> incr(delta_val);
339  const CPose3D incrPose = CPose3D::exp(incr);
340 
341  //new_pose = old_pose [+] delta
342  // = exp(delta) (+) old_pose
343  new_pose.composeFrom(incrPose, old_pose);
344 
345  // Move to the next entry in delta:
346  delta_val+=6;
347  delta_used_vals+=6;
348  }
349 
350  ASSERT_(delta_used_vals==delta_num_vals)
351 
352  MRPT_END
353 }
355  const TLandmarkLocationsVec & landmark_points,
356  const CVectorDouble & delta,
357  const size_t delta_first_idx,
358  const size_t delta_num_vals,
359  TLandmarkLocationsVec & new_landmark_points,
360  const size_t num_fix_points )
361 {
362  MRPT_START
363 
364  new_landmark_points.resize(landmark_points.size());
365 
366  for (size_t i=0;i<num_fix_points;++i)
367  new_landmark_points[i] = landmark_points[i];
368 
369  size_t delta_used_vals = 0;
370  const double *delta_val = &delta[delta_first_idx];
371 
372  for (size_t i=num_fix_points;i<landmark_points.size();i++)
373  {
374  const TPoint3D &old_point = landmark_points[i];
375  TPoint3D &new_point = new_landmark_points[i];
376 
377  for (size_t j=0;j<3;j++)
378  new_point[j] = old_point[j] + delta_val[j];
379 
380  // Move to the next entry in delta:
381  delta_val+=3;
382  delta_used_vals+=3;
383  }
384 
385  ASSERT_(delta_used_vals==delta_num_vals)
386 
387  MRPT_END
388 }
389 
390 
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
double VISION_IMPEXP reprojectionResiduals(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::utils::TCamera &camera_params, const mrpt::vision::TFramePosesVec &frame_poses, const mrpt::vision::TLandmarkLocationsVec &landmark_points, std::vector< mrpt::utils::CArray< double, 2 > > &out_residuals, const bool frame_poses_are_inverse, const bool use_robust_kernel=true, const double kernel_param=3.0, std::vector< double > *out_kernel_1st_deriv=NULL)
Compute reprojection error vector (used from within Bundle Adjustment methods, but can be used in gen...
Definition: ba_common.cpp:188
TLandmarkID id_feature
A unique ID of this feature.
#define ASSERT_BELOW_(__A, __B)
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
One feature observation entry, used within sequences with TSequenceFeatureObservations.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void VISION_IMPEXP add_se3_deltas_to_frames(const mrpt::vision::TFramePosesVec &frame_poses, const mrpt::math::CVectorDouble &delta, const size_t delta_first_idx, const size_t delta_num_vals, mrpt::vision::TFramePosesVec &new_frame_poses, const size_t num_fix_frames)
For each pose in the vector frame_poses, adds a "delta" increment to the manifold, with the "delta" given in the se(3) Lie algebra:
Definition: ba_common.cpp:314
mrpt::utils::TPixelCoordf px
The pixel coordinates of the observed feature.
TCameraPoseID frame_id
Definition: ba_internals.h:44
Helper types for STL containers with Eigen memory allocators.
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.
Definition: bits.h:52
mrpt::aligned_containers< mrpt::poses::CPose3D >::vector_t TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
A numeric matrix of compile-time fixed size.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:595
void VISION_IMPEXP add_3d_deltas_to_points(const mrpt::vision::TLandmarkLocationsVec &landmark_points, const mrpt::math::CVectorDouble &delta, const size_t delta_first_idx, const size_t delta_num_vals, mrpt::vision::TLandmarkLocationsVec &new_landmark_points, const size_t num_fix_points)
For each pose in the vector frame_poses, adds a "delta" increment to the manifold, with the "delta" given in the se(3) Lie algebra:
Definition: ba_common.cpp:354
A STL container (as wrapper) for arrays of constant size defined at compile time. ...
Definition: CArray.h:53
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
uint64_t TFeatureID
Definition of a feature ID.
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
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Definition: bits.h:176
#define MRPT_START
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
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
#define ASSERT_(f)
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
std::map< TLandmarkID, mrpt::math::TPoint3D > TLandmarkLocationsMap
A list of landmarks (3D points) indexed by unique IDs.
TCameraPoseID id_frame
A unique ID of a "frame" (camera position) from where the feature was observed.
void reprojectionResidualsElement(const TCamera &camera_params, const TFeatureObservation &OBS, CArray< double, 2 > &out_residual, const TFramePosesVec::value_type &frame, const TLandmarkLocationsVec::value_type &point, double &sum, const bool use_robust_kernel, const double kernel_param, double *out_kernel_1st_deriv)
Definition: ba_common.cpp:99
Lightweight 3D point.
void VISION_IMPEXP ba_initial_estimate(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::utils::TCamera &camera_params, mrpt::vision::TFramePosesVec &frame_poses, mrpt::vision::TLandmarkLocationsVec &landmark_points)
Fills the frames & landmark points maps with an initial gross estimate from the sequence observations...
Definition: ba_common.cpp:58
TLandmarkID point_id
Definition: ba_internals.h:45
mrpt::aligned_containers< TCameraPoseID, mrpt::poses::CPose3D >::map_t TFramePosesMap
A list of camera frames (6D poses) indexed by unique IDs.
#define ASSERTMSG_(f, __ERROR_MSG)
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