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 namespace srba { 00013 00014 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS> 00015 size_t RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS>::create_kf2kf_edge( 00016 const TKeyFrameID new_kf_id, 00017 const TPairKeyFrameID & new_edge, 00018 const typename traits_t::new_kf_observations_t & obs, 00019 const pose_t &init_inv_pose_val ) 00020 { 00021 // 1) Create new kf2kf structures (all but stuff related to the spanning trees) 00022 // --------------------------------------------------------------------------------- 00023 const size_t ed_id = rba_state.alloc_kf2kf_edge( new_edge, init_inv_pose_val ); // O(1) 00024 00025 m_profiler.enter("define_new_keyframe.st.update_symbolic"); 00026 00027 // 2) Update symbolic spanning trees: 00028 // --------------------------------------------------------------------------------- 00029 rba_state.spanning_tree.update_symbolic_new_node( 00030 new_kf_id, 00031 new_edge, 00032 parameters.srba.max_tree_depth ); 00033 00034 m_profiler.leave("define_new_keyframe.st.update_symbolic"); 00035 00036 return ed_id; 00037 } 00038 00039 } // end NS