All the important data of a RBA problem at any given instant of time Operations on this structure are performed via the public API of srba::RbaEngine.
Definition at line 474 of file srba_types.h.
#include <mrpt/srba/srba_types.h>
Classes | |
| struct | TLinearSystem |
| struct | TSpanningTree |
Public Member Functions | |
| void | clear () |
| Empties all members. More... | |
| TRBA_Problem_state () | |
| Ctor. More... | |
| bool | find_path_bfs (const TKeyFrameID from, const TKeyFrameID to, std::vector< TKeyFrameID > *out_path_IDs, typename kf2kf_pose_traits< KF2KF_POSE_TYPE >::k2k_edge_vector_t *out_path_edges=NULL) const |
| Auxiliary, brute force (BFS) method for finding the shortest path between any two Keyframes. More... | |
| void | compute_all_node_degrees (double &out_mean_degree, double &out_std_degree, double &out_max_degree) const |
| Computes stats on the degree (# of adjacent nodes) of all the nodes in the graph. More... | |
| bool | are_keyframes_connected (const TKeyFrameID id1, const TKeyFrameID id2) const |
| Returns true if the pair of KFs are connected thru a kf2kf edge, no matter the direction of the edge. More... | |
| size_t | alloc_kf2kf_edge (const TPairKeyFrameID &ids, const pose_t &init_inv_pose_val=pose_t()) |
| Creates a new kf2kf edge variable. More... | |
Public Attributes | |
Data | |
| keyframe_vector_t | keyframes |
| All key frames (global poses are not included in an RBA problem). Vector indices are "TKeyFrameID" IDs. More... | |
| k2k_edges_deque_t | k2k_edges |
| (unknowns) All keyframe-to-keyframe edges More... | |
| TRelativeLandmarkPosMap | unknown_lms |
| (unknown values) Landmarks with an unknown fixed 3D position relative to their base frame_id More... | |
| landmarks2infmatrix_t | unknown_lms_inf_matrices |
| Information matrices that model the uncertainty in each XYZ position for the unknown LMs - these matrices should be already scaled according to the camera noise in pixel standard deviations. More... | |
| TRelativeLandmarkPosMap | known_lms |
| (known values) Landmarks with a known, fixed 3D position relative to their base frame_id More... | |
| mrpt::aligned_containers< TLandmarkEntry >::deque_t | all_lms |
| Index (by feat ID) of ALL landmarks stored in unknown_lms and known_lms. More... | |
| TSpanningTree | spanning_tree |
| all_observations_deque_t | all_observations |
| All raw observation data (k2f edges) More... | |
| TLinearSystem | lin_system |
| The sparse linear system of equations. More... | |
| std::deque< char > | all_observations_Jacob_validity |
| Its size grows simultaneously to all_observations, its values are updated during optimization to reflect invalid conditions in some Jacobians so we can completely discard their information while building the Hessian. More... | |
Private Member Functions | |
| TRBA_Problem_state (const TRBA_Problem_state &) | |
| TRBA_Problem_state & | operator= (const TRBA_Problem_state &) |
| typedef mrpt::aligned_containers<k2f_edge_t>::deque_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::all_observations_deque_t |
Definition at line 494 of file srba_types.h.
| typedef kf2kf_pose_traits<KF2KF_POSE_TYPE>::frameid2pose_map_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::frameid2pose_map_t |
Definition at line 479 of file srba_types.h.
| typedef rba_joint_parameterization_traits_t<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE>::k2f_edge_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::k2f_edge_t |
Definition at line 485 of file srba_types.h.
| typedef kf2kf_pose_traits<KF2KF_POSE_TYPE>::k2k_edge_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::k2k_edge_t |
Definition at line 477 of file srba_types.h.
| typedef kf2kf_pose_traits<KF2KF_POSE_TYPE>::k2k_edge_vector_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::k2k_edge_vector_t |
Definition at line 478 of file srba_types.h.
| typedef mrpt::aligned_containers<k2k_edge_t>::deque_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::k2k_edges_deque_t |
Definition at line 493 of file srba_types.h.
| typedef rba_joint_parameterization_traits_t<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE>::keyframe_info mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::keyframe_info |
Definition at line 484 of file srba_types.h.
| typedef std::deque<keyframe_info> mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::keyframe_vector_t |
Index are "TKeyFrameID" IDs. There's no NEED to make this a deque<> for preservation of references, but is an efficiency improvement.
Definition at line 497 of file srba_types.h.
| typedef hessian_traits<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE>::landmarks2infmatrix_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::landmarks2infmatrix_t |
Definition at line 483 of file srba_types.h.
| typedef rba_joint_parameterization_traits_t<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE>::new_kf_observation_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::new_kf_observation_t |
Definition at line 487 of file srba_types.h.
| typedef rba_joint_parameterization_traits_t<KF2KF_POSE_TYPE,LM_TYPE,OBS_TYPE>::new_kf_observations_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::new_kf_observations_t |
Definition at line 486 of file srba_types.h.
| typedef kf2kf_pose_traits<KF2KF_POSE_TYPE>::pose_flag_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::pose_flag_t |
Definition at line 480 of file srba_types.h.
| typedef KF2KF_POSE_TYPE::pose_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::pose_t |
Definition at line 476 of file srba_types.h.
| typedef landmark_traits<LM_TYPE>::TLandmarkEntry mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::TLandmarkEntry |
Definition at line 482 of file srba_types.h.
| typedef landmark_traits<LM_TYPE>::TRelativeLandmarkPosMap mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::TRelativeLandmarkPosMap |
Definition at line 481 of file srba_types.h.
|
inline |
Ctor.
Definition at line 672 of file srba_types.h.
References mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::TSpanningTree::m_parent.
|
private |
| size_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::alloc_kf2kf_edge | ( | const TPairKeyFrameID & | ids, |
| const pose_t & | init_inv_pose_val = pose_t() |
||
| ) |
Creates a new kf2kf edge variable.
Called from create_kf2kf_edge()
| [in] | init_inv_pose_val | The initial value for the inverse pose stored in edge first->second, i.e. the pose of first wrt. second. |
Definition at line 17 of file alloc_kf2kf_edge.h.
References ASSERT_, mrpt::mrpt::format(), mrpt::srba::kf2kf_pose_traits< POSE_TRAITS >::k2k_edge_t::from, mrpt::srba::kf2kf_pose_traits< POSE_TRAITS >::k2k_edge_t::id, mrpt::srba::kf2kf_pose_traits< POSE_TRAITS >::k2k_edge_t::inv_pose, and mrpt::srba::kf2kf_pose_traits< POSE_TRAITS >::k2k_edge_t::to.
| bool mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::are_keyframes_connected | ( | const TKeyFrameID | id1, |
| const TKeyFrameID | id2 | ||
| ) | const |
Returns true if the pair of KFs are connected thru a kf2kf edge, no matter the direction of the edge.
Runs in worst-case O(D) with D the degree of the KF graph (that is, the maximum number of edges adjacent to one KF)
Definition at line 131 of file rba_problem_common.h.
References ASSERT_BELOW_, and mrpt::srba::getTheOtherFromPair2().
|
inline |
Empties all members.
Definition at line 659 of file srba_types.h.
References mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::TSpanningTree::clear(), and mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::TLinearSystem::clear().
| void mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::compute_all_node_degrees | ( | double & | out_mean_degree, |
| double & | out_std_degree, | ||
| double & | out_max_degree | ||
| ) | const |
Computes stats on the degree (# of adjacent nodes) of all the nodes in the graph.
Runs in O(N) with N=# of keyframes
Definition at line 106 of file rba_problem_common.h.
References mrpt::math::maximum(), and mrpt::math::meanAndStd().
| bool mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::find_path_bfs | ( | const TKeyFrameID | from, |
| const TKeyFrameID | to, | ||
| std::vector< TKeyFrameID > * | out_path_IDs, | ||
| typename kf2kf_pose_traits< KF2KF_POSE_TYPE >::k2k_edge_vector_t * | out_path_edges = NULL |
||
| ) | const |
Auxiliary, brute force (BFS) method for finding the shortest path between any two Keyframes.
Use only when the distance between nodes can be larger than the maximum depth of incrementally-built spanning trees
| [in,out] | out_path_IDs | (Ignored if ==NULL) Just leave this vector uninitialized at input, it'll be automatically initialized to the right size and values. |
| [in,out] | out_path_edges | (Ignored if ==NULL) Just like out_path_IDs, but here you'll receive the list of traversed edges, instead of the IDs of the visited KFs. |
Definition at line 302 of file spantree_update_symbolic.h.
References ASSERT_, ASSERTDEB_, mrpt::srba::TBFSEntry< k2k_edge_t >::dist, mrpt::srba::getTheOtherFromPair2(), mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::keyframes, mrpt::srba::TBFSEntry< k2k_edge_t >::prev, and mrpt::srba::TBFSEntry< k2k_edge_t >::prev_edge.
Referenced by mrpt::srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::find_path_bfs().
|
private |
| mrpt::aligned_containers<TLandmarkEntry>::deque_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::all_lms |
Index (by feat ID) of ALL landmarks stored in unknown_lms and known_lms.
Note that if gaps occur in the observed feature IDs, some pointers here will be NULL and some mem will be wasted, but in turn we have a O(1) search mechanism for all LMs.
Definition at line 643 of file srba_types.h.
| all_observations_deque_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::all_observations |
All raw observation data (k2f edges)
Definition at line 646 of file srba_types.h.
| std::deque<char> mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::all_observations_Jacob_validity |
Its size grows simultaneously to all_observations, its values are updated during optimization to reflect invalid conditions in some Jacobians so we can completely discard their information while building the Hessian.
Definition at line 654 of file srba_types.h.
| k2k_edges_deque_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::k2k_edges |
(unknowns) All keyframe-to-keyframe edges
Definition at line 636 of file srba_types.h.
Referenced by mrpt::srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::get_k2k_edges().
| keyframe_vector_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::keyframes |
All key frames (global poses are not included in an RBA problem). Vector indices are "TKeyFrameID" IDs.
Definition at line 635 of file srba_types.h.
Referenced by mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::find_path_bfs().
| TRelativeLandmarkPosMap mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::known_lms |
(known values) Landmarks with a known, fixed 3D position relative to their base frame_id
Definition at line 639 of file srba_types.h.
Referenced by mrpt::srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::get_known_feats().
| TLinearSystem mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::lin_system |
The sparse linear system of equations.
Definition at line 647 of file srba_types.h.
| TSpanningTree mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::spanning_tree |
Definition at line 645 of file srba_types.h.
| TRelativeLandmarkPosMap mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::unknown_lms |
(unknown values) Landmarks with an unknown fixed 3D position relative to their base frame_id
Definition at line 637 of file srba_types.h.
Referenced by mrpt::srba::RbaEngine< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::get_unknown_feats().
| landmarks2infmatrix_t mrpt::srba::TRBA_Problem_state< KF2KF_POSE_TYPE, LM_TYPE, OBS_TYPE, RBA_OPTIONS >::unknown_lms_inf_matrices |
Information matrices that model the uncertainty in each XYZ position for the unknown LMs - these matrices should be already scaled according to the camera noise in pixel standard deviations.
Definition at line 638 of file srba_types.h.
| Page generated by Doxygen 1.8.9.1 for MRPT 1.3.0 SVN: at Sun Sep 13 03:55:12 UTC 2015 |