Main MRPT website > C++ reference
MRPT logo
lev-marq_solvers.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-2014, 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 #pragma once
11 
12 #include <mrpt/math/CSparseMatrix.h>
13 #include <memory> // for auto_ptr, unique_ptr
14 
15 namespace mrpt { namespace srba {
16 
17 using namespace mrpt;
18 using namespace mrpt::math;
19 using namespace std;
20 
21 namespace internal
22 {
23 #if MRPT_HAS_CXX11
24  typedef std::unique_ptr<CSparseMatrix::CholeskyDecomp> SparseCholeskyDecompPtr;
25 #else
26  typedef std::auto_ptr<CSparseMatrix::CholeskyDecomp> SparseCholeskyDecompPtr;
27 #endif
28 
29  // ------------------------------------------------------------------------------------------
30  /** SOLVER: Lev-Marq without Schur, with Sparse Cholesky (CSparse library) */
31  template <class RBA_ENGINE>
32  struct solver_engine<false /*Schur*/,false /*dense Chol*/,RBA_ENGINE>
33  {
34  typedef typename RBA_ENGINE::hessian_traits_t hessian_traits_t;
35 
36  static const size_t POSE_DIMS = RBA_ENGINE::kf2kf_pose_type::REL_POSE_DIMS;
37  static const size_t LM_DIMS = RBA_ENGINE::lm_type::LM_DIMS;
38 
39  const int m_verbose_level;
40  mrpt::utils::CTimeLogger &m_profiler;
41  Eigen::VectorXd delta_eps; //!< The result of solving Ax=b will be stored here
42  typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp;
43  typename hessian_traits_t::TSparseBlocksHessian_f &Hf;
44  typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf;
45  Eigen::VectorXd &minus_grad;
46  const size_t nUnknowns_k2k, nUnknowns_k2f, nUnknowns_scalars,idx_start_f;
47 
48  CSparseMatrix* sS; //!< Sparse Hessian
49  bool sS_is_valid; //!< Whether the Hessian was filled in, in sS
50  /** Cholesky object, as a pointer to reuse it between iterations */
51  SparseCholeskyDecompPtr ptrCh;
52 
53  /** Constructor */
54  solver_engine(
55  const int verbose_level,
56  mrpt::utils::CTimeLogger & profiler,
57  typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp_,
58  typename hessian_traits_t::TSparseBlocksHessian_f &Hf_,
59  typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf_,
60  Eigen::VectorXd &minus_grad_,
61  const size_t nUnknowns_k2k_,
62  const size_t nUnknowns_k2f_) :
63  m_verbose_level(verbose_level),
64  m_profiler(profiler),
65  HAp(HAp_), Hf(Hf_), HApf(HApf_),
66  minus_grad(minus_grad_),
67  nUnknowns_k2k(nUnknowns_k2k_),
68  nUnknowns_k2f(nUnknowns_k2f_),
69  nUnknowns_scalars( POSE_DIMS*nUnknowns_k2k + LM_DIMS*nUnknowns_k2f ),
70  idx_start_f(POSE_DIMS*nUnknowns_k2k),
71  sS(NULL), sS_is_valid(false)
72  {
73  }
74 
75  ~solver_engine()
76  {
77  if (sS) { delete sS; sS=NULL; }
78  }
79 
80  // ----------------------------------------------------------------------
81  // Solve the entire H Ax = -g system with H as a single sparse Hessian.
82  // Return: true on success. false to retry with a different lambda (Lev-Marq algorithm is assumed)
83  // ----------------------------------------------------------------------
84  bool solve(const double lambda)
85  {
86  DETAILED_PROFILING_ENTER("opt.SparseTripletFill")
87 
88  if (!sS) sS = new CSparseMatrix(nUnknowns_scalars,nUnknowns_scalars);
89  else sS->clear(nUnknowns_scalars,nUnknowns_scalars);
90  sS_is_valid=false;
91 
92  // 1/3: Hp --------------------------------------
93  for (size_t i=0;i<nUnknowns_k2k;i++)
94  { // Only upper-half triangle:
95  const typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t & col_i = HAp.getCol(i);
96 
97  for (typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t::const_iterator itRowEntry = col_i.begin();itRowEntry != col_i.end(); ++itRowEntry )
98  {
99  if (itRowEntry->first==i)
100  {
101  // block Diagonal: Add lambda*I to these ones
102  typename hessian_traits_t::TSparseBlocksHessian_Ap::matrix_t sSii = itRowEntry->second.num;
103  for (size_t k=0;k<POSE_DIMS;k++)
104  sSii.coeffRef(k,k)+=lambda;
105  sS->insert_submatrix(POSE_DIMS*i,POSE_DIMS*i, sSii );
106  }
107  else
108  {
109  sS->insert_submatrix(
110  POSE_DIMS*itRowEntry->first, // row index
111  POSE_DIMS*i, // col index
112  itRowEntry->second.num );
113  }
114  }
115  }
116 
117  // 2/3: HApf --------------------------------------
118  for (size_t i=0;i<nUnknowns_k2k;i++)
119  {
120  const typename hessian_traits_t::TSparseBlocksHessian_Apf::col_t & row_i = HApf.getCol(i);
121 
122  for (typename hessian_traits_t::TSparseBlocksHessian_Apf::col_t::const_iterator itColEntry = row_i.begin();itColEntry != row_i.end(); ++itColEntry )
123  {
124  sS->insert_submatrix(
125  POSE_DIMS*i, // row index
126  idx_start_f+LM_DIMS*itColEntry->first, // col index
127  itColEntry->second.num );
128  }
129  }
130 
131  // 3/3: Hf --------------------------------------
132  for (size_t i=0;i<nUnknowns_k2f;i++)
133  { // Only upper-half triangle:
134  const typename hessian_traits_t::TSparseBlocksHessian_f::col_t & col_i = Hf.getCol(i);
135 
136  for (typename hessian_traits_t::TSparseBlocksHessian_f::col_t::const_iterator itRowEntry = col_i.begin();itRowEntry != col_i.end(); ++itRowEntry )
137  {
138  if (itRowEntry->first==i)
139  {
140  // block Diagonal: Add lambda*I to these ones
141  typename hessian_traits_t::TSparseBlocksHessian_f::matrix_t sSii = itRowEntry->second.num;
142  for (size_t k=0;k<LM_DIMS;k++)
143  sSii.coeffRef(k,k)+=lambda;
144  sS->insert_submatrix(idx_start_f+LM_DIMS*i,idx_start_f+LM_DIMS*i, sSii );
145  }
146  else
147  {
148  sS->insert_submatrix(
149  idx_start_f+LM_DIMS*itRowEntry->first, // row index
150  idx_start_f+LM_DIMS*i, // col index
151  itRowEntry->second.num );
152  }
153  }
154  }
155  DETAILED_PROFILING_LEAVE("opt.SparseTripletFill")
156 
157  // Compress the sparse matrix:
158  // --------------------------------------
159  DETAILED_PROFILING_ENTER("opt.SparseTripletCompress")
160  sS->compressFromTriplet();
161  DETAILED_PROFILING_LEAVE("opt.SparseTripletCompress")
162 
163  // Sparse cholesky
164  // --------------------------------------
165  DETAILED_PROFILING_ENTER("opt.SparseChol")
166  try
167  {
168  if (!ptrCh.get())
169  ptrCh = SparseCholeskyDecompPtr(new CSparseMatrix::CholeskyDecomp(*sS) );
170  else ptrCh.get()->update(*sS);
171 
172  sS_is_valid=true;
173 
174  DETAILED_PROFILING_LEAVE("opt.SparseChol")
175  }
176  catch (CExceptionNotDefPos &)
177  {
178  DETAILED_PROFILING_LEAVE("opt.SparseChol")
179  return false;
180  }
181 
182  // backsubtitution gives us "DeltaEps" from Cholesky and "-grad":
183  //
184  // (J^tJ + lambda*I) DeltaEps = -grad
185  // ----------------------------------------------------------------
186  DETAILED_PROFILING_ENTER("opt.backsub")
187  ptrCh->backsub(minus_grad,delta_eps);
188  DETAILED_PROFILING_LEAVE("opt.backsub")
189 
190  return true; // solved OK.
191  }
192 
193  void realize_relinearized()
194  {
195  // Nothing to do.
196  }
197  void realize_lambda_changed()
198  {
199  // Nothing to do.
200  }
201  bool was_ith_feature_invertible(const size_t i)
202  {
203  MRPT_UNUSED_PARAM(i);
204  return true;
205  }
206 
207  /** Here, out_info is of type mrpt::srba::options::solver_LM_schur_sparse_cholesky::extra_results_t */
208  void get_extra_results(typename RBA_ENGINE::rba_options_type::solver_t::extra_results_t & out_info )
209  {
210  out_info.hessian_valid = sS_is_valid;
211  if (sS) sS->swap(out_info.hessian);
212  }
213  };
214 
215  // ------------------------------------------------------------------------------------------
216  /** SOLVER: Lev-Marq with Schur, with Sparse Cholesky (CSparse library) */
217  template <class RBA_ENGINE>
218  struct solver_engine<true /*Schur*/,false /*dense Chol*/,RBA_ENGINE>
219  {
220  typedef typename RBA_ENGINE::hessian_traits_t hessian_traits_t;
221 
222  static const size_t POSE_DIMS = RBA_ENGINE::kf2kf_pose_type::REL_POSE_DIMS;
223  static const size_t LM_DIMS = RBA_ENGINE::lm_type::LM_DIMS;
224 
225  const int m_verbose_level;
226  mrpt::utils::CTimeLogger &m_profiler;
227 
228  const size_t nUnknowns_k2k, nUnknowns_k2f;
229  Eigen::VectorXd delta_eps;
230  typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp;
231  typename hessian_traits_t::TSparseBlocksHessian_f &Hf;
232  typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf;
233  Eigen::VectorXd &minus_grad;
234 
235  CSparseMatrix* sS; //!< Sparse Hessian
236  bool sS_is_valid; //!< Whether the Hessian was filled in, in sS
237  /** Cholesky object, as a pointer to reuse it between iterations */
238  SparseCholeskyDecompPtr ptrCh;
239 
240  SchurComplement<
241  typename hessian_traits_t::TSparseBlocksHessian_Ap,
242  typename hessian_traits_t::TSparseBlocksHessian_f,
243  typename hessian_traits_t::TSparseBlocksHessian_Apf
244  >
245  schur_compl;
246 
247  /** Constructor */
248  solver_engine(
249  const int verbose_level,
250  mrpt::utils::CTimeLogger & profiler,
251  typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp_,
252  typename hessian_traits_t::TSparseBlocksHessian_f &Hf_,
253  typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf_,
254  Eigen::VectorXd &minus_grad_,
255  const size_t nUnknowns_k2k_,
256  const size_t nUnknowns_k2f_) :
257  m_verbose_level(verbose_level),
258  m_profiler(profiler),
259  nUnknowns_k2k(nUnknowns_k2k_),
260  nUnknowns_k2f(nUnknowns_k2f_),
261  HAp(HAp_),Hf(Hf_),HApf(HApf_),
262  minus_grad(minus_grad_),
263  sS(NULL), sS_is_valid(false),
264  schur_compl(
265  HAp_,Hf_,HApf_, // The different symbolic/numeric Hessians
266  &minus_grad[0], // minus gradient of the Ap part
267  // Handle case of no unknown features:
268  nUnknowns_k2f!=0 ? &minus_grad[POSE_DIMS*nUnknowns_k2k] : NULL // minus gradient of the features part
269  )
270  {
271  }
272 
273  ~solver_engine()
274  {
275  if (sS) { delete sS; sS=NULL; }
276  }
277 
278  // ----------------------------------------------------------------------
279  // Solve the H·Ax = -g system using the Schur complement to generate a
280  // Ap-only reduced system.
281  // Return: always true (success).
282  // ----------------------------------------------------------------------
283  bool solve(const double lambda)
284  {
285  // 1st: Numeric part: Update HAp hessian into the reduced system
286  // Note: We have to re-evaluate the entire reduced Hessian HAp even if
287  // only lambda changed, because of the terms inv(Hf+\lambda*I).
288 
289  DETAILED_PROFILING_ENTER("opt.schur_build_reduced")
290  schur_compl.numeric_build_reduced_system(lambda);
291  DETAILED_PROFILING_LEAVE("opt.schur_build_reduced")
292 
293  if (schur_compl.getNumFeaturesFullRank()!=schur_compl.getNumFeatures())
294  VERBOSE_LEVEL(1) << "[OPT] Schur warning: only " << schur_compl.getNumFeaturesFullRank() << " out of " << schur_compl.getNumFeatures() << " features have full-rank.\n";
295 
296  // Only the H_Ap part of the Hessian:
297  if (!sS) sS = new CSparseMatrix(nUnknowns_k2k*POSE_DIMS,nUnknowns_k2k*POSE_DIMS);
298  else sS->clear(nUnknowns_k2k*POSE_DIMS,nUnknowns_k2k*POSE_DIMS);
299  sS_is_valid=false;
300 
301 
302  // Now write the updated "HAp" into its sparse matrix form:
303  DETAILED_PROFILING_ENTER("opt.SparseTripletFill")
304  for (size_t i=0;i<nUnknowns_k2k;i++)
305  { // Only upper-half triangle:
306  const typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t & col_i = HAp.getCol(i);
307 
308  for (typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t::const_iterator itRowEntry = col_i.begin();itRowEntry != col_i.end(); ++itRowEntry )
309  {
310  if (itRowEntry->first==i)
311  {
312  // block Diagonal: Add lambda*I to these ones
313  typename hessian_traits_t::TSparseBlocksHessian_Ap::matrix_t sSii = itRowEntry->second.num;
314  for (size_t k=0;k<POSE_DIMS;k++)
315  sSii.coeffRef(k,k)+=lambda;
316  sS->insert_submatrix(POSE_DIMS*i,POSE_DIMS*i, sSii );
317  }
318  else
319  {
320  sS->insert_submatrix(
321  POSE_DIMS*itRowEntry->first, // row index
322  POSE_DIMS*i, // col index
323  itRowEntry->second.num );
324  }
325  }
326  }
327  DETAILED_PROFILING_LEAVE("opt.SparseTripletFill")
328 
329  // Compress the sparse matrix:
330  // ----------------------------------
331  DETAILED_PROFILING_ENTER("opt.SparseTripletCompress")
332  sS->compressFromTriplet();
333  DETAILED_PROFILING_LEAVE("opt.SparseTripletCompress")
334 
335  // Sparse cholesky:
336  // ----------------------------------
337  DETAILED_PROFILING_ENTER("opt.SparseChol")
338  try
339  {
340  if (!ptrCh.get())
341  ptrCh = SparseCholeskyDecompPtr(new CSparseMatrix::CholeskyDecomp(*sS) );
342  else ptrCh.get()->update(*sS);
343 
344  sS_is_valid=true;
345 
346  DETAILED_PROFILING_LEAVE("opt.SparseChol")
347  }
348  catch (CExceptionNotDefPos &)
349  {
350  DETAILED_PROFILING_LEAVE("opt.SparseChol")
351  // not positive definite so increase lambda and try again
352  return false;
353  }
354 
355  // backsubtitution gives us "DeltaEps" from Cholesky and "-grad":
356  //
357  // (J^tJ + lambda*I) DeltaEps = -grad
358  // ----------------------------------------------------------------
359  DETAILED_PROFILING_ENTER("opt.backsub")
360 
361  delta_eps.assign(nUnknowns_k2k*POSE_DIMS + nUnknowns_k2f*LM_DIMS, 0); // Important: It's initialized to zeros!
362 
363  ptrCh->backsub(&minus_grad[0],&delta_eps[0],nUnknowns_k2k*POSE_DIMS);
364 
365  DETAILED_PROFILING_LEAVE("opt.backsub")
366 
367  // If using the Schur complement, at this point we must now solve the secondary
368  // system for features:
369  // -----------------------------------------------------------------------------
370  // 2nd numeric part: Solve for increments in features ----------
371  DETAILED_PROFILING_ENTER("opt.schur_features")
372 
373  schur_compl.numeric_solve_for_features(
374  &delta_eps[0],
375  // Handle case of no unknown features:
376  nUnknowns_k2f!=0 ? &delta_eps[nUnknowns_k2k*POSE_DIMS] : NULL // minus gradient of the features part
377  );
378 
379  DETAILED_PROFILING_LEAVE("opt.schur_features")
380 
381  return true;
382  } // end solve()
383 
384 
385  void realize_relinearized()
386  {
387  DETAILED_PROFILING_ENTER("opt.schur_realize_HAp_changed")
388  // Update the starting value of HAp for Schur:
389  schur_compl.realize_HAp_changed();
390  DETAILED_PROFILING_LEAVE("opt.schur_realize_HAp_changed")
391  }
392  void realize_lambda_changed()
393  {
394  // Nothing to do.
395  }
396  bool was_ith_feature_invertible(const size_t i)
397  {
398  return schur_compl.was_ith_feature_invertible(i);
399  }
400  /** Here, out_info is of type mrpt::srba::options::solver_LM_no_schur_sparse_cholesky::extra_results_t */
401  void get_extra_results(typename RBA_ENGINE::rba_options_type::solver_t::extra_results_t & out_info )
402  {
403  out_info.hessian_valid = sS_is_valid;
404  if (sS) sS->swap(out_info.hessian);
405  }
406  };
407 
408  // ------------------------------------------------------------------------------------------
409  /** SOLVER: Lev-Marq with Schur, with Sparse Cholesky (CSparse library) */
410  template <class RBA_ENGINE>
411  struct solver_engine<true /*Schur*/,true /*dense Chol*/,RBA_ENGINE>
412  {
413  typedef typename RBA_ENGINE::hessian_traits_t hessian_traits_t;
414 
415  static const size_t POSE_DIMS = RBA_ENGINE::kf2kf_pose_type::REL_POSE_DIMS;
416  static const size_t LM_DIMS = RBA_ENGINE::lm_type::LM_DIMS;
417 
418  const int m_verbose_level;
419  mrpt::utils::CTimeLogger &m_profiler;
420  const size_t nUnknowns_k2k, nUnknowns_k2f;
421 
422  Eigen::VectorXd delta_eps; //!< The result of solving Ax=b will be stored here
423  typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp;
424  typename hessian_traits_t::TSparseBlocksHessian_f &Hf;
425  typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf;
426  Eigen::VectorXd &minus_grad;
427 
428  SchurComplement<
429  typename hessian_traits_t::TSparseBlocksHessian_Ap,
430  typename hessian_traits_t::TSparseBlocksHessian_f,
431  typename hessian_traits_t::TSparseBlocksHessian_Apf
432  >
433  schur_compl;
434 
435 
436  // Data for dense cholesky:
437  Eigen::MatrixXd denseH;
438  Eigen::LLT<Eigen::MatrixXd,Eigen::Upper> denseChol;
439  bool denseChol_is_uptodate;
440  bool hessian_is_valid; //!< Whether the hessian was correctly evaluated at least once.
441 
442 
443  /** Constructor */
444  solver_engine(
445  const int verbose_level,
446  mrpt::utils::CTimeLogger & profiler,
447  typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp_,
448  typename hessian_traits_t::TSparseBlocksHessian_f &Hf_,
449  typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf_,
450  Eigen::VectorXd &minus_grad_,
451  const size_t nUnknowns_k2k_,
452  const size_t nUnknowns_k2f_) :
453  m_verbose_level(verbose_level),
454  m_profiler(profiler),
455  nUnknowns_k2k(nUnknowns_k2k_),
456  nUnknowns_k2f(nUnknowns_k2f_),
457  HAp(HAp_),Hf(Hf_),HApf(HApf_),
458  minus_grad(minus_grad_),
459  schur_compl(
460  HAp_,Hf_,HApf_, // The different symbolic/numeric Hessians
461  &minus_grad[0], // minus gradient of the Ap part
462  // Handle case of no unknown features:
463  nUnknowns_k2f!=0 ? &minus_grad[POSE_DIMS*nUnknowns_k2k] : NULL // minus gradient of the features part
464  ),
465  denseChol_is_uptodate (false),
466  hessian_is_valid (false)
467  {
468  }
469 
470  // ----------------------------------------------------------------------
471  // Solve the H·Ax = -g system using the Schur complement to generate a
472  // Ap-only reduced system.
473  // Return: always true (success).
474  // ----------------------------------------------------------------------
475  bool solve(const double lambda)
476  {
477  // 1st: Numeric part: Update HAp hessian into the reduced system
478  // Note: We have to re-evaluate the entire reduced Hessian HAp even if
479  // only lambda changed, because of the terms inv(Hf+\lambda*I).
480 
481  DETAILED_PROFILING_ENTER("opt.schur_build_reduced")
482  schur_compl.numeric_build_reduced_system(lambda);
483  DETAILED_PROFILING_LEAVE("opt.schur_build_reduced")
484 
485  if (schur_compl.getNumFeaturesFullRank()!=schur_compl.getNumFeatures())
486  VERBOSE_LEVEL(1) << "[OPT] Schur warning: only " << schur_compl.getNumFeaturesFullRank() << " out of " << schur_compl.getNumFeatures() << " features have full-rank.\n";
487 
488  if (!denseChol_is_uptodate)
489  {
490  // Use a dense Cholesky method for solving the set of unknowns:
491  denseH.setZero(nUnknowns_k2k*POSE_DIMS,nUnknowns_k2k*POSE_DIMS); // Only for the H_Ap part of the Hessian
492  hessian_is_valid = false;
493 
494  // Now write the updated "HAp" into its sparse matrix form:
495  DETAILED_PROFILING_ENTER("opt.DenseFill")
496  for (size_t i=0;i<nUnknowns_k2k;i++)
497  { // Only upper-half triangle:
498  const typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t & col_i = HAp.getCol(i);
499 
500  for (typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t::const_iterator itRowEntry = col_i.begin();itRowEntry != col_i.end(); ++itRowEntry )
501  {
502  if (itRowEntry->first==i)
503  {
504  // block Diagonal: Add lambda*I to these ones
505  typename hessian_traits_t::TSparseBlocksHessian_Ap::matrix_t sSii = itRowEntry->second.num;
506  for (size_t k=0;k<POSE_DIMS;k++)
507  sSii.coeffRef(k,k)+=lambda;
508  denseH.block<POSE_DIMS,POSE_DIMS>(POSE_DIMS*i,POSE_DIMS*i) = sSii;
509  }
510  else
511  {
512  denseH.block<POSE_DIMS,POSE_DIMS>(
513  POSE_DIMS*itRowEntry->first, // row index
514  POSE_DIMS*i) // col index
515  = itRowEntry->second.num;
516  }
517  }
518  }
519  DETAILED_PROFILING_LEAVE("opt.DenseFill")
520 
521  hessian_is_valid = true;
522 
523  // Dense cholesky:
524  // ----------------------------------
525  DETAILED_PROFILING_ENTER("opt.DenseChol")
526  denseChol = denseH.selfadjointView<Eigen::Upper>().llt();
527  DETAILED_PROFILING_LEAVE("opt.DenseChol")
528 
529  if (denseChol.info()!=Eigen::Success)
530  {
531  // not positive definite so increase lambda and try again
532  return false;
533  }
534 
535  denseChol_is_uptodate = true;
536  }
537 
538 
539  // backsubtitution gives us "DeltaEps" from Cholesky and "-grad":
540  //
541  // (J^tJ + lambda*I) DeltaEps = -grad
542  // ----------------------------------------------------------------
543  DETAILED_PROFILING_ENTER("opt.backsub")
544 
545  delta_eps.resize(nUnknowns_k2k*POSE_DIMS + nUnknowns_k2f*LM_DIMS );
546  delta_eps.tail(nUnknowns_k2f*LM_DIMS).setZero();
547 
548  delta_eps.head(nUnknowns_k2k*POSE_DIMS) = denseChol.solve( minus_grad.head(nUnknowns_k2k*POSE_DIMS) );
549 
550  DETAILED_PROFILING_LEAVE("opt.backsub")
551 
552  // If using the Schur complement, at this point we must now solve the secondary
553  // system for features:
554  // -----------------------------------------------------------------------------
555  // 2nd numeric part: Solve for increments in features ----------
556  DETAILED_PROFILING_ENTER("opt.schur_features")
557 
558  schur_compl.numeric_solve_for_features(
559  &delta_eps[0],
560  // Handle case of no unknown features:
561  nUnknowns_k2f!=0 ? &delta_eps[nUnknowns_k2k*POSE_DIMS] : NULL // minus gradient of the features part
562  );
563 
564  DETAILED_PROFILING_LEAVE("opt.schur_features")
565 
566  return true;
567  } // end solve()
568 
569  void realize_lambda_changed()
570  {
571  denseChol_is_uptodate = false;
572  }
573  void realize_relinearized()
574  {
575  DETAILED_PROFILING_ENTER("opt.schur_realize_HAp_changed")
576  // Update the starting value of HAp for Schur:
577  schur_compl.realize_HAp_changed();
578  DETAILED_PROFILING_LEAVE("opt.schur_realize_HAp_changed")
579  }
580  bool was_ith_feature_invertible(const size_t i)
581  {
582  return schur_compl.was_ith_feature_invertible(i);
583  }
584  /** Here, out_info is of type mrpt::srba::options::solver_LM_schur_dense_cholesky::extra_results_t */
585  void get_extra_results(typename RBA_ENGINE::rba_options_type::solver_t::extra_results_t & out_info )
586  {
587  out_info.hessian_valid = hessian_is_valid;
588  denseH.swap(out_info.hessian);
589  }
590  };
591 
592 } // end namespace "internal"
593 
594 } } // end namespaces
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::~solver_engine
~solver_engine()
Definition: lev-marq_solvers.h:273
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::nUnknowns_scalars
const size_t nUnknowns_scalars
Definition: lev-marq_solvers.h:46
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::HApf
hessian_traits_t::TSparseBlocksHessian_Apf & HApf
Definition: lev-marq_solvers.h:44
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::realize_lambda_changed
void realize_lambda_changed()
Definition: lev-marq_solvers.h:569
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::delta_eps
Eigen::VectorXd delta_eps
The result of solving Ax=b will be stored here.
Definition: lev-marq_solvers.h:422
DETAILED_PROFILING_ENTER
#define DETAILED_PROFILING_ENTER(_STR)
Definition: optimize_edges.h:29
CSparseMatrix.h
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::realize_relinearized
void realize_relinearized()
Definition: lev-marq_solvers.h:385
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::hessian_traits_t
RBA_ENGINE::hessian_traits_t hessian_traits_t
Definition: lev-marq_solvers.h:34
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::was_ith_feature_invertible
bool was_ith_feature_invertible(const size_t i)
Definition: lev-marq_solvers.h:201
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::solve
bool solve(const double lambda)
Definition: lev-marq_solvers.h:283
mrpt::math::CSparseMatrix::CholeskyDecomp
Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
Definition: CSparseMatrix.h:360
mrpt::math::CSparseMatrix
A sparse matrix structure, wrapping T.
Definition: CSparseMatrix.h:92
std
STL namespace.
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::was_ith_feature_invertible
bool was_ith_feature_invertible(const size_t i)
Definition: lev-marq_solvers.h:396
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::sS_is_valid
bool sS_is_valid
Whether the Hessian was filled in, in sS.
Definition: lev-marq_solvers.h:236
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::realize_relinearized
void realize_relinearized()
Definition: lev-marq_solvers.h:573
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::hessian_is_valid
bool hessian_is_valid
Whether the hessian was correctly evaluated at least once.
Definition: lev-marq_solvers.h:440
mrpt::math::CSparseMatrix::swap
void swap(CSparseMatrix &other)
Fast swap contents with another sparse matrix.
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::HApf
hessian_traits_t::TSparseBlocksHessian_Apf & HApf
Definition: lev-marq_solvers.h:425
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::get_extra_results
void get_extra_results(typename RBA_ENGINE::rba_options_type::solver_t::extra_results_t &out_info)
Here, out_info is of type mrpt::srba::options::solver_LM_schur_sparse_cholesky::extra_results_t.
Definition: lev-marq_solvers.h:208
mrpt::math::CSparseMatrix::clear
void clear(const size_t nRows=1, const size_t nCols=1)
Erase all previous contents and leave the matrix as a "triplet" ROWS x COLS matrix without any nonzer...
mrpt::math::CExceptionNotDefPos
Used in mrpt::math::CSparseMatrix.
Definition: CSparseMatrix.h:38
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::realize_lambda_changed
void realize_lambda_changed()
Definition: lev-marq_solvers.h:392
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::nUnknowns_k2k
const size_t nUnknowns_k2k
Definition: lev-marq_solvers.h:228
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArray.h:18
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::solver_engine
solver_engine(const int verbose_level, mrpt::utils::CTimeLogger &profiler, typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp_, typename hessian_traits_t::TSparseBlocksHessian_f &Hf_, typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf_, Eigen::VectorXd &minus_grad_, const size_t nUnknowns_k2k_, const size_t nUnknowns_k2f_)
Constructor.
Definition: lev-marq_solvers.h:444
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::sS_is_valid
bool sS_is_valid
Whether the Hessian was filled in, in sS.
Definition: lev-marq_solvers.h:49
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::hessian_traits_t
RBA_ENGINE::hessian_traits_t hessian_traits_t
Definition: lev-marq_solvers.h:220
mrpt::srba::internal::SparseCholeskyDecompPtr
std::auto_ptr< CSparseMatrix::CholeskyDecomp > SparseCholeskyDecompPtr
Definition: lev-marq_solvers.h:26
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::Hf
hessian_traits_t::TSparseBlocksHessian_f & Hf
Definition: lev-marq_solvers.h:424
mrpt::math::CSparseMatrix::compressFromTriplet
void compressFromTriplet()
ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: CColouredPointsMap.h:289
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::minus_grad
Eigen::VectorXd & minus_grad
Definition: lev-marq_solvers.h:233
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::get_extra_results
void get_extra_results(typename RBA_ENGINE::rba_options_type::solver_t::extra_results_t &out_info)
Here, out_info is of type mrpt::srba::options::solver_LM_no_schur_sparse_cholesky::extra_results_t.
Definition: lev-marq_solvers.h:401
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::ptrCh
SparseCholeskyDecompPtr ptrCh
Cholesky object, as a pointer to reuse it between iterations.
Definition: lev-marq_solvers.h:51
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::realize_relinearized
void realize_relinearized()
Definition: lev-marq_solvers.h:193
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::m_verbose_level
const int m_verbose_level
Definition: lev-marq_solvers.h:225
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::minus_grad
Eigen::VectorXd & minus_grad
Definition: lev-marq_solvers.h:426
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::m_verbose_level
const int m_verbose_level
Definition: lev-marq_solvers.h:418
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::schur_compl
SchurComplement< typename hessian_traits_t::TSparseBlocksHessian_Ap, typename hessian_traits_t::TSparseBlocksHessian_f, typename hessian_traits_t::TSparseBlocksHessian_Apf > schur_compl
Definition: lev-marq_solvers.h:245
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::solve
bool solve(const double lambda)
Definition: lev-marq_solvers.h:84
DETAILED_PROFILING_LEAVE
#define DETAILED_PROFILING_LEAVE(_STR)
Definition: optimize_edges.h:30
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::was_ith_feature_invertible
bool was_ith_feature_invertible(const size_t i)
Definition: lev-marq_solvers.h:580
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::get_extra_results
void get_extra_results(typename RBA_ENGINE::rba_options_type::solver_t::extra_results_t &out_info)
Here, out_info is of type mrpt::srba::options::solver_LM_schur_dense_cholesky::extra_results_t.
Definition: lev-marq_solvers.h:585
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::solve
bool solve(const double lambda)
Definition: lev-marq_solvers.h:475
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::nUnknowns_k2k
const size_t nUnknowns_k2k
Definition: lev-marq_solvers.h:420
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::minus_grad
Eigen::VectorXd & minus_grad
Definition: lev-marq_solvers.h:45
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::m_profiler
mrpt::utils::CTimeLogger & m_profiler
Definition: lev-marq_solvers.h:226
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::m_profiler
mrpt::utils::CTimeLogger & m_profiler
Definition: lev-marq_solvers.h:40
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::~solver_engine
~solver_engine()
Definition: lev-marq_solvers.h:75
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::m_profiler
mrpt::utils::CTimeLogger & m_profiler
Definition: lev-marq_solvers.h:419
mrpt::srba::internal::solver_engine
Generic solver declaration.
Definition: optimize_edges.h:37
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::HApf
hessian_traits_t::TSparseBlocksHessian_Apf & HApf
Definition: lev-marq_solvers.h:232
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::sS
CSparseMatrix * sS
Sparse Hessian.
Definition: lev-marq_solvers.h:48
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::Hf
hessian_traits_t::TSparseBlocksHessian_f & Hf
Definition: lev-marq_solvers.h:43
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::schur_compl
SchurComplement< typename hessian_traits_t::TSparseBlocksHessian_Ap, typename hessian_traits_t::TSparseBlocksHessian_f, typename hessian_traits_t::TSparseBlocksHessian_Apf > schur_compl
Definition: lev-marq_solvers.h:433
VERBOSE_LEVEL
#define VERBOSE_LEVEL(_LEVEL)
Definition: RbaEngine.h:25
mrpt::math::CSparseMatrix::insert_submatrix
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 ...
Definition: CSparseMatrix.h:263
mrpt::srba::SchurComplement
A generic symbolic and numeric Schur-complement handler for builing reduced systems of equations...
Definition: schur.h:17
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::denseChol_is_uptodate
bool denseChol_is_uptodate
Definition: lev-marq_solvers.h:439
mrpt::utils::CTimeLogger
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:35
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::ptrCh
SparseCholeskyDecompPtr ptrCh
Cholesky object, as a pointer to reuse it between iterations.
Definition: lev-marq_solvers.h:238
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::solver_engine
solver_engine(const int verbose_level, mrpt::utils::CTimeLogger &profiler, typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp_, typename hessian_traits_t::TSparseBlocksHessian_f &Hf_, typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf_, Eigen::VectorXd &minus_grad_, const size_t nUnknowns_k2k_, const size_t nUnknowns_k2f_)
Constructor.
Definition: lev-marq_solvers.h:54
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::solver_engine
solver_engine(const int verbose_level, mrpt::utils::CTimeLogger &profiler, typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp_, typename hessian_traits_t::TSparseBlocksHessian_f &Hf_, typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf_, Eigen::VectorXd &minus_grad_, const size_t nUnknowns_k2k_, const size_t nUnknowns_k2f_)
Constructor.
Definition: lev-marq_solvers.h:248
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::sS
CSparseMatrix * sS
Sparse Hessian.
Definition: lev-marq_solvers.h:235
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::delta_eps
Eigen::VectorXd delta_eps
Definition: lev-marq_solvers.h:229
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::realize_lambda_changed
void realize_lambda_changed()
Definition: lev-marq_solvers.h:197
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::HAp
hessian_traits_t::TSparseBlocksHessian_Ap & HAp
Definition: lev-marq_solvers.h:230
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::denseChol
Eigen::LLT< Eigen::MatrixXd, Eigen::Upper > denseChol
Definition: lev-marq_solvers.h:438
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::HAp
hessian_traits_t::TSparseBlocksHessian_Ap & HAp
Definition: lev-marq_solvers.h:423
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::denseH
Eigen::MatrixXd denseH
Definition: lev-marq_solvers.h:437
mrpt::srba::internal::solver_engine< true, true, RBA_ENGINE >::hessian_traits_t
RBA_ENGINE::hessian_traits_t hessian_traits_t
Definition: lev-marq_solvers.h:413
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::delta_eps
Eigen::VectorXd delta_eps
The result of solving Ax=b will be stored here.
Definition: lev-marq_solvers.h:41
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::m_verbose_level
const int m_verbose_level
Definition: lev-marq_solvers.h:39
mrpt::srba::internal::solver_engine< true, false, RBA_ENGINE >::Hf
hessian_traits_t::TSparseBlocksHessian_f & Hf
Definition: lev-marq_solvers.h:231
mrpt::srba::internal::solver_engine< false, false, RBA_ENGINE >::HAp
hessian_traits_t::TSparseBlocksHessian_Ap & HAp
Definition: lev-marq_solvers.h:42


Page generated by Doxygen 1.8.8 for MRPT 1.2.2 SVN:Unversioned directory at Tue Oct 14 02:14:08 UTC 2014