|
SRBA: Sparser Relative Bundle Adjustment
|
00001 /* +---------------------------------------------------------------------------+ 00002 | Mobile Robot Programming Toolkit (MRPT) | 00003 | http://www.mrpt.org/ | 00004 | | 00005 | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file | 00006 | See: http://www.mrpt.org/Authors - All rights reserved. | 00007 | Released under BSD License. See details in http://www.mrpt.org/License | 00008 +---------------------------------------------------------------------------+ */ 00009 00010 #pragma once 00011 00012 #include <mrpt/math/CSparseMatrix.h> 00013 #include <memory> // for auto_ptr, unique_ptr 00014 00015 namespace srba { 00016 00017 namespace internal 00018 { 00019 #if MRPT_HAS_CXX11 00020 typedef std::unique_ptr<mrpt::math::CSparseMatrix::CholeskyDecomp> SparseCholeskyDecompPtr; 00021 #else 00022 typedef std::auto_ptr<mrpt::math::CSparseMatrix::CholeskyDecomp> SparseCholeskyDecompPtr; 00023 #endif 00024 00025 // ------------------------------------------------------------------------------------------ 00027 template <class RBA_ENGINE> 00028 struct solver_engine<false /*Schur*/,false /*dense Chol*/,RBA_ENGINE> 00029 { 00030 typedef typename RBA_ENGINE::hessian_traits_t hessian_traits_t; 00031 00032 static const size_t POSE_DIMS = RBA_ENGINE::kf2kf_pose_t::REL_POSE_DIMS; 00033 static const size_t LM_DIMS = RBA_ENGINE::landmark_t::LM_DIMS; 00034 00035 const int m_verbose_level; 00036 mrpt::utils::CTimeLogger &m_profiler; 00037 Eigen::VectorXd delta_eps; 00038 typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp; 00039 typename hessian_traits_t::TSparseBlocksHessian_f &Hf; 00040 typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf; 00041 Eigen::VectorXd &minus_grad; 00042 const size_t nUnknowns_k2k, nUnknowns_k2f, nUnknowns_scalars,idx_start_f; 00043 00044 mrpt::math::CSparseMatrix* sS; 00045 bool sS_is_valid; 00046 00047 SparseCholeskyDecompPtr ptrCh; 00048 00050 solver_engine( 00051 const int verbose_level, 00052 mrpt::utils::CTimeLogger & profiler, 00053 typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp_, 00054 typename hessian_traits_t::TSparseBlocksHessian_f &Hf_, 00055 typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf_, 00056 Eigen::VectorXd &minus_grad_, 00057 const size_t nUnknowns_k2k_, 00058 const size_t nUnknowns_k2f_) : 00059 m_verbose_level(verbose_level), 00060 m_profiler(profiler), 00061 HAp(HAp_), Hf(Hf_), HApf(HApf_), 00062 minus_grad(minus_grad_), 00063 nUnknowns_k2k(nUnknowns_k2k_), 00064 nUnknowns_k2f(nUnknowns_k2f_), 00065 nUnknowns_scalars( POSE_DIMS*nUnknowns_k2k + LM_DIMS*nUnknowns_k2f ), 00066 idx_start_f(POSE_DIMS*nUnknowns_k2k), 00067 sS(NULL), sS_is_valid(false) 00068 { 00069 } 00070 00071 ~solver_engine() 00072 { 00073 if (sS) { delete sS; sS=NULL; } 00074 } 00075 00076 // ---------------------------------------------------------------------- 00077 // Solve the entire H Ax = -g system with H as a single sparse Hessian. 00078 // Return: true on success. false to retry with a different lambda (Lev-Marq algorithm is assumed) 00079 // ---------------------------------------------------------------------- 00080 bool solve(const double lambda) 00081 { 00082 DETAILED_PROFILING_ENTER("opt.SparseTripletFill") 00083 00084 if (!sS) sS = new mrpt::math::CSparseMatrix(nUnknowns_scalars,nUnknowns_scalars); 00085 else sS->clear(nUnknowns_scalars,nUnknowns_scalars); 00086 sS_is_valid=false; 00087 00088 // 1/3: Hp -------------------------------------- 00089 for (size_t i=0;i<nUnknowns_k2k;i++) 00090 { // Only upper-half triangle: 00091 const typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t & col_i = HAp.getCol(i); 00092 00093 for (typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t::const_iterator itRowEntry = col_i.begin();itRowEntry != col_i.end(); ++itRowEntry ) 00094 { 00095 if (itRowEntry->first==i) 00096 { 00097 // block Diagonal: Add lambda*I to these ones 00098 typename hessian_traits_t::TSparseBlocksHessian_Ap::matrix_t sSii = itRowEntry->second.num; 00099 for (size_t k=0;k<POSE_DIMS;k++) 00100 sSii.coeffRef(k,k)+=lambda; 00101 sS->insert_submatrix(POSE_DIMS*i,POSE_DIMS*i, sSii ); 00102 } 00103 else 00104 { 00105 sS->insert_submatrix( 00106 POSE_DIMS*itRowEntry->first, // row index 00107 POSE_DIMS*i, // col index 00108 itRowEntry->second.num ); 00109 } 00110 } 00111 } 00112 00113 // 2/3: HApf -------------------------------------- 00114 for (size_t i=0;i<nUnknowns_k2k;i++) 00115 { 00116 const typename hessian_traits_t::TSparseBlocksHessian_Apf::col_t & row_i = HApf.getCol(i); 00117 00118 for (typename hessian_traits_t::TSparseBlocksHessian_Apf::col_t::const_iterator itColEntry = row_i.begin();itColEntry != row_i.end(); ++itColEntry ) 00119 { 00120 sS->insert_submatrix( 00121 POSE_DIMS*i, // row index 00122 idx_start_f+LM_DIMS*itColEntry->first, // col index 00123 itColEntry->second.num ); 00124 } 00125 } 00126 00127 // 3/3: Hf -------------------------------------- 00128 for (size_t i=0;i<nUnknowns_k2f;i++) 00129 { // Only upper-half triangle: 00130 const typename hessian_traits_t::TSparseBlocksHessian_f::col_t & col_i = Hf.getCol(i); 00131 00132 for (typename hessian_traits_t::TSparseBlocksHessian_f::col_t::const_iterator itRowEntry = col_i.begin();itRowEntry != col_i.end(); ++itRowEntry ) 00133 { 00134 if (itRowEntry->first==i) 00135 { 00136 // block Diagonal: Add lambda*I to these ones 00137 typename hessian_traits_t::TSparseBlocksHessian_f::matrix_t sSii = itRowEntry->second.num; 00138 for (size_t k=0;k<LM_DIMS;k++) 00139 sSii.coeffRef(k,k)+=lambda; 00140 sS->insert_submatrix(idx_start_f+LM_DIMS*i,idx_start_f+LM_DIMS*i, sSii ); 00141 } 00142 else 00143 { 00144 sS->insert_submatrix( 00145 idx_start_f+LM_DIMS*itRowEntry->first, // row index 00146 idx_start_f+LM_DIMS*i, // col index 00147 itRowEntry->second.num ); 00148 } 00149 } 00150 } 00151 DETAILED_PROFILING_LEAVE("opt.SparseTripletFill") 00152 00153 // Compress the sparse matrix: 00154 // -------------------------------------- 00155 DETAILED_PROFILING_ENTER("opt.SparseTripletCompress") 00156 sS->compressFromTriplet(); 00157 DETAILED_PROFILING_LEAVE("opt.SparseTripletCompress") 00158 00159 // Sparse cholesky 00160 // -------------------------------------- 00161 DETAILED_PROFILING_ENTER("opt.SparseChol") 00162 try 00163 { 00164 if (!ptrCh.get()) 00165 ptrCh = SparseCholeskyDecompPtr(new mrpt::math::CSparseMatrix::CholeskyDecomp(*sS) ); 00166 else ptrCh.get()->update(*sS); 00167 00168 sS_is_valid=true; 00169 00170 DETAILED_PROFILING_LEAVE("opt.SparseChol") 00171 } 00172 catch (mrpt::math::CExceptionNotDefPos &) 00173 { 00174 DETAILED_PROFILING_LEAVE("opt.SparseChol") 00175 return false; 00176 } 00177 00178 // backsubtitution gives us "DeltaEps" from Cholesky and "-grad": 00179 // 00180 // (J^tJ + lambda*I) DeltaEps = -grad 00181 // ---------------------------------------------------------------- 00182 DETAILED_PROFILING_ENTER("opt.backsub") 00183 ptrCh->backsub(minus_grad,delta_eps); 00184 DETAILED_PROFILING_LEAVE("opt.backsub") 00185 00186 return true; // solved OK. 00187 } 00188 00189 void realize_relinearized() 00190 { 00191 // Nothing to do. 00192 } 00193 void realize_lambda_changed() 00194 { 00195 // Nothing to do. 00196 } 00197 bool was_ith_feature_invertible(const size_t i) 00198 { 00199 MRPT_UNUSED_PARAM(i); 00200 return true; 00201 } 00202 00204 void get_extra_results(typename RBA_ENGINE::rba_options_t::solver_t::extra_results_t & out_info ) 00205 { 00206 out_info.hessian_valid = sS_is_valid; 00207 if (sS) sS->swap(out_info.hessian); 00208 } 00209 }; 00210 00211 // ------------------------------------------------------------------------------------------ 00213 template <class RBA_ENGINE> 00214 struct solver_engine<true /*Schur*/,false /*dense Chol*/,RBA_ENGINE> 00215 { 00216 typedef typename RBA_ENGINE::hessian_traits_t hessian_traits_t; 00217 00218 static const size_t POSE_DIMS = RBA_ENGINE::kf2kf_pose_t::REL_POSE_DIMS; 00219 static const size_t LM_DIMS = RBA_ENGINE::landmark_t::LM_DIMS; 00220 00221 const int m_verbose_level; 00222 mrpt::utils::CTimeLogger &m_profiler; 00223 00224 const size_t nUnknowns_k2k, nUnknowns_k2f; 00225 Eigen::VectorXd delta_eps; 00226 typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp; 00227 typename hessian_traits_t::TSparseBlocksHessian_f &Hf; 00228 typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf; 00229 Eigen::VectorXd &minus_grad; 00230 00231 mrpt::math::CSparseMatrix* sS; 00232 bool sS_is_valid; 00233 00234 SparseCholeskyDecompPtr ptrCh; 00235 00236 SchurComplement< 00237 typename hessian_traits_t::TSparseBlocksHessian_Ap, 00238 typename hessian_traits_t::TSparseBlocksHessian_f, 00239 typename hessian_traits_t::TSparseBlocksHessian_Apf 00240 > 00241 schur_compl; 00242 00244 solver_engine( 00245 const int verbose_level, 00246 mrpt::utils::CTimeLogger & profiler, 00247 typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp_, 00248 typename hessian_traits_t::TSparseBlocksHessian_f &Hf_, 00249 typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf_, 00250 Eigen::VectorXd &minus_grad_, 00251 const size_t nUnknowns_k2k_, 00252 const size_t nUnknowns_k2f_) : 00253 m_verbose_level(verbose_level), 00254 m_profiler(profiler), 00255 nUnknowns_k2k(nUnknowns_k2k_), 00256 nUnknowns_k2f(nUnknowns_k2f_), 00257 HAp(HAp_),Hf(Hf_),HApf(HApf_), 00258 minus_grad(minus_grad_), 00259 sS(NULL), sS_is_valid(false), 00260 schur_compl( 00261 HAp_,Hf_,HApf_, // The different symbolic/numeric Hessians 00262 &minus_grad[0], // minus gradient of the Ap part 00263 // Handle case of no unknown features: 00264 nUnknowns_k2f!=0 ? &minus_grad[POSE_DIMS*nUnknowns_k2k] : NULL // minus gradient of the features part 00265 ) 00266 { 00267 } 00268 00269 ~solver_engine() 00270 { 00271 if (sS) { delete sS; sS=NULL; } 00272 } 00273 00274 // ---------------------------------------------------------------------- 00275 // Solve the H*Ax = -g system using the Schur complement to generate a 00276 // Ap-only reduced system. 00277 // Return: always true (success). 00278 // ---------------------------------------------------------------------- 00279 bool solve(const double lambda) 00280 { 00281 // 1st: Numeric part: Update HAp hessian into the reduced system 00282 // Note: We have to re-evaluate the entire reduced Hessian HAp even if 00283 // only lambda changed, because of the terms inv(Hf+\lambda*I). 00284 00285 DETAILED_PROFILING_ENTER("opt.schur_build_reduced") 00286 schur_compl.numeric_build_reduced_system(lambda); 00287 DETAILED_PROFILING_LEAVE("opt.schur_build_reduced") 00288 00289 if (schur_compl.getNumFeaturesFullRank()!=schur_compl.getNumFeatures()) 00290 { 00291 VERBOSE_LEVEL_COLOR(1,mrpt::system::CONCOL_RED) << "[OPT] Schur warning: only " << schur_compl.getNumFeaturesFullRank() << " out of " << schur_compl.getNumFeatures() << " features have full-rank.\n"; 00292 VERBOSE_LEVEL_COLOR_POST(); 00293 } 00294 00295 // Only the H_Ap part of the Hessian: 00296 if (!sS) sS = new mrpt::math::CSparseMatrix(nUnknowns_k2k*POSE_DIMS,nUnknowns_k2k*POSE_DIMS); 00297 else sS->clear(nUnknowns_k2k*POSE_DIMS,nUnknowns_k2k*POSE_DIMS); 00298 sS_is_valid=false; 00299 00300 00301 // Now write the updated "HAp" into its sparse matrix form: 00302 DETAILED_PROFILING_ENTER("opt.SparseTripletFill") 00303 for (size_t i=0;i<nUnknowns_k2k;i++) 00304 { // Only upper-half triangle: 00305 const typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t & col_i = HAp.getCol(i); 00306 00307 for (typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t::const_iterator itRowEntry = col_i.begin();itRowEntry != col_i.end(); ++itRowEntry ) 00308 { 00309 if (itRowEntry->first==i) 00310 { 00311 // block Diagonal: Add lambda*I to these ones 00312 typename hessian_traits_t::TSparseBlocksHessian_Ap::matrix_t sSii = itRowEntry->second.num; 00313 for (size_t k=0;k<POSE_DIMS;k++) 00314 sSii.coeffRef(k,k)+=lambda; 00315 sS->insert_submatrix(POSE_DIMS*i,POSE_DIMS*i, sSii ); 00316 } 00317 else 00318 { 00319 sS->insert_submatrix( 00320 POSE_DIMS*itRowEntry->first, // row index 00321 POSE_DIMS*i, // col index 00322 itRowEntry->second.num ); 00323 } 00324 } 00325 } 00326 DETAILED_PROFILING_LEAVE("opt.SparseTripletFill") 00327 00328 // Compress the sparse matrix: 00329 // ---------------------------------- 00330 DETAILED_PROFILING_ENTER("opt.SparseTripletCompress") 00331 sS->compressFromTriplet(); 00332 DETAILED_PROFILING_LEAVE("opt.SparseTripletCompress") 00333 00334 // Sparse cholesky: 00335 // ---------------------------------- 00336 DETAILED_PROFILING_ENTER("opt.SparseChol") 00337 try 00338 { 00339 if (!ptrCh.get()) 00340 ptrCh = SparseCholeskyDecompPtr(new mrpt::math::CSparseMatrix::CholeskyDecomp(*sS) ); 00341 else ptrCh.get()->update(*sS); 00342 00343 sS_is_valid=true; 00344 00345 DETAILED_PROFILING_LEAVE("opt.SparseChol") 00346 } 00347 catch (mrpt::math::CExceptionNotDefPos &) 00348 { 00349 DETAILED_PROFILING_LEAVE("opt.SparseChol") 00350 // not positive definite so increase lambda and try again 00351 return false; 00352 } 00353 00354 // backsubtitution gives us "DeltaEps" from Cholesky and "-grad": 00355 // 00356 // (J^tJ + lambda*I) DeltaEps = -grad 00357 // ---------------------------------------------------------------- 00358 DETAILED_PROFILING_ENTER("opt.backsub") 00359 00360 delta_eps.assign(nUnknowns_k2k*POSE_DIMS + nUnknowns_k2f*LM_DIMS, 0); // Important: It's initialized to zeros! 00361 00362 ptrCh->backsub(&minus_grad[0],&delta_eps[0],nUnknowns_k2k*POSE_DIMS); 00363 00364 DETAILED_PROFILING_LEAVE("opt.backsub") 00365 00366 // If using the Schur complement, at this point we must now solve the secondary 00367 // system for features: 00368 // ----------------------------------------------------------------------------- 00369 // 2nd numeric part: Solve for increments in features ---------- 00370 DETAILED_PROFILING_ENTER("opt.schur_features") 00371 00372 schur_compl.numeric_solve_for_features( 00373 &delta_eps[0], 00374 // Handle case of no unknown features: 00375 nUnknowns_k2f!=0 ? &delta_eps[nUnknowns_k2k*POSE_DIMS] : NULL // minus gradient of the features part 00376 ); 00377 00378 DETAILED_PROFILING_LEAVE("opt.schur_features") 00379 00380 return true; 00381 } // end solve() 00382 00383 00384 void realize_relinearized() 00385 { 00386 DETAILED_PROFILING_ENTER("opt.schur_realize_HAp_changed") 00387 // Update the starting value of HAp for Schur: 00388 schur_compl.realize_HAp_changed(); 00389 DETAILED_PROFILING_LEAVE("opt.schur_realize_HAp_changed") 00390 } 00391 void realize_lambda_changed() 00392 { 00393 // Nothing to do. 00394 } 00395 bool was_ith_feature_invertible(const size_t i) 00396 { 00397 return schur_compl.was_ith_feature_invertible(i); 00398 } 00400 void get_extra_results(typename RBA_ENGINE::rba_options_t::solver_t::extra_results_t & out_info ) 00401 { 00402 out_info.hessian_valid = sS_is_valid; 00403 if (sS) sS->swap(out_info.hessian); 00404 } 00405 }; 00406 00407 // ------------------------------------------------------------------------------------------ 00409 template <class RBA_ENGINE> 00410 struct solver_engine<true /*Schur*/,true /*dense Chol*/,RBA_ENGINE> 00411 { 00412 typedef typename RBA_ENGINE::hessian_traits_t hessian_traits_t; 00413 00414 static const size_t POSE_DIMS = RBA_ENGINE::kf2kf_pose_t::REL_POSE_DIMS; 00415 static const size_t LM_DIMS = RBA_ENGINE::landmark_t::LM_DIMS; 00416 00417 const int m_verbose_level; 00418 mrpt::utils::CTimeLogger &m_profiler; 00419 const size_t nUnknowns_k2k, nUnknowns_k2f; 00420 00421 Eigen::VectorXd delta_eps; 00422 typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp; 00423 typename hessian_traits_t::TSparseBlocksHessian_f &Hf; 00424 typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf; 00425 Eigen::VectorXd &minus_grad; 00426 00427 SchurComplement< 00428 typename hessian_traits_t::TSparseBlocksHessian_Ap, 00429 typename hessian_traits_t::TSparseBlocksHessian_f, 00430 typename hessian_traits_t::TSparseBlocksHessian_Apf 00431 > 00432 schur_compl; 00433 00434 00435 // Data for dense cholesky: 00436 Eigen::MatrixXd denseH; 00437 Eigen::LLT<Eigen::MatrixXd,Eigen::Upper> denseChol; 00438 bool denseChol_is_uptodate; 00439 bool hessian_is_valid; 00440 00441 00443 solver_engine( 00444 const int verbose_level, 00445 mrpt::utils::CTimeLogger & profiler, 00446 typename hessian_traits_t::TSparseBlocksHessian_Ap &HAp_, 00447 typename hessian_traits_t::TSparseBlocksHessian_f &Hf_, 00448 typename hessian_traits_t::TSparseBlocksHessian_Apf &HApf_, 00449 Eigen::VectorXd &minus_grad_, 00450 const size_t nUnknowns_k2k_, 00451 const size_t nUnknowns_k2f_) : 00452 m_verbose_level(verbose_level), 00453 m_profiler(profiler), 00454 nUnknowns_k2k(nUnknowns_k2k_), 00455 nUnknowns_k2f(nUnknowns_k2f_), 00456 HAp(HAp_),Hf(Hf_),HApf(HApf_), 00457 minus_grad(minus_grad_), 00458 schur_compl( 00459 HAp_,Hf_,HApf_, // The different symbolic/numeric Hessians 00460 &minus_grad[0], // minus gradient of the Ap part 00461 // Handle case of no unknown features: 00462 nUnknowns_k2f!=0 ? &minus_grad[POSE_DIMS*nUnknowns_k2k] : NULL // minus gradient of the features part 00463 ), 00464 denseChol_is_uptodate (false), 00465 hessian_is_valid (false) 00466 { 00467 } 00468 00469 // ---------------------------------------------------------------------- 00470 // Solve the H*Ax = -g system using the Schur complement to generate a 00471 // Ap-only reduced system. 00472 // Return: always true (success). 00473 // ---------------------------------------------------------------------- 00474 bool solve(const double lambda) 00475 { 00476 // 1st: Numeric part: Update HAp hessian into the reduced system 00477 // Note: We have to re-evaluate the entire reduced Hessian HAp even if 00478 // only lambda changed, because of the terms inv(Hf+\lambda*I). 00479 00480 DETAILED_PROFILING_ENTER("opt.schur_build_reduced") 00481 schur_compl.numeric_build_reduced_system(lambda); 00482 DETAILED_PROFILING_LEAVE("opt.schur_build_reduced") 00483 00484 if (schur_compl.getNumFeaturesFullRank()!=schur_compl.getNumFeatures()) { 00485 VERBOSE_LEVEL_COLOR(1,mrpt::system::CONCOL_RED) << "[OPT] Schur warning: only " << schur_compl.getNumFeaturesFullRank() << " out of " << schur_compl.getNumFeatures() << " features have full-rank.\n"; 00486 VERBOSE_LEVEL_COLOR_POST(); 00487 } 00488 00489 if (!denseChol_is_uptodate) 00490 { 00491 // Use a dense Cholesky method for solving the set of unknowns: 00492 denseH.setZero(nUnknowns_k2k*POSE_DIMS,nUnknowns_k2k*POSE_DIMS); // Only for the H_Ap part of the Hessian 00493 hessian_is_valid = false; 00494 00495 // Now write the updated "HAp" into its sparse matrix form: 00496 DETAILED_PROFILING_ENTER("opt.DenseFill") 00497 for (size_t i=0;i<nUnknowns_k2k;i++) 00498 { // Only upper-half triangle: 00499 const typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t & col_i = HAp.getCol(i); 00500 00501 for (typename hessian_traits_t::TSparseBlocksHessian_Ap::col_t::const_iterator itRowEntry = col_i.begin();itRowEntry != col_i.end(); ++itRowEntry ) 00502 { 00503 if (itRowEntry->first==i) 00504 { 00505 // block Diagonal: Add lambda*I to these ones 00506 typename hessian_traits_t::TSparseBlocksHessian_Ap::matrix_t sSii = itRowEntry->second.num; 00507 for (size_t k=0;k<POSE_DIMS;k++) 00508 sSii.coeffRef(k,k)+=lambda; 00509 denseH.block<POSE_DIMS,POSE_DIMS>(POSE_DIMS*i,POSE_DIMS*i) = sSii; 00510 } 00511 else 00512 { 00513 denseH.block<POSE_DIMS,POSE_DIMS>( 00514 POSE_DIMS*itRowEntry->first, // row index 00515 POSE_DIMS*i) // col index 00516 = itRowEntry->second.num; 00517 } 00518 } 00519 } 00520 DETAILED_PROFILING_LEAVE("opt.DenseFill") 00521 00522 hessian_is_valid = true; 00523 00524 // Dense cholesky: 00525 // ---------------------------------- 00526 DETAILED_PROFILING_ENTER("opt.DenseChol") 00527 denseChol = denseH.selfadjointView<Eigen::Upper>().llt(); 00528 DETAILED_PROFILING_LEAVE("opt.DenseChol") 00529 00530 if (denseChol.info()!=Eigen::Success) 00531 { 00532 // not positive definite so increase lambda and try again 00533 return false; 00534 } 00535 00536 denseChol_is_uptodate = true; 00537 } 00538 00539 00540 // backsubtitution gives us "DeltaEps" from Cholesky and "-grad": 00541 // 00542 // (J^tJ + lambda*I) DeltaEps = -grad 00543 // ---------------------------------------------------------------- 00544 DETAILED_PROFILING_ENTER("opt.backsub") 00545 00546 delta_eps.resize(nUnknowns_k2k*POSE_DIMS + nUnknowns_k2f*LM_DIMS ); 00547 delta_eps.tail(nUnknowns_k2f*LM_DIMS).setZero(); 00548 00549 delta_eps.head(nUnknowns_k2k*POSE_DIMS) = denseChol.solve( minus_grad.head(nUnknowns_k2k*POSE_DIMS) ); 00550 00551 DETAILED_PROFILING_LEAVE("opt.backsub") 00552 00553 // If using the Schur complement, at this point we must now solve the secondary 00554 // system for features: 00555 // ----------------------------------------------------------------------------- 00556 // 2nd numeric part: Solve for increments in features ---------- 00557 DETAILED_PROFILING_ENTER("opt.schur_features") 00558 00559 schur_compl.numeric_solve_for_features( 00560 &delta_eps[0], 00561 // Handle case of no unknown features: 00562 nUnknowns_k2f!=0 ? &delta_eps[nUnknowns_k2k*POSE_DIMS] : NULL // minus gradient of the features part 00563 ); 00564 00565 DETAILED_PROFILING_LEAVE("opt.schur_features") 00566 00567 return true; 00568 } // end solve() 00569 00570 void realize_lambda_changed() 00571 { 00572 denseChol_is_uptodate = false; 00573 } 00574 void realize_relinearized() 00575 { 00576 DETAILED_PROFILING_ENTER("opt.schur_realize_HAp_changed") 00577 // Update the starting value of HAp for Schur: 00578 schur_compl.realize_HAp_changed(); 00579 DETAILED_PROFILING_LEAVE("opt.schur_realize_HAp_changed") 00580 } 00581 bool was_ith_feature_invertible(const size_t i) 00582 { 00583 return schur_compl.was_ith_feature_invertible(i); 00584 } 00586 void get_extra_results(typename RBA_ENGINE::rba_options_t::solver_t::extra_results_t & out_info ) 00587 { 00588 out_info.hessian_valid = hessian_is_valid; 00589 denseH.swap(out_info.hessian); 00590 } 00591 }; 00592 00593 } // end namespace "internal" 00594 00595 } // End of namespaces
1.7.6.1
