Main MRPT website > C++ reference for MRPT 1.5.6
ba_full.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/utils/CTimeLogger.h>
16 
17 #include <memory> // std::auto_ptr, unique_ptr
18 
19 #include "ba_internals.h"
20 
21 using namespace std;
22 using namespace mrpt;
23 using namespace mrpt::vision;
24 using namespace mrpt::utils;
25 using namespace mrpt::math;
26 
28 
29 // Define -> estimate the inverse of poses; Undefined -> estimate the camera poses (read the doc of mrpt::vision::bundle_adj_full)
30 #define USE_INVERSE_POSES
31 
32 #ifdef USE_INVERSE_POSES
33 # define INV_POSES_BOOL true
34 #else
35 # define INV_POSES_BOOL false
36 #endif
37 
38 /* ----------------------------------------------------------
39  bundle_adj_full
40 
41  See bundle_adjustment.h for docs.
42 
43  This implementation is almost entirely an adapted version from
44  RobotVision (at OpenSLAM.org) (C) by Hauke Strasdat (Imperial
45  College London), licensed under GNU LGPL.
46  See the related paper: H. Strasdat, J.M.M. Montiel,
47  A.J. Davison: "Scale Drift-Aware Large Scale Monocular SLAM",
48  RSS2010, http://www.roboticsproceedings.org/rss06/p10.html
49 
50  ---------------------------------------------------------- */
52  const TSequenceFeatureObservations & observations,
53  const TCamera & camera_params,
54  TFramePosesVec & frame_poses,
55  TLandmarkLocationsVec & landmark_points,
56  const mrpt::utils::TParametersDouble & extra_params,
57  const TBundleAdjustmentFeedbackFunctor user_feedback )
58 {
60 
61  // Generic BA problem dimension numbers:
62  static const unsigned int FrameDof = 6; // Poses: x y z yaw pitch roll
63  static const unsigned int PointDof = 3; // Landmarks: x y z
64  static const unsigned int ObsDim = 2; // Obs: x y (pixels)
65 
66  // Typedefs for this specific BA problem:
67  typedef JacData<FrameDof,PointDof,ObsDim> MyJacData;
68  typedef aligned_containers<MyJacData>::vector_t MyJacDataVec;
69 
70  typedef CArray<double,ObsDim> Array_O;
71  typedef CArrayDouble<FrameDof> Array_F;
72  typedef CArrayDouble<PointDof> Array_P;
76 
77  // Extra params:
78  const bool use_robust_kernel = 0!=extra_params.getWithDefaultVal("robust_kernel",1);
79  const bool verbose = 0!=extra_params.getWithDefaultVal("verbose",0);
80  const double initial_mu = extra_params.getWithDefaultVal("mu",-1);
81  const size_t max_iters = extra_params.getWithDefaultVal("max_iterations",50);
82  const size_t num_fix_frames = extra_params.getWithDefaultVal("num_fix_frames",1);
83  const size_t num_fix_points = extra_params.getWithDefaultVal("num_fix_points",0);
84  const double kernel_param = extra_params.getWithDefaultVal("kernel_param",3.0);
85 
86  const bool enable_profiler = 0!=extra_params.getWithDefaultVal("profiler",0);
87 
88  mrpt::utils::CTimeLogger profiler(enable_profiler);
89 
90  profiler.enter("bundle_adj_full (complete run)");
91 
92  // Input data sizes:
93  const size_t num_points = landmark_points.size();
94  const size_t num_frames = frame_poses.size();
95  const size_t num_obs = observations.size();
96 
97 
98  ASSERT_ABOVE_(num_frames,0)
99  ASSERT_ABOVE_(num_points,0)
100  ASSERT_(num_fix_frames>=1)
101  ASSERT_ABOVEEQ_(num_frames,num_fix_frames);
102  ASSERT_ABOVEEQ_(num_points,num_fix_points);
103 
104 #ifdef USE_INVERSE_POSES
105  // *Warning*: This implementation assumes inverse camera poses: inverse them at the entrance and at exit:
106  profiler.enter("invert_poses");
107  for (size_t i=0;i<num_frames;i++)
108  frame_poses[i].inverse();
109  profiler.leave("invert_poses");
110 #endif
111 
112 
113  MyJacDataVec jac_data_vec(num_obs);
114  vector<Array_O> residual_vec(num_obs);
115  vector<double> kernel_1st_deriv(num_obs);
116 
117  // prepare structure of sparse Jacobians:
118  for (size_t i=0; i<num_obs; i++)
119  {
120  jac_data_vec[i].frame_id = observations[i].id_frame;
121  jac_data_vec[i].point_id = observations[i].id_feature;
122  }
123 
124  // Compute sparse Jacobians:
125  profiler.enter("compute_Jacobians");
126  ba_compute_Jacobians<INV_POSES_BOOL>(frame_poses, landmark_points, camera_params, jac_data_vec, num_fix_frames, num_fix_points);
127  profiler.leave("compute_Jacobians");
128 
129 
130  profiler.enter("reprojectionResiduals");
132  observations, camera_params, frame_poses, landmark_points,
133  residual_vec,
134  INV_POSES_BOOL, // are poses inverse?
135  use_robust_kernel,
136  kernel_param,
137  use_robust_kernel ? &kernel_1st_deriv : NULL );
138  profiler.leave("reprojectionResiduals");
139 
141 
142  VERBOSE_COUT << "res: " << res << endl;
143 
144  // Auxiliary vars:
145  Array_F arrF_zeros;
146  arrF_zeros.assign(0);
147  Array_P arrP_zeros;
148  arrP_zeros.assign(0);
149 
150  const size_t num_free_frames = num_frames-num_fix_frames;
151  const size_t num_free_points = num_points-num_fix_points;
152  const size_t len_free_frames = FrameDof * num_free_frames;
153  const size_t len_free_points = PointDof * num_free_points;
154 
155  aligned_containers<Matrix_FxF>::vector_t H_f (num_free_frames);
156  aligned_containers<Array_F>::vector_t eps_frame (num_free_frames, arrF_zeros);
157  aligned_containers<Matrix_PxP>::vector_t H_p (num_free_points);
158  aligned_containers<Array_P>::vector_t eps_point (num_free_points, arrP_zeros);
159 
160  profiler.enter("build_gradient_Hessians");
161  ba_build_gradient_Hessians(observations,residual_vec,jac_data_vec, H_f,eps_frame,H_p,eps_point, num_fix_frames, num_fix_points, use_robust_kernel ? &kernel_1st_deriv : NULL );
162  profiler.leave("build_gradient_Hessians");
163 
164  double nu = 2;
165  double eps = 1e-16; // 0.000000000000001;
166  bool stop = false;
167  double mu = initial_mu;
168 
169  // Automatic guess of "mu":
170  if (mu<0)
171  {
172  double norm_max_A = 0;
173  for (size_t j=0; j<num_free_frames; ++j)
174  for (size_t dim=0; dim<FrameDof; dim++)
175  keep_max(norm_max_A, H_f[j](dim,dim) );
176 
177  for (size_t i=0; i<num_free_points; ++i)
178  for (size_t dim=0; dim<PointDof; dim++)
179  keep_max(norm_max_A, H_p[i](dim,dim) );
180  double tau = 1e-3;
181  mu = tau*norm_max_A;
182  }
183 
184  Matrix_FxF I_muFrame(UNINITIALIZED_MATRIX);
185  Matrix_PxP I_muPoint(UNINITIALIZED_MATRIX);
186 
187  // Cholesky object, as a pointer to reuse it between iterations:
188 #if MRPT_HAS_CXX11
189  typedef std::unique_ptr<CSparseMatrix::CholeskyDecomp> SparseCholDecompPtr;
190 #else
191  typedef std::auto_ptr<CSparseMatrix::CholeskyDecomp> SparseCholDecompPtr;
192 #endif
193  SparseCholDecompPtr ptrCh;
194 
195  for (size_t iter=0; iter<max_iters; iter++)
196  {
197  VERBOSE_COUT << "iteration: "<< iter << endl;
198 
199  // provide feedback to the user:
200  if (user_feedback)
201  (*user_feedback)(iter, res, max_iters, observations, frame_poses, landmark_points );
202 
203  bool has_improved = false;
204  do
205  {
206  profiler.enter("COMPLETE_ITER");
207 
208  VERBOSE_COUT << "mu: " <<mu<< endl;
209 
210  I_muFrame.unit(FrameDof,mu);
211  I_muPoint.unit(PointDof,mu);
212 
213  aligned_containers<Matrix_FxF>::vector_t U_star(num_free_frames, I_muFrame);
214  aligned_containers<Matrix_PxP>::vector_t V_inv (num_free_points);
215 
216  for (size_t i=0; i<U_star.size(); ++i)
217  U_star[i] += H_f[i];
218 
219  for (size_t i=0; i<H_p.size(); ++i)
220  (H_p[i]+I_muPoint).inv_fast( V_inv[i] );
221 
222 
223  typedef aligned_containers<pair<TCameraPoseID,TLandmarkID>,Matrix_FxP>::map_t WMap;
224  WMap W,Y;
225 
226  // For quick look-up of entries in W affecting a given point ID:
227  vector<vector<WMap::iterator> > W_entries(num_points); // Index is "TLandmarkID"
228 
229  MyJacDataVec::const_iterator jac_iter = jac_data_vec.begin();
230  for (TSequenceFeatureObservations::const_iterator it_obs=observations.begin(); it_obs!=observations.end(); ++it_obs)
231  {
232  const TFeatureID feat_id = it_obs->id_feature;
233  const TCameraPoseID frame_id = it_obs->id_frame;
234 
235  if (jac_iter->J_frame_valid && jac_iter->J_point_valid)
236  {
237  const CMatrixFixedNumeric<double,ObsDim,FrameDof> & J_frame = jac_iter->J_frame;
238  const CMatrixFixedNumeric<double,ObsDim,PointDof> & J_point = jac_iter->J_point;
239  const pair<TCameraPoseID,TLandmarkID> id_pair = make_pair(frame_id, feat_id);
240 
241  Matrix_FxP tmp(UNINITIALIZED_MATRIX);
242  tmp.multiply_AtB(J_frame, J_point);
243 
244  // W[ids] = J_f^T * J_p
245  // Was: W[id_pair] = tmp;
246  const pair<WMap::iterator,bool> &retInsert = W.insert( make_pair(id_pair,tmp) );
247  ASSERT_(retInsert.second==true)
248  W_entries[feat_id].push_back(retInsert.first); // Keep the iterator
249 
250  // Y[ids] = W[ids] * H_p^{-1}
251  Y[id_pair].multiply_AB(tmp, V_inv[feat_id-num_fix_points]);
252  }
253  ++jac_iter;
254  }
255 
256  aligned_containers<pair<TCameraPoseID,TLandmarkID>,Matrix_FxF>::map_t YW_map;
257  for (size_t i=0; i<H_f.size(); ++i)
258  YW_map[std::pair<TCameraPoseID,TLandmarkID>(i,i)] = U_star[i];
259 
260  CVectorDouble delta( len_free_frames + len_free_points ); // The optimal step
261  CVectorDouble e ( len_free_frames );
262 
263 
264  profiler.enter("Schur.build.reduced.frames");
265  for (size_t j=0; j<num_free_frames; ++j)
266  ::memcpy( &e[j*FrameDof], &eps_frame[j][0], sizeof(e[0])*FrameDof ); // e.slice(j*FrameDof,FrameDof) = AT(eps_frame,j);
267 
268  for (WMap::iterator Y_ij=Y.begin(); Y_ij!=Y.end(); ++Y_ij)
269  {
270  const TLandmarkID point_id = Y_ij->first.second;
271  const size_t i = Y_ij->first.second - num_fix_points; // point index
272  const size_t j = Y_ij->first.first - num_fix_frames; // frame index
273 
274  const vector<WMap::iterator> &iters = W_entries[point_id]; //->second;
275 
276  for (size_t itIdx=0; itIdx<iters.size(); itIdx++)
277  //for (size_t k=0; k<num_free_frames; ++k)
278  {
279  const WMap::iterator &W_ik = iters[itIdx];
280  const TLandmarkID k = W_ik->first.first-num_fix_frames;
281 
282  Matrix_FxF YWt(UNINITIALIZED_MATRIX); //-(Y_ij->second) * (W_ik->second).T();
283  YWt.multiply_ABt( Y_ij->second, W_ik->second );
284  //YWt*=-1.0; // The "-" sign is taken into account below:
285 
286  const pair<TCameraPoseID,TLandmarkID> ids_jk = pair<TCameraPoseID,TLandmarkID>(j,k);
287 
288  aligned_containers<pair<TCameraPoseID,TLandmarkID>,Matrix_FxF>::map_t::iterator it = YW_map.find(ids_jk);
289  if(it!=YW_map.end())
290  it->second -= YWt; // += (-YWt);
291  else
292  YW_map[ids_jk] = YWt*(-1.0);
293  }
294 
296  Y_ij->second.multiply_Ab( eps_point[i] ,r);
297  for (size_t k=0; k<FrameDof; k++)
298  e[j*FrameDof+k]-=r[k];
299  }
300  profiler.leave("Schur.build.reduced.frames");
301 
302 
303  profiler.enter("sS:ALL");
304  profiler.enter("sS:fill");
305 
306  VERBOSE_COUT << "Entries in YW_map:" << YW_map.size() << endl;
307 
308  CSparseMatrix sS(len_free_frames, len_free_frames);
309 
310  for (aligned_containers<pair<TCameraPoseID,TLandmarkID>,Matrix_FxF>::map_t::const_iterator it= YW_map.begin(); it!=YW_map.end(); ++it)
311  {
312  const pair<TCameraPoseID,TLandmarkID> & ids = it->first;
313  const Matrix_FxF & YW = it->second;
314  sS.insert_submatrix(ids.first*FrameDof,ids.second*FrameDof, YW);
315  }
316  profiler.leave("sS:fill");
317 
318  // Compress the sparse matrix:
319  profiler.enter("sS:compress");
320  sS.compressFromTriplet();
321  profiler.leave("sS:compress");
322 
323  try
324  {
325  profiler.enter("sS:chol");
326  if (!ptrCh.get())
327  ptrCh = SparseCholDecompPtr(new CSparseMatrix::CholeskyDecomp(sS) );
328  else ptrCh.get()->update(sS);
329  profiler.leave("sS:chol");
330 
331  profiler.enter("sS:backsub");
332  CVectorDouble bck_res;
333  ptrCh->backsub(e, bck_res); // Ax = b --> delta= x*
334  ::memcpy(&delta[0],&bck_res[0],bck_res.size()*sizeof(bck_res[0])); // delta.slice(0,...) = Ch.backsub(e);
335  profiler.leave("sS:backsub");
336  profiler.leave("sS:ALL");
337  }
338  catch (CExceptionNotDefPos &)
339  {
340  profiler.leave("sS:ALL");
341  // not positive definite so increase mu and try again
342  mu *= nu;
343  nu *= 2.;
344  stop = (mu>999999999.f);
345  continue;
346  }
347 
348  profiler.enter("PostSchur.landmarks");
349 
350  CVectorDouble g(len_free_frames+len_free_points);
351  ::memcpy(&g[0],&e[0],len_free_frames*sizeof(g[0])); //g.slice(0,FrameDof*(num_frames-num_fix_frames)) = e;
352 
353  for (size_t i=0; i<num_free_points; ++i)
354  {
355  Array_P tmp = eps_point[i]; // eps_point.slice(PointDof*i,PointDof);
356 
357  for (size_t j=0; j<num_free_frames; ++j)
358  {
359  WMap::iterator W_ij;
360  W_ij = W.find(make_pair(TCameraPoseID(j+num_fix_frames), TLandmarkID(i+num_fix_points) ));
361 
362  if (W_ij!=W.end())
363  {
364  //tmp -= W_ij->second.T() * delta.slice(j*FrameDof,FrameDof);
365  const Array_F v( &delta[j*FrameDof] );
366  Array_P r;
367  W_ij->second.multiply_Atb(v, r); // r= A^t * v
368  tmp-=r;
369  }
370  }
371  Array_P Vi_tmp;
372  V_inv[i].multiply_Ab(tmp, Vi_tmp); // Vi_tmp = V_inv[i] * tmp
373 
374  ::memcpy(&delta[len_free_frames + i*PointDof], &Vi_tmp[0], sizeof(Vi_tmp[0])*PointDof );
375  ::memcpy(&g[len_free_frames + i*PointDof], &eps_point[i][0], sizeof(eps_point[0][0])*PointDof );
376  }
377  profiler.leave("PostSchur.landmarks");
378 
379 
380  // Vars for temptative new estimates:
381  TFramePosesVec new_frame_poses;
382  TLandmarkLocationsVec new_landmark_points;
383 
384  profiler.enter("add_se3_deltas_to_frames");
386  frame_poses,
387  delta, 0,len_free_frames,
388  new_frame_poses, num_fix_frames );
389  profiler.leave("add_se3_deltas_to_frames");
390 
391  profiler.enter("add_3d_deltas_to_points");
393  landmark_points,
394  delta, len_free_frames, len_free_points,
395  new_landmark_points, num_fix_points );
396  profiler.leave("add_3d_deltas_to_points");
397 
398  vector<Array_O> new_residual_vec(num_obs);
399  vector<double> new_kernel_1st_deriv(num_obs);
400 
401  profiler.enter("reprojectionResiduals");
402  double res_new = mrpt::vision::reprojectionResiduals(
403  observations, camera_params,
404  new_frame_poses, new_landmark_points,
405  new_residual_vec,
406  INV_POSES_BOOL, // are poses inverse?
407  use_robust_kernel,
408  kernel_param,
409  use_robust_kernel ? &new_kernel_1st_deriv : NULL );
410  profiler.leave("reprojectionResiduals");
411 
412  MRPT_CHECK_NORMAL_NUMBER(res_new)
413 
414  has_improved = (res_new<res);
415 
416  if(has_improved)
417  {
418  //rho = (res-)/ (delta.array()*(mu*delta + g).array() ).sum();
419  // Good: Accept new values
420  VERBOSE_COUT << "new total sqr.err=" << res_new << " avr.err(px):"<< std::sqrt(res/num_obs) <<"->" << std::sqrt(res_new/num_obs) << endl;
421 
422  // swap is faster than "="
423  frame_poses.swap(new_frame_poses);
424  landmark_points.swap(new_landmark_points);
425  residual_vec.swap( new_residual_vec );
426  kernel_1st_deriv.swap( new_kernel_1st_deriv );
427 
428  res = res_new;
429 
430  profiler.enter("compute_Jacobians");
431  ba_compute_Jacobians<INV_POSES_BOOL>(frame_poses, landmark_points, camera_params, jac_data_vec, num_fix_frames, num_fix_points);
432  profiler.leave("compute_Jacobians");
433 
434 
435  // Reset to zeros:
436  H_f.assign(num_free_frames,Matrix_FxF() );
437  H_p.assign(num_free_points,Matrix_PxP() );
438  eps_frame.assign(num_free_frames, arrF_zeros);
439  eps_point.assign(num_free_points, arrP_zeros);
440 
441  profiler.enter("build_gradient_Hessians");
442  ba_build_gradient_Hessians(observations,residual_vec,jac_data_vec,H_f,eps_frame,H_p,eps_point, num_fix_frames, num_fix_points, use_robust_kernel ? &kernel_1st_deriv : NULL );
443  profiler.leave("build_gradient_Hessians");
444 
445  stop = norm_inf(g)<=eps;
446  //mu *= max(1.0/3.0, 1-std::pow(2*rho-1,3.0) );
447  mu *= 0.1;
448  mu = std::max(mu, 1e-100);
449  nu = 2.0;
450  }
451  else
452  {
453  VERBOSE_COUT << "no update: res vs.res_new "
454  << res
455  << " vs. "
456  << res_new
457  << endl;
458  mu *= nu;
459  nu *= 2.0;
460  stop = (mu>1e9);
461  }
462 
463  profiler.leave("COMPLETE_ITER");
464  }
465  while(!has_improved && !stop);
466 
467  if (stop)
468  break;
469 
470  } // end for each "iter"
471 
472  // *Warning*: This implementation assumes inverse camera poses: inverse them at the entrance and at exit:
473 #ifdef USE_INVERSE_POSES
474  profiler.enter("invert_poses");
475  for (size_t i=0;i<num_frames;i++)
476  frame_poses[i].inverse();
477  profiler.leave("invert_poses");
478 #endif
479 
480  profiler.leave("bundle_adj_full (complete run)");
481 
482  return res;
483  MRPT_END
484 }
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
const GLdouble * v
Definition: glew.h:1296
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
T getWithDefaultVal(const std::string &s, const T &defaultVal) const
A const version of the [] operator and with a default value in case the parameter is not set (for usa...
Definition: TParameters.h:88
EIGEN_STRONG_INLINE Scalar norm_inf() const
Compute the norm-infinite of a vector ($f[ ||{v}||_ $f]), ie the maximum absolute value of the elemen...
#define ASSERT_ABOVE_(__A, __B)
GLboolean GLboolean g
Definition: glew.h:5406
#define MRPT_CHECK_NORMAL_NUMBER(val)
Scalar * iterator
Definition: eigen_plugins.h:23
This file implements several operations that operate element-wise on individual or pairs of container...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
A sparse matrix structure, wrapping T.
Definition: CSparseMatrix.h:92
const GLuint * ids
Definition: glew.h:1589
void compressFromTriplet()
ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
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
Helper types for STL containers with Eigen memory allocators.
double VISION_IMPEXP bundle_adj_full(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::utils::TCamera &camera_params, mrpt::vision::TFramePosesVec &frame_poses, mrpt::vision::TLandmarkLocationsVec &landmark_points, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble(), const mrpt::vision::TBundleAdjustmentFeedbackFunctor user_feedback=NULL)
Sparse Levenberg-Marquart solution to bundle adjustment - optimizes all the camera frames & the landm...
Definition: ba_full.cpp:51
A complete sequence of observations of features from different camera frames (poses).
#define VERBOSE_COUT
Definition: ba_internals.h:31
Used in mrpt::math::CSparseMatrix.
Definition: CSparseMatrix.h:38
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
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
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
const double eps
#define INV_POSES_BOOL
Definition: ba_full.cpp:33
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
GLuint res
Definition: glew.h:7143
#define MRPT_START
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
uint64_t TLandmarkID
Unique IDs for landmarks.
#define ASSERT_ABOVEEQ_(__A, __B)
void insert_submatrix(const size_t row, const size_t col, const MATRIX &M)
ONLY for TRIPLET matrices: insert a given matrix (in any of the MRPT formats) at a given location of ...
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
#define ASSERT_(f)
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
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
void(* TBundleAdjustmentFeedbackFunctor)(const size_t cur_iter, const double cur_total_sq_error, const size_t max_iters, const mrpt::vision::TSequenceFeatureObservations &input_observations, const mrpt::vision::TFramePosesVec &current_frame_estimate, const mrpt::vision::TLandmarkLocationsVec &current_landmark_estimate)
A functor type for BA methods.
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
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