|
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/lightweight_geom_data.h> 00013 #include <mrpt/math/MatrixBlockSparseCols.h> 00014 #include <mrpt/math/CArrayNumeric.h> 00015 #include <mrpt/utils/TEnumType.h> 00016 #include <mrpt/system/memory.h> // for MRPT_MAKE_ALIGNED_OPERATOR_NEW 00017 #include <set> 00018 00019 namespace srba 00020 { 00021 typedef uint64_t TKeyFrameID; 00022 typedef uint64_t TLandmarkID; 00023 typedef uint64_t topo_dist_t; 00024 typedef std::pair<TKeyFrameID,TKeyFrameID> TPairKeyFrameID; 00025 typedef std::multimap<size_t,TKeyFrameID,std::greater<size_t> > base_sorted_lst_t; 00026 00027 #define VERBOSE_LEVEL(_LEVEL) if (m_verbose_level>=_LEVEL) std::cout 00028 00029 #define VERBOSE_LEVEL_COLOR(_LEVEL,_TEXT_COLOR) if (m_verbose_level>=_LEVEL) { mrpt::system::setConsoleColor(_TEXT_COLOR); std::cout 00030 #define VERBOSE_LEVEL_COLOR_POST() mrpt::system::setConsoleColor(mrpt::system::CONCOL_NORMAL); } 00031 00032 00033 #define SRBA_INVALID_KEYFRAMEID static_cast<TKeyFrameID>(-1) 00034 #define SRBA_INVALID_INDEX static_cast<size_t>(-1) 00035 00036 // ------------------------------------------------------------------------------- 00043 template <class landmark_t,class obs_t> 00044 struct sensor_model; 00045 00049 template <class POSE_TRAITS> 00050 struct kf2kf_pose_traits: public POSE_TRAITS 00051 { 00052 typedef kf2kf_pose_traits<POSE_TRAITS> me_t; 00053 typedef typename POSE_TRAITS::pose_t pose_t; 00054 typedef typename mrpt::math::CArrayDouble<POSE_TRAITS::REL_POSE_DIMS> array_pose_t; 00055 00057 struct pose_flag_t 00058 { 00059 pose_t pose; 00060 mutable bool updated; 00061 00062 pose_flag_t() : updated(false) { } 00063 pose_flag_t(const pose_t &pose_, const bool updated_) : pose(pose_),updated(updated_) { } 00064 00065 inline void mark_outdated() const { updated=false; } 00066 00067 MRPT_MAKE_ALIGNED_OPERATOR_NEW // Needed because we have fixed-length Eigen matrices (within CPose3D) 00068 }; 00069 00071 typedef typename mrpt::aligned_containers<TKeyFrameID, pose_flag_t>::map_t frameid2pose_map_t; 00072 00073 // Use special map-like container with an underlying planar deque container, which avoid reallocations 00074 // and still provides map-like [] access at O(1). 00075 typedef mrpt::utils::map_as_vector< 00076 TKeyFrameID, 00077 frameid2pose_map_t, 00078 typename std::deque<std::pair<TKeyFrameID,frameid2pose_map_t> > 00079 > TRelativePosesForEachTarget; 00080 00082 struct k2k_edge_t 00083 { 00084 TKeyFrameID from; 00085 TKeyFrameID to; 00086 pose_t inv_pose; 00087 00088 size_t id; 00089 00090 MRPT_MAKE_ALIGNED_OPERATOR_NEW // Needed because we have fixed-length Eigen matrices (within CPose3D) 00091 }; 00092 00093 typedef std::deque<k2k_edge_t*> k2k_edge_vector_t; 00094 }; // end of kf2kf_pose_traits 00095 00099 template <class LM_TRAITS> 00100 struct landmark_traits: public LM_TRAITS 00101 { 00102 typedef landmark_traits<LM_TRAITS> me_t; 00103 typedef typename mrpt::math::CArrayDouble<LM_TRAITS::LM_DIMS> array_landmark_t; 00104 00106 struct TRelativeLandmarkPos 00107 { 00108 inline TRelativeLandmarkPos() { } 00112 template <typename LANDMARK_POS> 00113 inline TRelativeLandmarkPos(const TKeyFrameID _id_frame_base, const LANDMARK_POS &_pos) : id_frame_base(_id_frame_base) { 00114 for (size_t i=0;i<LM_TRAITS::LM_DIMS;i++) pos[i]=_pos[i]; 00115 } 00116 00117 TKeyFrameID id_frame_base; 00118 array_landmark_t pos; 00119 00120 MRPT_MAKE_ALIGNED_OPERATOR_NEW // Needed because we have fixed-length Eigen matrices ("array_landmark_t") 00121 }; 00122 00124 typedef std::map<TLandmarkID, TRelativeLandmarkPos> TRelativeLandmarkPosMap; 00125 00127 struct TLandmarkEntry 00128 { 00129 bool has_known_pos; 00130 TRelativeLandmarkPos *rfp; 00131 00132 TLandmarkEntry() : has_known_pos(true), rfp(NULL) {} 00133 TLandmarkEntry(bool has_known_pos_, TRelativeLandmarkPos *rfp_) : has_known_pos(has_known_pos_), rfp(rfp_) 00134 {} 00135 }; 00136 00137 }; // end of landmark_traits 00138 00139 template <class OBS_TRAITS> 00140 struct observation_traits : public OBS_TRAITS 00141 { 00142 typedef typename mrpt::math::CArrayDouble<OBS_TRAITS::OBS_DIMS> array_obs_t; 00143 typedef typename mrpt::math::CArrayDouble<OBS_TRAITS::OBS_DIMS> residual_t; 00144 00145 typedef typename mrpt::aligned_containers<residual_t>::vector_t vector_residuals_t; 00146 00148 struct observation_t 00149 { 00150 TLandmarkID feat_id; 00151 typename OBS_TRAITS::obs_data_t obs_data; 00152 }; 00153 00154 MRPT_MAKE_ALIGNED_OPERATOR_NEW 00155 00156 }; // end of "observation_traits" 00157 00159 // -------------------------------------------------------------- 00160 00164 enum TCovarianceRecoveryPolicy { 00166 crpNone = 0, 00168 crpLandmarksApprox 00169 }; 00170 00172 template <typename PAIR,typename V> 00173 V getTheOtherFromPair(const V one, const PAIR &p) { 00174 return p.first==one ? p.second : p.first; 00175 } 00176 00178 template <typename K2K_EDGE,typename V> 00179 V getTheOtherFromPair2(const V one, const K2K_EDGE &p) { 00180 return p.from==one ? p.to: p.from; 00181 } 00182 00184 struct TNewEdgeInfo 00185 { 00186 size_t id; 00187 bool has_approx_init_val; 00188 00192 TKeyFrameID loopclosure_observer_kf, loopclosure_base_kf; 00193 00194 TNewEdgeInfo() : 00195 id (SRBA_INVALID_INDEX), 00196 has_approx_init_val (false), 00197 loopclosure_observer_kf (SRBA_INVALID_KEYFRAMEID), 00198 loopclosure_base_kf (SRBA_INVALID_KEYFRAMEID) 00199 {} 00200 }; 00201 00204 template <class kf2kf_pose_t, class LANDMARK_TYPE> 00205 struct TJacobianSymbolicInfo_dh_dAp 00206 { 00207 typedef kf2kf_pose_traits<kf2kf_pose_t> kf2kf_traits_t; 00208 typedef landmark_traits<LANDMARK_TYPE> lm_traits_t; 00209 00212 const typename kf2kf_traits_t::pose_flag_t * rel_pose_d1_from_obs, * rel_pose_base_from_d1; 00213 00216 bool edge_normal_dir; 00217 00219 const typename lm_traits_t::TRelativeLandmarkPos * feat_rel_pos; 00220 00223 TKeyFrameID kf_d; 00224 TKeyFrameID kf_base; 00225 size_t k2k_edge_id; 00226 size_t obs_idx; 00227 char * is_valid; 00228 00229 TJacobianSymbolicInfo_dh_dAp() : 00230 rel_pose_d1_from_obs(NULL), 00231 rel_pose_base_from_d1(NULL), 00232 edge_normal_dir(true), 00233 feat_rel_pos(NULL), 00234 kf_d(SRBA_INVALID_KEYFRAMEID), 00235 kf_base(SRBA_INVALID_KEYFRAMEID), 00236 k2k_edge_id(SRBA_INVALID_INDEX), 00237 obs_idx(SRBA_INVALID_INDEX), 00238 is_valid(NULL) 00239 {} 00240 }; 00241 00242 00245 template <class kf2kf_pose_t, class LANDMARK_TYPE> 00246 struct TJacobianSymbolicInfo_dh_df 00247 { 00248 typedef kf2kf_pose_traits<kf2kf_pose_t> kf2kf_traits_t; 00249 typedef landmark_traits<LANDMARK_TYPE> lm_traits_t; 00250 00251 typename lm_traits_t::TRelativeLandmarkPos *feat_rel_pos; 00252 00255 const typename kf2kf_traits_t::pose_flag_t * rel_pose_base_from_obs; 00256 00257 size_t obs_idx; 00258 char * is_valid; 00259 00260 TJacobianSymbolicInfo_dh_df() : 00261 feat_rel_pos(NULL), 00262 rel_pose_base_from_obs(NULL), 00263 obs_idx(SRBA_INVALID_INDEX), 00264 is_valid(NULL) 00265 {} 00266 }; 00267 00274 template <typename Scalar,int N,int M1,int M2> 00275 struct THessianSymbolicInfo 00276 { 00277 typedef Eigen::Matrix<Scalar,N,M1> matrix1_t; 00278 typedef Eigen::Matrix<Scalar,N,M2> matrix2_t; 00279 00284 struct THessianSymbolicInfoEntry 00285 { 00286 const matrix1_t * J1; 00287 const matrix2_t * J2; 00288 const char * J1_valid; 00289 const char * J2_valid; 00290 size_t obs_idx; 00291 00292 THessianSymbolicInfoEntry(const matrix1_t * const J1_, const matrix2_t * const J2_, const char * const J1_valid_, const char * const J2_valid_, const size_t obs_idx_ ) : 00293 J1(J1_), J2(J2_),J1_valid(J1_valid_),J2_valid(J2_valid_),obs_idx(obs_idx_) 00294 { } 00295 00296 // Default ctor: should not be invoked under normal usage, but just in case: 00297 THessianSymbolicInfoEntry() : J1(NULL),J2(NULL),J1_valid(NULL),J2_valid(NULL),obs_idx(static_cast<size_t>(-1)) {} 00298 }; 00299 00300 typedef std::vector<THessianSymbolicInfoEntry> list_jacob_blocks_t; 00301 00302 list_jacob_blocks_t lst_jacob_blocks; 00303 }; 00304 00305 00311 template < 00312 typename Scalar, 00313 int NROWS, 00314 int NCOLS, 00315 typename INFO, 00316 bool HAS_REMAP, 00317 typename INDEX_REMAP_MAP_IMPL = mrpt::utils::map_as_vector<size_t,size_t> 00318 > 00319 struct SparseBlockMatrix : public mrpt::math::MatrixBlockSparseCols<Scalar,NROWS,NCOLS,INFO,HAS_REMAP,INDEX_REMAP_MAP_IMPL> 00320 { 00321 typedef mrpt::math::MatrixBlockSparseCols<Scalar,NROWS,NCOLS,INFO,HAS_REMAP,INDEX_REMAP_MAP_IMPL> base_t; 00322 00324 size_t findRowSpan(size_t *row_min_idx=NULL, size_t *row_max_idx=NULL) const 00325 { 00326 size_t row_min=std::numeric_limits<size_t>::max(); 00327 size_t row_max=0; 00328 const size_t nCols = base_t::getColCount(); 00329 for (size_t j=0;j<nCols;j++) 00330 for (typename base_t::col_t::const_iterator itRow=base_t::getCol(j).begin();itRow!=base_t::getCol(j).end();++itRow) { 00331 mrpt::utils::keep_max(row_max, itRow->first); 00332 mrpt::utils::keep_min(row_min, itRow->first); 00333 } 00334 if (row_min==std::numeric_limits<size_t>::max()) 00335 row_min=0; 00336 if (row_min_idx) *row_min_idx = row_min; 00337 if (row_max_idx) *row_max_idx = row_max; 00338 return (row_max-row_min)+1; 00339 } 00340 00342 static size_t findRowSpan(const std::vector<typename base_t::col_t*> &lstColumns, size_t *row_min_idx=NULL, size_t *row_max_idx=NULL) 00343 { 00344 size_t row_min=std::numeric_limits<size_t>::max(); 00345 size_t row_max=0; 00346 for (size_t i=0;i<lstColumns.size();++i) 00347 for (typename base_t::col_t::const_iterator itRow=lstColumns[i]->begin();itRow!=lstColumns[i]->end();++itRow) { 00348 mrpt::utils::keep_max(row_max, itRow->first); 00349 mrpt::utils::keep_min(row_min, itRow->first); 00350 } 00351 if (row_min==std::numeric_limits<size_t>::max()) 00352 row_min=0; 00353 if (row_min_idx) *row_min_idx = row_min; 00354 if (row_max_idx) *row_max_idx = row_max; 00355 return (row_max-row_min)+1; 00356 } 00357 00362 void getSparsityStats(size_t &nMaxBlocks, size_t &nNonZeroBlocks ) const { 00363 const size_t nCols = base_t::getColCount(); 00364 const size_t nRows = findRowSpan(); 00365 nMaxBlocks = nCols * nRows; 00366 nNonZeroBlocks = 0; 00367 for (size_t j=0;j<nCols;j++) 00368 nNonZeroBlocks+= base_t::getCol(j).size(); 00369 } 00370 00372 static void getSparsityStats(const std::vector<typename base_t::col_t*> &lstColumns, size_t &nMaxBlocks, size_t &nNonZeroBlocks ) 00373 { 00374 const size_t nCols = lstColumns.size(); 00375 const size_t nRows = findRowSpan(lstColumns); 00376 nMaxBlocks = nCols * nRows; 00377 nNonZeroBlocks = 0; 00378 for (size_t j=0;j<nCols;j++) 00379 nNonZeroBlocks+=lstColumns[j]->size(); 00380 } 00381 00382 }; // end SparseBlockMatrix() 00383 00384 00390 template <class kf2kf_pose_t, class LANDMARK_TYPE,class obs_t> 00391 struct jacobian_traits 00392 { 00393 static const size_t OBS_DIMS = obs_t::OBS_DIMS; 00394 static const size_t REL_POSE_DIMS = kf2kf_pose_t::REL_POSE_DIMS; 00395 static const size_t LM_DIMS = LANDMARK_TYPE::LM_DIMS; 00396 00397 typedef TJacobianSymbolicInfo_dh_dAp<kf2kf_pose_t,LANDMARK_TYPE> jacob_dh_dAp_info_t; 00398 typedef TJacobianSymbolicInfo_dh_df<kf2kf_pose_t,LANDMARK_TYPE> jacob_dh_df_info_t; 00399 00400 typedef SparseBlockMatrix<double,OBS_DIMS,REL_POSE_DIMS,jacob_dh_dAp_info_t, false> TSparseBlocksJacobians_dh_dAp; 00401 typedef SparseBlockMatrix<double,OBS_DIMS,LM_DIMS,jacob_dh_df_info_t, true > TSparseBlocksJacobians_dh_df; // The "true" is to "remap" indices 00402 }; 00403 00411 template <class kf2kf_pose_t, class LANDMARK_TYPE,class obs_t> 00412 struct hessian_traits 00413 { 00414 static const size_t OBS_DIMS = obs_t::OBS_DIMS; 00415 static const size_t REL_POSE_DIMS = kf2kf_pose_t::REL_POSE_DIMS; 00416 static const size_t LM_DIMS = LANDMARK_TYPE::LM_DIMS; 00417 00418 typedef THessianSymbolicInfo<double,OBS_DIMS,REL_POSE_DIMS,REL_POSE_DIMS> hessian_Ap_info_t; 00419 typedef THessianSymbolicInfo<double,OBS_DIMS,LM_DIMS,LM_DIMS> hessian_f_info_t; 00420 typedef THessianSymbolicInfo<double,OBS_DIMS,REL_POSE_DIMS,LM_DIMS> hessian_Apf_info_t; 00421 00422 // (the final "false" in all types is because we don't need remapping of indices in hessians) 00423 typedef SparseBlockMatrix<double,REL_POSE_DIMS , REL_POSE_DIMS , hessian_Ap_info_t , false> TSparseBlocksHessian_Ap; 00424 typedef SparseBlockMatrix<double,LM_DIMS , LM_DIMS , hessian_f_info_t , false> TSparseBlocksHessian_f; 00425 typedef SparseBlockMatrix<double,REL_POSE_DIMS , LM_DIMS , hessian_Apf_info_t, false> TSparseBlocksHessian_Apf; 00426 00428 typedef mrpt::utils::map_as_vector< 00429 TLandmarkID, 00430 typename TSparseBlocksHessian_f::matrix_t, 00431 typename mrpt::aligned_containers<std::pair<TLandmarkID,typename TSparseBlocksHessian_f::matrix_t > >::deque_t> landmarks2infmatrix_t; 00432 }; 00433 00434 00437 template <class kf2kf_pose_t,class landmark_t,class obs_t> 00438 struct rba_joint_parameterization_traits_t 00439 { 00440 typedef landmark_t original_landmark_t; 00441 typedef kf2kf_pose_t original_kf2kf_pose_t; 00442 00443 typedef kf2kf_pose_traits<kf2kf_pose_t> kf2kf_traits_t; 00444 typedef observation_traits<obs_t> obs_traits_t; 00445 typedef landmark_traits<landmark_t> lm_traits_t; 00446 00447 typedef typename kf2kf_traits_t::k2k_edge_t k2k_edge_t; 00448 00473 struct new_kf_observation_t 00474 { 00476 new_kf_observation_t() : is_fixed(false), is_unknown_with_init_val(false) { feat_rel_pos.setZero(); } 00477 00478 typename obs_traits_t::observation_t obs; 00479 00482 bool is_fixed; 00483 00487 bool is_unknown_with_init_val; 00488 00489 typename lm_traits_t::array_landmark_t feat_rel_pos; 00490 00492 template <class REL_POS> inline void setRelPos(const REL_POS &pos) { 00493 for (size_t i=0;i<landmark_t::LM_DIMS;i++) feat_rel_pos[i]=pos[i]; 00494 } 00495 }; 00496 00498 typedef std::deque<new_kf_observation_t> new_kf_observations_t; 00499 00501 struct kf_observation_t 00502 { 00503 inline kf_observation_t() {} 00504 inline kf_observation_t(const typename obs_traits_t::observation_t &obs_, const TKeyFrameID kf_id_) : obs(obs_), kf_id(kf_id_) {} 00505 00506 typename obs_traits_t::observation_t obs; 00507 typename obs_traits_t::array_obs_t obs_arr; 00508 TKeyFrameID kf_id; 00509 00510 MRPT_MAKE_ALIGNED_OPERATOR_NEW // This forces aligned mem allocation 00511 }; 00512 00513 00515 struct k2f_edge_t 00516 { 00517 kf_observation_t obs; 00518 bool feat_has_known_rel_pos; 00519 bool is_first_obs_of_unknown; 00520 typename lm_traits_t::TRelativeLandmarkPos *feat_rel_pos; 00521 00522 inline const TLandmarkID get_observed_feature_id() const { return obs.obs.feat_id; } 00523 00524 MRPT_MAKE_ALIGNED_OPERATOR_NEW // This forces aligned mem allocation 00525 }; 00526 00528 struct keyframe_info 00529 { 00530 std::deque<k2k_edge_t*> adjacent_k2k_edges; 00531 std::deque<k2f_edge_t*> adjacent_k2f_edges; 00532 }; 00533 00534 }; // end of "rba_joint_parameterization_traits_t" 00535 00536 00538 struct TSpanTreeEntry 00539 { 00540 TKeyFrameID next; 00541 topo_dist_t distance; 00542 }; 00543 00548 template <class kf2kf_pose_t,class landmark_t,class obs_t,class RBA_OPTIONS> 00549 struct TRBA_Problem_state 00550 { 00551 typedef typename kf2kf_pose_t::pose_t pose_t; 00552 typedef typename kf2kf_pose_traits<kf2kf_pose_t>::k2k_edge_t k2k_edge_t; 00553 typedef typename kf2kf_pose_traits<kf2kf_pose_t>::k2k_edge_vector_t k2k_edge_vector_t; 00554 typedef typename kf2kf_pose_traits<kf2kf_pose_t>::frameid2pose_map_t frameid2pose_map_t; 00555 typedef typename kf2kf_pose_traits<kf2kf_pose_t>::pose_flag_t pose_flag_t; 00556 typedef typename landmark_traits<landmark_t>::TRelativeLandmarkPosMap TRelativeLandmarkPosMap; 00557 typedef typename landmark_traits<landmark_t>::TLandmarkEntry TLandmarkEntry; 00558 typedef typename hessian_traits<kf2kf_pose_t,landmark_t,obs_t>::landmarks2infmatrix_t landmarks2infmatrix_t; 00559 typedef typename rba_joint_parameterization_traits_t<kf2kf_pose_t,landmark_t,obs_t>::keyframe_info keyframe_info; 00560 typedef typename rba_joint_parameterization_traits_t<kf2kf_pose_t,landmark_t,obs_t>::k2f_edge_t k2f_edge_t; 00561 typedef typename rba_joint_parameterization_traits_t<kf2kf_pose_t,landmark_t,obs_t>::new_kf_observations_t new_kf_observations_t; 00562 typedef typename rba_joint_parameterization_traits_t<kf2kf_pose_t,landmark_t,obs_t>::new_kf_observation_t new_kf_observation_t; 00563 00564 typedef typename mrpt::aligned_containers<k2k_edge_t>::deque_t k2k_edges_deque_t; // Note: A std::deque() does not invalidate pointers/references, as we always insert elements at the end; we'll exploit this... 00565 typedef typename mrpt::aligned_containers<k2f_edge_t>::deque_t all_observations_deque_t; 00566 00567 typedef std::deque<keyframe_info> keyframe_vector_t; 00568 00569 struct TSpanningTree 00570 { 00572 typedef mrpt::utils::map_as_vector< 00573 TKeyFrameID, 00574 std::map<TKeyFrameID,TSpanTreeEntry>, 00575 std::deque<std::pair<TKeyFrameID,std::map<TKeyFrameID,TSpanTreeEntry> > > 00576 > next_edge_maps_t; 00577 00579 typedef mrpt::utils::map_as_vector< 00580 TKeyFrameID, 00581 std::map<TKeyFrameID, k2k_edge_vector_t>, 00582 std::deque<std::pair<TKeyFrameID,std::map<TKeyFrameID, k2k_edge_vector_t > > > 00583 > all_edges_maps_t; 00584 00585 const TRBA_Problem_state<kf2kf_pose_t,landmark_t,obs_t,RBA_OPTIONS> *m_parent; 00586 00591 struct TSpanningTreeSym 00592 { 00599 next_edge_maps_t next_edge; 00600 00607 all_edges_maps_t all_edges; 00608 } 00609 sym; 00610 00620 typename kf2kf_pose_traits<kf2kf_pose_t>::TRelativePosesForEachTarget num; 00621 00629 void clear(); 00630 00634 void update_symbolic_new_node( 00635 const TKeyFrameID new_node_id, 00636 const TPairKeyFrameID & new_edge, 00637 const topo_dist_t max_depth 00638 ); 00639 00643 size_t update_numeric(bool skip_marked_as_uptodate = false); 00644 00646 size_t update_numeric(const std::set<TKeyFrameID> & kfs_to_update,bool skip_marked_as_uptodate = false); 00647 00651 size_t update_numeric_only_all_from_node( const typename all_edges_maps_t::const_iterator & it,bool skip_marked_as_uptodate = false); 00652 00659 void dump_as_text(std::string &s) const; 00660 00662 bool dump_as_text_to_file(const std::string &sFileName) const; 00663 00668 bool save_as_dot_file(const std::string &sFileName, const std::vector<TKeyFrameID> &kf_roots_to_save = std::vector<TKeyFrameID>() ) const; 00669 00671 void get_stats( 00672 size_t &num_nodes_min, 00673 size_t &num_nodes_max, 00674 double &num_nodes_mean, 00675 double &num_nodes_std) const; 00676 00679 }; // end of TSpanningTree 00680 00681 00682 struct TLinearSystem 00683 { 00684 TLinearSystem() : 00685 dh_dAp(), 00686 dh_df() 00687 { 00688 } 00689 00690 typename jacobian_traits<kf2kf_pose_t,landmark_t,obs_t>::TSparseBlocksJacobians_dh_dAp dh_dAp; 00691 typename jacobian_traits<kf2kf_pose_t,landmark_t,obs_t>::TSparseBlocksJacobians_dh_df dh_df; 00692 00693 void clear() { 00694 dh_dAp.clearAll(); 00695 dh_df.clearAll(); 00696 } 00697 00698 }; // end of TLinearSystem 00699 00703 keyframe_vector_t keyframes; 00704 k2k_edges_deque_t k2k_edges; 00705 TRelativeLandmarkPosMap unknown_lms; 00706 landmarks2infmatrix_t unknown_lms_inf_matrices; 00707 TRelativeLandmarkPosMap known_lms; 00708 00711 typename mrpt::aligned_containers<TLandmarkEntry>::deque_t all_lms; 00712 00713 TSpanningTree spanning_tree; 00714 all_observations_deque_t all_observations; 00715 TLinearSystem lin_system; 00716 00722 std::deque<char> all_observations_Jacob_validity; 00723 00725 std::set<size_t> last_timestep_touched_kfs; 00729 void clear() { 00730 keyframes.clear(); 00731 k2k_edges.clear(); 00732 unknown_lms.clear(); 00733 unknown_lms_inf_matrices.clear(); 00734 known_lms.clear(); 00735 all_lms.clear(); 00736 spanning_tree.clear(); 00737 all_observations.clear(); 00738 lin_system.clear(); 00739 last_timestep_touched_kfs.clear(); 00740 } 00741 00743 TRBA_Problem_state() { 00744 spanning_tree.m_parent=this; // Not passed as ctor argument to avoid compiler warnings... 00745 } 00746 00753 bool find_path_bfs( 00754 const TKeyFrameID from, 00755 const TKeyFrameID to, 00756 std::vector<TKeyFrameID> * out_path_IDs, 00757 typename kf2kf_pose_traits<kf2kf_pose_t>::k2k_edge_vector_t * out_path_edges = NULL) const; 00758 00759 00761 void compute_all_node_degrees( 00762 double &out_mean_degree, 00763 double &out_std_degree, 00764 double &out_max_degree) const; 00765 00767 bool are_keyframes_connected(const TKeyFrameID id1, const TKeyFrameID id2) const; 00768 00776 size_t alloc_kf2kf_edge( 00777 const TPairKeyFrameID &ids, 00778 const pose_t &init_inv_pose_val = pose_t() ); 00779 00780 private: 00781 // Forbid making copies of this object, since it heavily relies on internal lists of pointers: 00782 TRBA_Problem_state(const TRBA_Problem_state &); 00783 TRBA_Problem_state & operator =(const TRBA_Problem_state &); 00784 00785 }; // end of TRBA_Problem_state 00786 00787 } // end of namespace "srba" 00788 00789 // Specializations MUST occur at the same namespace: 00790 namespace mrpt { namespace utils 00791 { 00792 template <> 00793 struct TEnumTypeFiller<srba::TCovarianceRecoveryPolicy> 00794 { 00795 typedef srba::TCovarianceRecoveryPolicy enum_t; 00796 static void fill(bimap<enum_t,std::string> &m_map) 00797 { 00798 m_map.insert(srba::crpNone, "crpNone"); 00799 m_map.insert(srba::crpLandmarksApprox, "crpLandmarksApprox"); 00800 } 00801 }; 00802 } } // End of namespace
1.7.6.1
