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


Page generated by Doxygen 1.8.9.1 for MRPT 1.3.0 SVN: at Sun Sep 13 03:55:12 UTC 2015