|
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 00016 #include <mrpt/utils/CTimeLogger.h> 00017 #include <mrpt/utils/CLoadableOptions.h> 00018 #include <mrpt/opengl/CSetOfObjects.h> 00019 #include <mrpt/poses/CPose3DQuat.h> // Needed by "CNetworkOfPoses.h" in older mrpt versions 00020 #include <mrpt/graphs/CNetworkOfPoses.h> 00021 #include <mrpt/system/os.h> 00022 #include <mrpt/system/memory.h> // MRPT_MAKE_ALIGNED_OPERATOR_NEW 00023 #include "impl/make_ordered_list_base_kfs.h" // Internal aux function 00024 00025 #include "srba_types.h" 00026 #include "srba_options.h" 00027 #include "srba_edge_creation_policies.h" 00028 #include "landmark_jacob_families.h" 00029 #include "landmark_matcher.h" 00030 00031 namespace srba 00032 { 00039 struct RBA_OPTIONS_DEFAULT 00040 { 00041 typedef ecps::local_areas_fixed_size edge_creation_policy_t; 00042 typedef options::sensor_pose_on_robot_none sensor_pose_on_robot_t; 00043 typedef options::observation_noise_identity obs_noise_matrix_t; 00044 typedef options::solver_LM_schur_dense_cholesky solver_t; 00045 }; 00046 00066 template <class KF2KF_POSE_TYPE,class LM_TYPE,class OBS_TYPE,class RBA_OPTIONS = RBA_OPTIONS_DEFAULT> 00067 class RbaEngine 00068 { 00069 public: 00072 typedef RbaEngine<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS> rba_engine_t; 00073 00074 00075 typedef KF2KF_POSE_TYPE kf2kf_pose_t; 00076 typedef LM_TYPE landmark_t; 00077 typedef OBS_TYPE obs_t; 00078 typedef RBA_OPTIONS rba_options_t; 00079 00080 static const size_t REL_POSE_DIMS = kf2kf_pose_t::REL_POSE_DIMS; 00081 static const size_t LM_DIMS = landmark_t::LM_DIMS; 00082 static const size_t OBS_DIMS = obs_t::OBS_DIMS; 00083 00084 typedef typename kf2kf_pose_t::se_traits_t se_traits_t; 00085 00086 typedef rba_joint_parameterization_traits_t<kf2kf_pose_t,landmark_t,obs_t> traits_t; 00087 typedef jacobian_traits<kf2kf_pose_t,landmark_t,obs_t> jacobian_traits_t; 00088 typedef hessian_traits<kf2kf_pose_t,landmark_t,obs_t> hessian_traits_t; 00089 typedef kf2kf_pose_traits<kf2kf_pose_t> kf2kf_pose_traits_t; 00090 typedef landmark_traits<landmark_t> landmark_traits_t; 00091 typedef observation_traits<obs_t> observation_traits_t; 00092 00093 typedef sensor_model<landmark_t,obs_t> sensor_model_t; 00094 00095 typedef typename kf2kf_pose_t::pose_t pose_t; 00096 typedef TRBA_Problem_state<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE,RBA_OPTIONS> rba_problem_state_t; 00097 00098 typedef typename rba_problem_state_t::k2f_edge_t k2f_edge_t; 00099 typedef typename rba_problem_state_t::k2k_edge_t k2k_edge_t; 00100 typedef typename rba_problem_state_t::k2k_edges_deque_t k2k_edges_deque_t; 00101 00102 typedef typename kf2kf_pose_traits_t::pose_flag_t pose_flag_t; 00103 typedef typename kf2kf_pose_traits_t::frameid2pose_map_t frameid2pose_map_t; 00104 typedef typename kf2kf_pose_traits_t::TRelativePosesForEachTarget TRelativePosesForEachTarget; 00105 typedef typename landmark_traits_t::TRelativeLandmarkPosMap TRelativeLandmarkPosMap; 00106 typedef typename landmark_traits_t::TRelativeLandmarkPos TRelativeLandmarkPos; 00107 typedef typename traits_t::keyframe_info keyframe_info; 00108 typedef typename traits_t::new_kf_observation_t new_kf_observation_t; 00109 typedef typename traits_t::new_kf_observations_t new_kf_observations_t; 00110 00111 typedef typename kf2kf_pose_traits_t::array_pose_t array_pose_t; 00112 typedef typename landmark_traits_t::array_landmark_t array_landmark_t; 00113 typedef typename observation_traits_t::array_obs_t array_obs_t; 00114 typedef typename observation_traits_t::residual_t residual_t; 00115 typedef typename observation_traits_t::vector_residuals_t vector_residuals_t; 00116 00117 typedef typename jacobian_traits<kf2kf_pose_t,landmark_t,obs_t>::TSparseBlocksJacobians_dh_dAp TSparseBlocksJacobians_dh_dAp; 00118 typedef typename jacobian_traits<kf2kf_pose_t,landmark_t,obs_t>::TSparseBlocksJacobians_dh_df TSparseBlocksJacobians_dh_df; 00122 RbaEngine(); 00123 00125 struct TOptimizeExtraOutputInfo 00126 { 00127 TOptimizeExtraOutputInfo() 00128 { 00129 clear(); 00130 } 00131 00132 size_t num_observations; 00133 size_t num_jacobians; 00134 size_t num_kf2kf_edges_optimized; 00135 size_t num_kf2lm_edges_optimized; 00136 size_t num_total_scalar_optimized; 00137 size_t num_kf_optimized; 00138 size_t num_lm_optimized; 00139 size_t num_span_tree_numeric_updates; 00140 double obs_rmse; 00141 double total_sqr_error_init, total_sqr_error_final; 00142 double HAp_condition_number; 00143 00147 size_t sparsity_dh_dAp_nnz, sparsity_dh_dAp_max_size, sparsity_dh_df_nnz, sparsity_dh_df_max_size, sparsity_HAp_nnz, sparsity_HAp_max_size, 00148 sparsity_Hf_nnz, sparsity_Hf_max_size, sparsity_HApf_nnz, sparsity_HApf_max_size; 00149 00150 std::vector<size_t> optimized_k2k_edge_indices; 00151 std::vector<size_t> optimized_landmark_indices; 00152 00154 typename RBA_OPTIONS::solver_t::extra_results_t extra_results; 00155 00156 void clear() 00157 { 00158 num_observations = 0; 00159 num_jacobians = 0; 00160 num_kf2kf_edges_optimized = 0; 00161 num_kf2lm_edges_optimized = 0; 00162 num_total_scalar_optimized = 0; 00163 num_kf_optimized = 0; 00164 num_lm_optimized = 0; 00165 num_span_tree_numeric_updates=0; 00166 total_sqr_error_init=0.; 00167 total_sqr_error_final=0.; 00168 HAp_condition_number=0.; 00169 sparsity_dh_dAp_nnz = sparsity_dh_dAp_max_size = sparsity_dh_df_nnz = sparsity_dh_df_max_size = 00170 sparsity_HAp_nnz = sparsity_HAp_max_size = sparsity_Hf_nnz = sparsity_Hf_max_size = sparsity_HApf_nnz = sparsity_HApf_max_size = 0; 00171 optimized_k2k_edge_indices.clear(); 00172 optimized_landmark_indices.clear(); 00173 extra_results.clear(); 00174 } 00175 00176 MRPT_MAKE_ALIGNED_OPERATOR_NEW // Required by Eigen containers 00177 }; 00178 00180 struct TNewKeyFrameInfo 00181 { 00182 TKeyFrameID kf_id; 00183 std::vector<TNewEdgeInfo> created_edge_ids; 00184 TOptimizeExtraOutputInfo optimize_results; 00185 TOptimizeExtraOutputInfo optimize_results_stg1; 00186 00187 void clear() 00188 { 00189 kf_id = static_cast<TKeyFrameID>(-1); 00190 created_edge_ids.clear(); 00191 optimize_results.clear(); 00192 } 00193 00194 MRPT_MAKE_ALIGNED_OPERATOR_NEW // Required by Eigen containers 00195 }; 00196 00207 void define_new_keyframe( 00208 const typename traits_t::new_kf_observations_t & obs, 00209 TNewKeyFrameInfo & out_new_kf_info, 00210 const bool run_local_optimization = true 00211 ); 00212 00214 struct TOptimizeLocalAreaParams 00215 { 00216 bool optimize_k2k_edges; 00217 bool optimize_landmarks; 00218 TKeyFrameID max_visitable_kf_id; 00219 size_t dont_optimize_landmarks_seen_less_than_n_times; 00220 00221 TOptimizeLocalAreaParams() : 00222 optimize_k2k_edges(true), 00223 optimize_landmarks(true), 00224 max_visitable_kf_id( static_cast<TKeyFrameID>(-1) ), 00225 dont_optimize_landmarks_seen_less_than_n_times( 2 ) 00226 {} 00227 }; 00228 00235 void optimize_local_area( 00236 const TKeyFrameID root_id, 00237 const unsigned int win_size, 00238 TOptimizeExtraOutputInfo & out_info, 00239 const TOptimizeLocalAreaParams ¶ms = TOptimizeLocalAreaParams(), 00240 const std::vector<size_t> & observation_indices_to_optimize = std::vector<size_t>() 00241 ); 00242 00243 00244 struct TOpenGLRepresentationOptions : public landmark_t::render_mode_t::TOpenGLRepresentationOptionsExtra 00245 { 00246 TOpenGLRepresentationOptions() : 00247 span_tree_max_depth( std::numeric_limits<size_t>::max() ), 00248 draw_unknown_feats(true), 00249 draw_unknown_feats_ellipses(true), 00250 draw_unknown_feats_ellipses_quantiles(1), 00251 show_unknown_feats_ids(true), 00252 draw_kf_hierarchical(false), 00253 draw_kf_hierarchical_height(10.0) 00254 { 00255 } 00256 00257 size_t span_tree_max_depth; 00258 bool draw_unknown_feats; 00259 bool draw_unknown_feats_ellipses; 00260 double draw_unknown_feats_ellipses_quantiles; 00261 bool show_unknown_feats_ids; 00262 bool draw_kf_hierarchical; 00263 double draw_kf_hierarchical_height; 00264 }; 00265 00271 void build_opengl_representation( 00272 const srba::TKeyFrameID root_keyframe, 00273 const TOpenGLRepresentationOptions &options, 00274 mrpt::opengl::CSetOfObjectsPtr out_scene, 00275 mrpt::opengl::CSetOfObjectsPtr out_root_tree = mrpt::opengl::CSetOfObjectsPtr() 00276 ) const; 00277 00280 bool save_graph_as_dot( 00281 const std::string &targetFileName, 00282 const bool all_landmarks = false 00283 ) const; 00284 00289 bool save_graph_top_structure_as_dot( 00290 const std::string &targetFileName, 00291 const bool set_node_coordinates 00292 ) const; 00293 00301 double eval_overall_squared_error() const; 00302 00303 struct ExportGraphSLAM_Params 00304 { 00305 TKeyFrameID root_kf_id; 00306 00307 ExportGraphSLAM_Params() : root_kf_id(0) {} 00308 }; 00316 template <class POSE_GRAPH> 00317 void get_global_graphslam_problem(POSE_GRAPH &global_graph, const ExportGraphSLAM_Params ¶ms = ExportGraphSLAM_Params() ) const; 00318 00319 // End of main API methods 00321 00322 00327 void clear(); 00328 00334 TKeyFrameID alloc_keyframe(); 00335 00343 size_t create_kf2kf_edge( 00344 const TKeyFrameID new_kf_id, 00345 const TPairKeyFrameID & new_edge, 00346 const typename traits_t::new_kf_observations_t & obs, 00347 const pose_t &init_inv_pose_val = pose_t() ); 00348 00349 00359 void create_complete_spanning_tree( 00360 const TKeyFrameID root_id, 00361 frameid2pose_map_t & span_tree, 00362 const size_t max_depth = std::numeric_limits<size_t>::max(), 00363 std::vector<bool> * aux_ws = NULL 00364 ) const; 00365 00375 bool find_path_bfs( 00376 const TKeyFrameID src_kf, 00377 const TKeyFrameID trg_kf, 00378 std::vector<TKeyFrameID> & found_path) const 00379 { 00380 return rba_state.find_path_bfs(src_kf,trg_kf,&found_path); 00381 } 00382 00385 const pose_t * get_kf_relative_pose(const TKeyFrameID kf_query, const TKeyFrameID kf_reference) const 00386 { 00387 // Get the relative pose from the numeric spanning tree, which should be up-to-date: 00388 typename kf2kf_pose_traits_t::TRelativePosesForEachTarget::const_iterator it_tree4_central = rba_state.spanning_tree.num.find(kf_reference); 00389 if (it_tree4_central==rba_state.spanning_tree.num.end()) 00390 return NULL; 00391 typename kf2kf_pose_traits_t::frameid2pose_map_t::const_iterator it_nei_1 = 00392 it_tree4_central->second.find(kf_query); 00393 if (it_nei_1!=it_tree4_central->second.end()) 00394 return &it_nei_1->second.pose; 00395 else return NULL; 00396 } 00397 00398 00402 template < 00403 class KF_VISITOR, 00404 class FEAT_VISITOR, 00405 class K2K_EDGE_VISITOR, 00406 class K2F_EDGE_VISITOR 00407 > 00408 void bfs_visitor( 00409 const TKeyFrameID root_id, 00410 const topo_dist_t max_distance, 00411 const bool rely_on_prebuilt_spanning_trees, 00412 KF_VISITOR & kf_visitor, 00413 FEAT_VISITOR & feat_visitor, 00414 K2K_EDGE_VISITOR & k2k_edge_visitor, 00415 K2F_EDGE_VISITOR & k2f_edge_visitor ) const; 00416 // End of Extra API methods 00418 00419 00424 struct TSRBAParameters : public mrpt::utils::CLoadableOptions 00425 { 00426 TSRBAParameters(); 00427 00429 virtual void loadFromConfigFile(const mrpt::utils::CConfigFileBase & source,const std::string & section); 00431 virtual void saveToConfigFile(mrpt::utils::CConfigFileBase &out,const std::string & section) const; 00432 00434 topo_dist_t max_tree_depth; 00435 00437 topo_dist_t max_optimize_depth; 00438 // ------------------------------------------------------- 00439 00440 // Parameters for optimize_*() 00441 // ------------------------------------- 00442 bool optimize_new_edges_alone; 00443 bool use_robust_kernel; 00444 bool use_robust_kernel_stage1; 00445 double kernel_param; 00446 size_t max_iters; 00447 double max_error_per_obs_to_stop; 00448 double max_rho; 00449 double max_lambda; 00450 double min_error_reduction_ratio_to_relinearize; 00451 bool numeric_jacobians; 00452 void (*feedback_user_iteration)(unsigned int iter, const double total_sq_err, const double mean_sqroot_error); 00453 bool compute_condition_number; 00454 bool compute_sparsity_stats; 00455 double max_rmse_show_red_warning; 00456 00457 TCovarianceRecoveryPolicy cov_recovery; 00458 // ------------------------------------- 00459 00460 }; 00461 00463 struct TAllParameters 00464 { 00465 TSRBAParameters srba; 00466 typename obs_t::TObservationParams sensor; 00467 typename RBA_OPTIONS::sensor_pose_on_robot_t::parameters_t sensor_pose; 00468 typename RBA_OPTIONS::obs_noise_matrix_t::parameters_t obs_noise; 00469 typename RBA_OPTIONS::edge_creation_policy_t::parameters_t ecp; 00470 00471 MRPT_MAKE_ALIGNED_OPERATOR_NEW // Required by Eigen containers 00472 }; 00473 TAllParameters parameters; 00474 00475 typename RBA_OPTIONS::edge_creation_policy_t edge_creation_policy; 00476 // End of data fields 00478 00479 00481 void inline enable_time_profiler(bool enable=true) { m_profiler.enable(enable); } 00482 00483 const k2k_edges_deque_t & get_k2k_edges() const { return rba_state.k2k_edges; } 00484 00485 const TRelativeLandmarkPosMap & get_known_feats() const { return rba_state.known_lms; } 00486 const TRelativeLandmarkPosMap & get_unknown_feats() const { return rba_state.unknown_lms; } 00487 00488 const rba_problem_state_t & get_rba_state() const { return rba_state; } 00489 rba_problem_state_t & get_rba_state() { return rba_state; } 00490 00492 inline mrpt::utils::CTimeLogger & get_time_profiler() { return m_profiler; } 00493 00495 inline void setVerbosityLevel(int level) { m_verbose_level = level; } 00496 00500 template <class HESS_Ap, class HESS_f,class HESS_Apf, class JACOB_COLUMN_dh_dAp,class JACOB_COLUMN_dh_df> 00501 static void sparse_hessian_build_symbolic( 00502 HESS_Ap & HAp, 00503 HESS_f & Hf, 00504 HESS_Apf & HApf, 00505 const std::vector<JACOB_COLUMN_dh_dAp*> & dh_dAp, 00506 const std::vector<JACOB_COLUMN_dh_df*> & dh_df); 00507 00513 template <class SPARSEBLOCKHESSIAN> 00514 size_t sparse_hessian_update_numeric( SPARSEBLOCKHESSIAN & H ) const; 00515 00516 00517 protected: 00518 int m_verbose_level; 00519 00524 void determine_kf2kf_edges_to_create( 00525 const TKeyFrameID new_kf_id, 00526 const typename traits_t::new_kf_observations_t & obs, 00527 std::vector<TNewEdgeInfo> &new_k2k_edge_ids ); 00528 00533 void optimize_edges( 00534 const std::vector<size_t> & run_k2k_edges, 00535 const std::vector<size_t> & run_feat_ids_in, 00536 TOptimizeExtraOutputInfo & out_info, 00537 const std::vector<size_t> & observation_indices_to_optimize = std::vector<size_t>() 00538 ); 00539 00543 struct VisitorOptimizeLocalArea 00544 { 00545 VisitorOptimizeLocalArea(const rba_problem_state_t & rba_state_, const TOptimizeLocalAreaParams ¶ms_) : 00546 rba_state(rba_state_), 00547 params(params_) 00548 { } 00549 00550 const rba_problem_state_t & rba_state; 00551 const TOptimizeLocalAreaParams ¶ms; 00552 00553 std::vector<size_t> k2k_edges_to_optimize, lm_IDs_to_optimize; 00554 std::map<TLandmarkID,size_t> lm_times_seen; 00555 00556 /* Implementation of FEAT_VISITOR */ 00557 inline bool visit_filter_feat(const TLandmarkID lm_ID,const topo_dist_t cur_dist) 00558 { 00559 MRPT_UNUSED_PARAM(lm_ID); MRPT_UNUSED_PARAM(cur_dist); 00560 return false; // Don't need to visit landmark nodes. 00561 } 00562 00563 inline void visit_feat(const TLandmarkID lm_ID,const topo_dist_t cur_dist) 00564 { 00565 MRPT_UNUSED_PARAM(lm_ID); MRPT_UNUSED_PARAM(cur_dist); 00566 // Nothing to do 00567 } 00568 00569 /* Implementation of KF_VISITOR */ 00570 inline bool visit_filter_kf(const TKeyFrameID kf_ID,const topo_dist_t cur_dist) 00571 { 00572 MRPT_UNUSED_PARAM(cur_dist); 00573 return (kf_ID<=params.max_visitable_kf_id); 00574 } 00575 00576 inline void visit_kf(const TKeyFrameID kf_ID,const topo_dist_t cur_dist) 00577 { 00578 MRPT_UNUSED_PARAM(kf_ID); MRPT_UNUSED_PARAM(cur_dist); 00579 // Nothing to do. 00580 } 00581 00582 /* Implementation of K2K_EDGE_VISITOR */ 00583 inline bool visit_filter_k2k(const TKeyFrameID current_kf, const TKeyFrameID next_kf, 00584 const k2k_edge_t* edge, const topo_dist_t cur_dist) 00585 { 00586 MRPT_UNUSED_PARAM(current_kf); MRPT_UNUSED_PARAM(next_kf); 00587 MRPT_UNUSED_PARAM(edge); MRPT_UNUSED_PARAM(cur_dist); 00588 return true; // Visit all k2k edges 00589 } 00590 00591 inline void visit_k2k(const TKeyFrameID current_kf, const TKeyFrameID next_kf, 00592 const k2k_edge_t* edge, const topo_dist_t cur_dist) 00593 { 00594 MRPT_UNUSED_PARAM(current_kf); MRPT_UNUSED_PARAM(next_kf); 00595 MRPT_UNUSED_PARAM(cur_dist); 00596 if (params.optimize_k2k_edges) 00597 k2k_edges_to_optimize.push_back(edge->id); 00598 } 00599 00600 /* Implementation of K2F_EDGE_VISITOR */ 00601 inline bool visit_filter_k2f(const TKeyFrameID current_kf, const k2f_edge_t* edge, 00602 const topo_dist_t cur_dist) 00603 { 00604 MRPT_UNUSED_PARAM(current_kf); MRPT_UNUSED_PARAM(edge); MRPT_UNUSED_PARAM(cur_dist); 00605 return params.optimize_landmarks; // Yes: visit all feature nodes if we're asked to 00606 } 00607 inline void visit_k2f(const TKeyFrameID current_kf, const k2f_edge_t* edge, const topo_dist_t cur_dist) 00608 { 00609 MRPT_UNUSED_PARAM(current_kf); MRPT_UNUSED_PARAM(cur_dist); 00610 if (!edge->feat_has_known_rel_pos) 00611 { 00612 const TLandmarkID lm_ID = edge->obs.obs.feat_id; 00613 // Use an "==" so we only add the LM_ID to the list ONCE, just when it passes the threshold 00614 if (++lm_times_seen[lm_ID] == params.dont_optimize_landmarks_seen_less_than_n_times) 00615 lm_IDs_to_optimize.push_back(lm_ID); 00616 } 00617 } 00618 }; 00619 00620 private: 00621 rba_problem_state_t rba_state; 00622 00623 mutable std::vector<bool> m_complete_st_ws; 00624 00628 mutable mrpt::utils::CTimeLogger m_profiler; 00629 00645 size_t add_observation( 00646 const TKeyFrameID observing_kf_id, 00647 const typename observation_traits_t::observation_t & new_obs, 00648 const array_landmark_t * fixed_relative_position = NULL, 00649 const array_landmark_t * unknown_relative_position_init_val = NULL 00650 ); 00651 00653 void prepare_Jacobians_required_tree_roots( 00654 std::set<TKeyFrameID> & kfs_num_spantrees_to_update, 00655 const std::vector<typename TSparseBlocksJacobians_dh_dAp::col_t*> &lst_JacobCols_dAp, 00656 const std::vector<typename TSparseBlocksJacobians_dh_df::col_t*> &lst_JacobCols_df ); 00657 00658 00660 size_t recompute_all_Jacobians( 00661 std::vector<typename TSparseBlocksJacobians_dh_dAp::col_t*> &lst_JacobCols_dAp, 00662 std::vector<typename TSparseBlocksJacobians_dh_df::col_t*> &lst_JacobCols_df, 00663 std::vector<const pose_flag_t*> * out_list_of_required_num_poses = NULL ); 00664 00665 public: 00666 00668 struct TBFSEntryEdges 00669 { 00670 TBFSEntryEdges() : dist( std::numeric_limits<topo_dist_t>::max() ), edge(NULL) 00671 {} 00672 00673 TKeyFrameID prev; 00674 topo_dist_t dist; 00675 const k2k_edge_t* edge; 00676 }; 00677 00678 00691 void compute_jacobian_dh_dp( 00692 typename TSparseBlocksJacobians_dh_dAp::TEntry &jacob, 00693 const k2f_edge_t & observation, 00694 const k2k_edges_deque_t &k2k_edges, 00695 std::vector<const pose_flag_t*> *out_list_of_required_num_poses) const; 00696 00707 void compute_jacobian_dh_df( 00708 typename TSparseBlocksJacobians_dh_df::TEntry &jacob, 00709 const k2f_edge_t & observation, 00710 std::vector<const pose_flag_t*> *out_list_of_required_num_poses) const; 00711 00712 void gl_aux_draw_node(mrpt::opengl::CSetOfObjects &soo, const std::string &label, const float x, const float y) const; 00713 00714 const pose_t aux_null_pose; 00715 00716 struct TNumeric_dh_dAp_params 00717 { 00718 TNumeric_dh_dAp_params( 00719 const size_t _k2k_edge_id, 00720 const pose_t * _pose_d1_wrt_obs, 00721 const pose_t & _pose_base_wrt_d1, 00722 const array_landmark_t & _xji_i, 00723 const bool _is_inverse_dir, 00724 const k2k_edges_deque_t &_k2k_edges, 00725 const typename obs_t::TObservationParams & _sensor_params, 00726 const typename RBA_OPTIONS::sensor_pose_on_robot_t::parameters_t & _sensor_pose 00727 ) : 00728 k2k_edge_id(_k2k_edge_id), 00729 pose_d1_wrt_obs(_pose_d1_wrt_obs), 00730 pose_base_wrt_d1(_pose_base_wrt_d1), 00731 xji_i(_xji_i), 00732 is_inverse_dir(_is_inverse_dir), 00733 k2k_edges(_k2k_edges), 00734 sensor_params(_sensor_params), 00735 sensor_pose(_sensor_pose) 00736 { 00737 } 00738 00739 const size_t k2k_edge_id; 00740 const pose_t * pose_d1_wrt_obs; 00741 const pose_t & pose_base_wrt_d1; 00742 const array_landmark_t & xji_i; 00743 const bool is_inverse_dir; 00744 const k2k_edges_deque_t &k2k_edges; 00745 const typename obs_t::TObservationParams & sensor_params; 00746 const typename RBA_OPTIONS::sensor_pose_on_robot_t::parameters_t & sensor_pose; 00747 }; 00748 00749 struct TNumeric_dh_df_params 00750 { 00751 TNumeric_dh_df_params( 00752 const pose_t * _pose_base_wrt_obs, 00753 const array_landmark_t & _xji_i, 00754 const typename obs_t::TObservationParams & _sensor_params, 00755 const typename RBA_OPTIONS::sensor_pose_on_robot_t::parameters_t & _sensor_pose 00756 ) : 00757 pose_base_wrt_obs(_pose_base_wrt_obs), 00758 xji_i(_xji_i), 00759 sensor_params(_sensor_params), 00760 sensor_pose(_sensor_pose) 00761 { 00762 } 00763 00764 const pose_t * pose_base_wrt_obs; 00765 const array_landmark_t & xji_i; 00766 const typename obs_t::TObservationParams & sensor_params; 00767 const typename RBA_OPTIONS::sensor_pose_on_robot_t::parameters_t & sensor_pose; 00768 }; 00769 00771 static void numeric_dh_dAp(const array_pose_t &x, const TNumeric_dh_dAp_params& params, array_obs_t &y); 00773 static void numeric_dh_df(const array_landmark_t &x, const TNumeric_dh_df_params& params, array_obs_t &y); 00774 00775 static inline void add_edge_ij_to_list_needed_roots(std::set<TKeyFrameID> & lst, const TKeyFrameID i, const TKeyFrameID j) 00776 { 00777 lst.insert(i); 00778 lst.insert(j); 00779 } 00780 00781 void compute_minus_gradient( 00782 Eigen::VectorXd & minus_grad, 00783 const std::vector<typename TSparseBlocksJacobians_dh_dAp::col_t*> & sparse_jacobs_Ap, 00784 const std::vector<typename TSparseBlocksJacobians_dh_df::col_t*> & sparse_jacobs_f, 00785 const vector_residuals_t & residuals, 00786 const std::map<size_t,size_t> &obs_global_idx2residual_idx 00787 ) const; 00788 00790 struct TObsUsed 00791 { 00792 TObsUsed(const size_t obs_idx_, k2f_edge_t * const k2f_) : obs_idx(obs_idx_),k2f(k2f_) 00793 { } 00794 00795 size_t obs_idx; 00796 k2f_edge_t* k2f; 00797 00798 private: 00799 // Don't use this constructor 00800 TObsUsed() : obs_idx(0), k2f(NULL) {} 00801 }; // end of TObsUsed 00802 00803 00804 inline double reprojection_residuals( 00805 vector_residuals_t & residuals, // Out: 00806 const std::vector<TObsUsed> & observations // In: 00807 ) const; 00808 00810 static inline double huber_kernel(double delta, const double kernel_param) 00811 { 00812 return std::abs(2*mrpt::utils::square(kernel_param)*(std::sqrt(1+mrpt::utils::square(delta/kernel_param))-1)); 00813 } 00814 00815 MRPT_MAKE_ALIGNED_OPERATOR_NEW // Required by Eigen containers 00816 }; // end of class "RbaEngine" 00817 00818 00819 } // end of namespace "srba" 00820 00821 // ----------------------------------------------------------------- 00822 // Include all template implementation files 00823 // ----------------------------------------------------------------- 00824 #include "impl/add-observations.h" 00825 #include "impl/alloc_keyframe.h" 00826 #include "impl/alloc_kf2kf_edge.h" 00827 #include "impl/create_kf2kf_edge.h" 00828 #include "impl/define_new_keyframe.h" 00829 #include "impl/jacobians.h" 00830 #include "impl/rba_problem_common.h" 00831 #include "impl/schur.h" 00832 #include "impl/sparse_hessian_build_symbolic.h" 00833 #include "impl/sparse_hessian_update_numeric.h" 00834 00835 #include "impl/spantree_create_complete.h" 00836 #include "impl/spantree_update_symbolic.h" 00837 #include "impl/spantree_update_numeric.h" 00838 #include "impl/spantree_misc.h" 00839 #include "impl/jacobians.h" 00840 00841 #include "impl/export_opengl.h" 00842 #include "impl/export_dot.h" 00843 #include "impl/get_global_graphslam_problem.h" 00844 #include "impl/eval_overall_error.h" 00845 #include "impl/determine_kf2kf_edges_to_create.h" 00846 #include "impl/reprojection_residuals.h" 00847 #include "impl/compute_minus_gradient.h" 00848 #include "impl/optimize_edges.h" 00849 #include "impl/lev-marq_solvers.h" 00850 #include "impl/bfs_visitor.h" 00851 #include "impl/optimize_local_area.h" 00852 // ----------------------------------------------------------------- 00853 // ^^ End of implementation files ^^ 00854 // ----------------------------------------------------------------- 00855 00856 // Undefine temporary macro for verbose output 00857 #undef VERBOSE_LEVEL 00858
1.7.6.1
