Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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-lgpl-precomp.h" // Precompiled headers
11 
17 
18 #include <memory> // unique_ptr
19 
20 #include "ba_internals.h"
21 
22 using namespace std;
23 using namespace mrpt;
24 using namespace mrpt::vision;
25 using namespace mrpt::img;
26 using namespace mrpt::math;
27 
28 // Define -> estimate the inverse of poses; Undefined -> estimate the camera
29 // 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, TFramePosesVec& frame_poses,
54  TLandmarkLocationsVec& landmark_points,
55  const mrpt::system::TParametersDouble& extra_params,
56  const TBundleAdjustmentFeedbackFunctor user_feedback)
57 {
59 
60  // Generic BA problem dimension numbers:
61  static const unsigned int FrameDof = 6; // Poses: x y z yaw pitch roll
62  static const unsigned int PointDof = 3; // Landmarks: x y z
63  static const unsigned int ObsDim = 2; // Obs: x y (pixels)
64 
65  // Typedefs for this specific BA problem:
66  using MyJacData = JacData<FrameDof, PointDof, ObsDim>;
67  using MyJacDataVec = mrpt::aligned_std_vector<MyJacData>;
68 
69  using Array_O = std::array<double, ObsDim>;
70  using Array_F = CArrayDouble<FrameDof>;
71  using Array_P = CArrayDouble<PointDof>;
75 
76  // Extra params:
77  const bool use_robust_kernel =
78  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 =
82  extra_params.getWithDefaultVal("max_iterations", 50);
83  const size_t num_fix_frames =
84  extra_params.getWithDefaultVal("num_fix_frames", 1);
85  const size_t num_fix_points =
86  extra_params.getWithDefaultVal("num_fix_points", 0);
87  const double kernel_param =
88  extra_params.getWithDefaultVal("kernel_param", 3.0);
89 
90  const bool enable_profiler =
91  0 != extra_params.getWithDefaultVal("profiler", 0);
92 
93  mrpt::system::CTimeLogger profiler(enable_profiler);
94 
95  profiler.enter("bundle_adj_full (complete run)");
96 
97  // Input data sizes:
98  const size_t num_points = landmark_points.size();
99  const size_t num_frames = frame_poses.size();
100  const size_t num_obs = observations.size();
101 
102  ASSERT_ABOVE_(num_frames, 0);
103  ASSERT_ABOVE_(num_points, 0);
104  ASSERT_(num_fix_frames >= 1);
105  ASSERT_ABOVEEQ_(num_frames, num_fix_frames);
106  ASSERT_ABOVEEQ_(num_points, num_fix_points);
107 
108 #ifdef USE_INVERSE_POSES
109  // *Warning*: This implementation assumes inverse camera poses: inverse them
110  // at the entrance and at exit:
111  profiler.enter("invert_poses");
112  for (size_t i = 0; i < num_frames; i++) frame_poses[i].inverse();
113  profiler.leave("invert_poses");
114 #endif
115 
116  MyJacDataVec jac_data_vec(num_obs);
117  vector<Array_O> residual_vec(num_obs);
118  vector<double> kernel_1st_deriv(num_obs);
119 
120  // prepare structure of sparse Jacobians:
121  for (size_t i = 0; i < num_obs; i++)
122  {
123  jac_data_vec[i].frame_id = observations[i].id_frame;
124  jac_data_vec[i].point_id = observations[i].id_feature;
125  }
126 
127  // Compute sparse Jacobians:
128  profiler.enter("compute_Jacobians");
129  ba_compute_Jacobians<INV_POSES_BOOL>(
130  frame_poses, landmark_points, camera_params, jac_data_vec,
131  num_fix_frames, num_fix_points);
132  profiler.leave("compute_Jacobians");
133 
134  profiler.enter("reprojectionResiduals");
136  observations, camera_params, frame_poses, landmark_points, residual_vec,
137  INV_POSES_BOOL, // are poses inverse?
138  use_robust_kernel, kernel_param,
139  use_robust_kernel ? &kernel_1st_deriv : nullptr);
140  profiler.leave("reprojectionResiduals");
141 
143 
144  VERBOSE_COUT << "res: " << res << endl;
145 
146  // Auxiliary vars:
147  Array_F arrF_zeros;
148  arrF_zeros.assign(0);
149  Array_P arrP_zeros;
150  arrP_zeros.assign(0);
151 
152  const size_t num_free_frames = num_frames - num_fix_frames;
153  const size_t num_free_points = num_points - num_fix_points;
154  const size_t len_free_frames = FrameDof * num_free_frames;
155  const size_t len_free_points = PointDof * num_free_points;
156 
157  mrpt::aligned_std_vector<Matrix_FxF> H_f(num_free_frames);
158  mrpt::aligned_std_vector<Array_F> eps_frame(num_free_frames, arrF_zeros);
159  mrpt::aligned_std_vector<Matrix_PxP> H_p(num_free_points);
160  mrpt::aligned_std_vector<Array_P> eps_point(num_free_points, arrP_zeros);
161 
162  profiler.enter("build_gradient_Hessians");
164  observations, residual_vec, jac_data_vec, H_f, eps_frame, H_p,
165  eps_point, num_fix_frames, num_fix_points,
166  use_robust_kernel ? &kernel_1st_deriv : nullptr);
167  profiler.leave("build_gradient_Hessians");
168 
169  double nu = 2;
170  double eps = 1e-16; // 0.000000000000001;
171  bool stop = false;
172  double mu = initial_mu;
173 
174  // Automatic guess of "mu":
175  if (mu < 0)
176  {
177  double norm_max_A = 0;
178  for (size_t j = 0; j < num_free_frames; ++j)
179  for (size_t dim = 0; dim < FrameDof; dim++)
180  keep_max(norm_max_A, H_f[j](dim, dim));
181 
182  for (size_t i = 0; i < num_free_points; ++i)
183  for (size_t dim = 0; dim < PointDof; dim++)
184  keep_max(norm_max_A, H_p[i](dim, dim));
185  double tau = 1e-3;
186  mu = tau * norm_max_A;
187  }
188 
189  Matrix_FxF I_muFrame(UNINITIALIZED_MATRIX);
190  Matrix_PxP I_muPoint(UNINITIALIZED_MATRIX);
191 
192  // Cholesky object, as a pointer to reuse it between iterations:
193  using SparseCholDecompPtr = std::unique_ptr<CSparseMatrix::CholeskyDecomp>;
194 
195  SparseCholDecompPtr ptrCh;
196 
197  for (size_t iter = 0; iter < max_iters; iter++)
198  {
199  VERBOSE_COUT << "iteration: " << iter << endl;
200 
201  // provide feedback to the user:
202  if (user_feedback)
203  user_feedback(
204  iter, res, max_iters, observations, frame_poses,
205  landmark_points);
206 
207  bool has_improved = false;
208  do
209  {
210  profiler.enter("COMPLETE_ITER");
211 
212  VERBOSE_COUT << "mu: " << mu << endl;
213 
214  I_muFrame.unit(FrameDof, mu);
215  I_muPoint.unit(PointDof, mu);
216 
218  num_free_frames, I_muFrame);
219  mrpt::aligned_std_vector<Matrix_PxP> V_inv(num_free_points);
220 
221  for (size_t i = 0; i < U_star.size(); ++i) U_star[i] += H_f[i];
222 
223  for (size_t i = 0; i < H_p.size(); ++i)
224  (H_p[i] + I_muPoint).inv_fast(V_inv[i]);
225 
227  Matrix_FxP>;
228  WMap W, Y;
229 
230  // For quick look-up of entries in W affecting a given point ID:
231  vector<vector<WMap::iterator>> W_entries(
232  num_points); // Index is "TLandmarkID"
233 
234  MyJacDataVec::const_iterator jac_iter = jac_data_vec.begin();
236  observations.begin();
237  it_obs != observations.end(); ++it_obs)
238  {
239  const TFeatureID feat_id = it_obs->id_feature;
240  const TCameraPoseID frame_id = it_obs->id_frame;
241 
242  if (jac_iter->J_frame_valid && jac_iter->J_point_valid)
243  {
245  J_frame = jac_iter->J_frame;
247  J_point = jac_iter->J_point;
248  const pair<TCameraPoseID, TLandmarkID> id_pair =
249  make_pair(frame_id, feat_id);
250 
251  Matrix_FxP tmp(UNINITIALIZED_MATRIX);
252  tmp.multiply_AtB(J_frame, J_point);
253 
254  // W[ids] = J_f^T * J_p
255  // Was: W[id_pair] = tmp;
256  const pair<WMap::iterator, bool>& retInsert =
257  W.insert(make_pair(id_pair, tmp));
258  ASSERT_(retInsert.second == true);
259  W_entries[feat_id].push_back(
260  retInsert.first); // Keep the iterator
261 
262  // Y[ids] = W[ids] * H_p^{-1}
263  Y[id_pair].multiply_AB(
264  tmp, V_inv[feat_id - num_fix_points]);
265  }
266  ++jac_iter;
267  }
268 
270  YW_map;
271  for (size_t i = 0; i < H_f.size(); ++i)
272  YW_map[std::pair<TCameraPoseID, TLandmarkID>(i, i)] = U_star[i];
273 
274  CVectorDouble delta(
275  len_free_frames + len_free_points); // The optimal step
276  CVectorDouble e(len_free_frames);
277 
278  profiler.enter("Schur.build.reduced.frames");
279  for (size_t j = 0; j < num_free_frames; ++j)
280  ::memcpy(
281  &e[j * FrameDof], &eps_frame[j][0],
282  sizeof(e[0]) * FrameDof); // e.slice(j*FrameDof,FrameDof) =
283  // AT(eps_frame,j);
284 
285  for (WMap::iterator Y_ij = Y.begin(); Y_ij != Y.end(); ++Y_ij)
286  {
287  const TLandmarkID point_id = Y_ij->first.second;
288  const size_t i =
289  Y_ij->first.second - num_fix_points; // point index
290  const size_t j =
291  Y_ij->first.first - num_fix_frames; // frame index
292 
293  const vector<WMap::iterator>& iters =
294  W_entries[point_id]; //->second;
295 
296  for (size_t itIdx = 0; itIdx < iters.size(); itIdx++)
297  // for (size_t k=0; k<num_free_frames; ++k)
298  {
299  const WMap::iterator& W_ik = iters[itIdx];
300  const TLandmarkID k = W_ik->first.first - num_fix_frames;
301 
302  Matrix_FxF YWt(UNINITIALIZED_MATRIX); //-(Y_ij->second) *
303  //(W_ik->second).T();
304  YWt.multiply_ABt(Y_ij->second, W_ik->second);
305  // YWt*=-1.0; // The "-" sign is taken into account below:
306 
307  const pair<TCameraPoseID, TLandmarkID> ids_jk =
308  pair<TCameraPoseID, TLandmarkID>(j, k);
309 
310  auto it = YW_map.find(ids_jk);
311  if (it != YW_map.end())
312  it->second -= YWt; // += (-YWt);
313  else
314  YW_map[ids_jk] = YWt * (-1.0);
315  }
316 
318  Y_ij->second.multiply_Ab(eps_point[i], r);
319  for (size_t k = 0; k < FrameDof; k++)
320  e[j * FrameDof + k] -= r[k];
321  }
322  profiler.leave("Schur.build.reduced.frames");
323 
324  profiler.enter("sS:ALL");
325  profiler.enter("sS:fill");
326 
327  VERBOSE_COUT << "Entries in YW_map:" << YW_map.size() << endl;
328 
329  CSparseMatrix sS(len_free_frames, len_free_frames);
330 
331  for (auto it = YW_map.begin(); it != YW_map.end(); ++it)
332  {
333  const pair<TCameraPoseID, TLandmarkID>& ids = it->first;
334  const Matrix_FxF& YW = it->second;
335  sS.insert_submatrix(
336  ids.first * FrameDof, ids.second * FrameDof, YW);
337  }
338  profiler.leave("sS:fill");
339 
340  // Compress the sparse matrix:
341  profiler.enter("sS:compress");
342  sS.compressFromTriplet();
343  profiler.leave("sS:compress");
344 
345  try
346  {
347  profiler.enter("sS:chol");
348  if (!ptrCh.get())
349  ptrCh.reset(new CSparseMatrix::CholeskyDecomp(sS));
350  else
351  ptrCh.get()->update(sS);
352  profiler.leave("sS:chol");
353 
354  profiler.enter("sS:backsub");
355  CVectorDouble bck_res;
356  ptrCh->backsub(e, bck_res); // Ax = b --> delta= x*
357  ::memcpy(
358  &delta[0], &bck_res[0],
359  bck_res.size() * sizeof(bck_res[0])); // delta.slice(0,...)
360  // = Ch.backsub(e);
361  profiler.leave("sS:backsub");
362  profiler.leave("sS:ALL");
363  }
364  catch (CExceptionNotDefPos&)
365  {
366  profiler.leave("sS:ALL");
367  // not positive definite so increase mu and try again
368  mu *= nu;
369  nu *= 2.;
370  stop = (mu > 999999999.f);
371  continue;
372  }
373 
374  profiler.enter("PostSchur.landmarks");
375 
376  CVectorDouble g(len_free_frames + len_free_points);
377  ::memcpy(
378  &g[0], &e[0],
379  len_free_frames *
380  sizeof(
381  g[0])); // g.slice(0,FrameDof*(num_frames-num_fix_frames))
382  // = e;
383 
384  for (size_t i = 0; i < num_free_points; ++i)
385  {
386  Array_P tmp =
387  eps_point[i]; // eps_point.slice(PointDof*i,PointDof);
388 
389  for (size_t j = 0; j < num_free_frames; ++j)
390  {
391  WMap::iterator W_ij;
392  W_ij = W.find(
393  make_pair(
394  TCameraPoseID(j + num_fix_frames),
395  TLandmarkID(i + num_fix_points)));
396 
397  if (W_ij != W.end())
398  {
399  // tmp -= W_ij->second.T() *
400  // delta.slice(j*FrameDof,FrameDof);
401  const Array_F v(&delta[j * FrameDof]);
402  Array_P r;
403  W_ij->second.multiply_Atb(v, r); // r= A^t * v
404  tmp -= r;
405  }
406  }
407  Array_P Vi_tmp;
408  V_inv[i].multiply_Ab(tmp, Vi_tmp); // Vi_tmp = V_inv[i] * tmp
409 
410  ::memcpy(
411  &delta[len_free_frames + i * PointDof], &Vi_tmp[0],
412  sizeof(Vi_tmp[0]) * PointDof);
413  ::memcpy(
414  &g[len_free_frames + i * PointDof], &eps_point[i][0],
415  sizeof(eps_point[0][0]) * PointDof);
416  }
417  profiler.leave("PostSchur.landmarks");
418 
419  // Vars for temptative new estimates:
420  TFramePosesVec new_frame_poses;
421  TLandmarkLocationsVec new_landmark_points;
422 
423  profiler.enter("add_se3_deltas_to_frames");
425  frame_poses, delta, 0, len_free_frames, new_frame_poses,
426  num_fix_frames);
427  profiler.leave("add_se3_deltas_to_frames");
428 
429  profiler.enter("add_3d_deltas_to_points");
431  landmark_points, delta, len_free_frames, len_free_points,
432  new_landmark_points, num_fix_points);
433  profiler.leave("add_3d_deltas_to_points");
434 
435  vector<Array_O> new_residual_vec(num_obs);
436  vector<double> new_kernel_1st_deriv(num_obs);
437 
438  profiler.enter("reprojectionResiduals");
439  double res_new = mrpt::vision::reprojectionResiduals(
440  observations, camera_params, new_frame_poses,
441  new_landmark_points, new_residual_vec,
442  INV_POSES_BOOL, // are poses inverse?
443  use_robust_kernel, kernel_param,
444  use_robust_kernel ? &new_kernel_1st_deriv : nullptr);
445  profiler.leave("reprojectionResiduals");
446 
447  MRPT_CHECK_NORMAL_NUMBER(res_new);
448 
449  has_improved = (res_new < res);
450 
451  if (has_improved)
452  {
453  // rho = (res-)/ (delta.array()*(mu*delta + g).array() ).sum();
454  // Good: Accept new values
455  VERBOSE_COUT << "new total sqr.err=" << res_new
456  << " avr.err(px):" << std::sqrt(res / num_obs)
457  << "->" << std::sqrt(res_new / num_obs) << endl;
458 
459  // swap is faster than "="
460  frame_poses.swap(new_frame_poses);
461  landmark_points.swap(new_landmark_points);
462  residual_vec.swap(new_residual_vec);
463  kernel_1st_deriv.swap(new_kernel_1st_deriv);
464 
465  res = res_new;
466 
467  profiler.enter("compute_Jacobians");
468  ba_compute_Jacobians<INV_POSES_BOOL>(
469  frame_poses, landmark_points, camera_params, jac_data_vec,
470  num_fix_frames, num_fix_points);
471  profiler.leave("compute_Jacobians");
472 
473  // Reset to zeros:
474  H_f.assign(num_free_frames, Matrix_FxF());
475  H_p.assign(num_free_points, Matrix_PxP());
476  eps_frame.assign(num_free_frames, arrF_zeros);
477  eps_point.assign(num_free_points, arrP_zeros);
478 
479  profiler.enter("build_gradient_Hessians");
481  observations, residual_vec, jac_data_vec, H_f, eps_frame,
482  H_p, eps_point, num_fix_frames, num_fix_points,
483  use_robust_kernel ? &kernel_1st_deriv : nullptr);
484  profiler.leave("build_gradient_Hessians");
485 
486  stop = norm_inf(g) <= eps;
487  // mu *= max(1.0/3.0, 1-std::pow(2*rho-1,3.0) );
488  mu *= 0.1;
489  mu = std::max(mu, 1e-100);
490  nu = 2.0;
491  }
492  else
493  {
494  VERBOSE_COUT << "no update: res vs.res_new " << res << " vs. "
495  << res_new << endl;
496  mu *= nu;
497  nu *= 2.0;
498  stop = (mu > 1e9);
499  }
500 
501  profiler.leave("COMPLETE_ITER");
502  } while (!has_improved && !stop);
503 
504  if (stop) break;
505 
506  } // end for each "iter"
507 
508 // *Warning*: This implementation assumes inverse camera poses: inverse them at
509 // the entrance and at exit:
510 #ifdef USE_INVERSE_POSES
511  profiler.enter("invert_poses");
512  for (size_t i = 0; i < num_frames; i++) frame_poses[i].inverse();
513  profiler.leave("invert_poses");
514 #endif
515 
516  profiler.leave("bundle_adj_full (complete run)");
517 
518  return res;
519  MRPT_END
520 }
Scalar * iterator
Definition: eigen_plugins.h:26
GLuint * ids
Definition: glext.h:3906
uint64_t TCameraPoseID
Unique IDs for camera frames (poses)
#define MRPT_START
Definition: exceptions.h:262
uint64_t TFeatureID
Definition of a feature ID.
uint64_t TLandmarkID
Unique IDs for landmarks.
Used in mrpt::math::CSparseMatrix.
Definition: CSparseMatrix.h:36
This file implements several operations that operate element-wise on individual or pairs of container...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
A sparse matrix structure, wrapping T.
Definition: CSparseMatrix.h:97
STL namespace.
void compressFromTriplet()
ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
void 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:328
std::map< KEY, VALUE, std::less< KEY >, mrpt::aligned_allocator_cpp11< std::pair< const KEY, VALUE > >> aligned_std_map
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
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...
A complete sequence of observations of features from different camera frames (poses).
#define VERBOSE_COUT
Definition: ba_internals.h:46
#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.
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...
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
Definition: exceptions.h:118
void 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:364
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:82
GLubyte g
Definition: glext.h:6279
const double eps
#define INV_POSES_BOOL
Definition: ba_full.cpp:33
Classes for computer vision, detectors, features, etc.
Definition: CCamModel.h:20
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:29
double leave(const char *func_name)
End of a named section.
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
double reprojectionResiduals(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::img::TCamera &camera_params, const mrpt::vision::TFramePosesVec &frame_poses, const mrpt::vision::TLandmarkLocationsVec &landmark_points, std::vector< std::array< 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=nullptr)
Compute reprojection error vector (used from within Bundle Adjustment methods, but can be used in gen...
Definition: ba_common.cpp:205
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
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 ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:171
#define MRPT_END
Definition: exceptions.h:266
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 ...
mrpt::aligned_std_vector< mrpt::poses::CPose3D > TFramePosesVec
A list of camera frames (6D poses), which assumes indexes are unique, consecutive IDs...
GLuint res
Definition: glext.h:7268
double bundle_adj_full(const mrpt::vision::TSequenceFeatureObservations &observations, const mrpt::img::TCamera &camera_params, mrpt::vision::TFramePosesVec &frame_poses, mrpt::vision::TLandmarkLocationsVec &landmark_points, const mrpt::system::TParametersDouble &extra_params=mrpt::system::TParametersDouble(), const mrpt::vision::TBundleAdjustmentFeedbackFunctor user_feedback=mrpt::vision::TBundleAdjustmentFeedbackFunctor())
Sparse Levenberg-Marquart solution to bundle adjustment - optimizes all the camera frames & the landm...
Definition: ba_full.cpp:51
std::function< void(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)> TBundleAdjustmentFeedbackFunctor
A functor type for BA methods.
std::vector< mrpt::math::TPoint3D > TLandmarkLocationsVec
A list of landmarks (3D points), which assumes indexes are unique, consecutive IDs.
const Scalar * const_iterator
Definition: eigen_plugins.h:27
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:105
void enter(const char *func_name)
Start of a named section.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:356



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