Detailed Description
template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >
Edge Registration Decider scheme specialized in Loop Closing.
Description
Current decider is implemented based on the following papers:
- [1] Consistent Observation Grouping for Generating Metric-Topological Maps that Improves Robot Localization - J. Blanco, J. Gonzalez, J. Antonio Fernandez Madrigal, 2006
- [2] Recognizing places using spectrally clustered local matches - E. Olson, 2009
Specifications
- Map type: 2D
- MRPT rawlog format: #1, #2
- Graph Type: CPosePDFGaussianInf
- Observations: CObservation2DRangeScan
- Edge Registration Strategy: ICP Scan-matching between nearby nodes
- Loop Registration Strategy: Pair-wise Consistency of ICP Edges
Loop Closing Strategy
The Loop closure registration strategy is described below:
- We split the graph under-construction into groups of nodes. The groups are formatted based on the observations gathered in each node position. The actual split between the groups is decided by the minimum normalized Cut (minNcut) as described in [1].
- Having assembled the groups of nodes, we find the groups that might contain loop closure edges (these groups contain successive nodes with large difference in their IDs). These groups are then split into two subgroups A, B, with the former containing the lower NodeIDs and the latter the higher. To minimize computational cost as well as the possibility of wrong loop closure registration, we search for loop closures only between the last nodes of group B and the first nodes of group A. Based on [2] the potential loop closure edges are not evaluated individually but rather in sets. Refer to [2] for insight on the algorithm and to evaluatePartitionsForLC method for the actual implementation. See below for images of potential Loop closure edges.
- Note
- Olson uses the following formula for evaluating the pairwise consistency between two hypotheses i,j:
-
Where:
- T is the rigid body transformation using the two hypotheses and the two Dijkstra Links connecting the nodes that form the hypotheses
is the covariance matrix of the aforementioned rigid body transformation
-
However this formula is inconsistent with the rest of the paper explanations and mathematical formulas :
- The author states that:
"This quantity is proportional to the probability density that the rigid-body transformation is the identity matrix (i.e., T = [0 0 0])"
This is inconsistent with the given formula. Suppose that a wrong loop closure is included in the
, therefore the pairwise-consistency element should have a low value. For this to hold true the exponent of the consistency element shoud be small and neglecting the covariance matrix of rigid-body transformation (e.g. unit covariance matrix), 
should be small. When a wrong loop closure is evaluated the aforementioned quantity increases since the hypotheses do not form a correct loop. Therefore the worse the rigid-body transformation the higher the exponent term, therefore the higher the consistency element - Author uses the information matrix
in the exponential. However in the optimal case (high certainty of two correct loop closure hypotheses) information matrix and rigid body transformation vector T have opposite effects in the exponent term:
- The author states that:
-
Based on the previous comments the following formula is used in the decider:
![$ \text{Correct loop closure} \Rightarrow T \rightarrow [0, 0, 0] \Rightarrow \text{exponent} \downarrow $](form_112.png)
.ini Configuration Parameters
The configuration parameters of the current decider class are listed below. These parameters are parsed from an external .ini configuration file from their respective section. If an optional parameter is not found a default value is used instead. If a mandatory/required parameter is not found a runtime error is raised. For a sample configuration file see $mrpt/share/mrpt/config_files/graphslam-engine/laser_odometry.ini (Modify $mrpt according to the path of your mrpt source directory).- class_verbosity
- Section : EdgeRegistrationDeciderParameters
- Default value : 1 (LVL_INFO)
- Required : FALSE
- use_scan_matching
- Section : EdgeRegistrationDeciderParameters
- Default value : TRUE
- Required : FALSE
- Description :Indicates whether the decider uses scan matching between the current and previous laser scans to correct the robot trajectory.
- visualize_laser_scans
- Section : VisualizationParameters
- Default value : 10
- Required : FALSE
- LC_min_nodeid_diff
- Section : GeneralConfiguration
- Default value : 30
- Required : FALSE
- Description : Minimum NodeID difference for an edge to be considered a loop closure.
- LC_min_remote_nodes
- Section : EdgeRegistrationDeciderParameters
- Default value : 3
- Required : FALSE
- Description : Number of remote nodes that must exist in order for the Loop Closure procedure to commence
- LC_eigenvalues_ratio_thresh
- Section : EdgeRegistrationDeciderParameters
- Default value : 2
- Required : FALSE
- Description : Minimum ratio of the two dominant eigenvalues for a loop closing hypotheses set to be considered valid
- LC_check_curr_partition_only
- Section : EdgeRegistrationDeciderParameters
- Default value : TRUE
- Required : FALSE
- Description : Boolean flag indicating whether to check for loop closures only in the current node's partition
- visualize_map_partitions
- Section : VisualizationParameters
- Default value : TRUE
- Required : FALSE
- Note
- Class contains an instance of the mrpt::slam::CIncrementalMapPartitioner class and it parses the configuration parameters of the latter from the "EdgeRegistrationDeciderParameters" section. Refer to mrpt::slam::CIncrementalMapPartitioner documentation for its list of configuration parameters
Definition at line 235 of file CLoopCloserERD.h.
#include <mrpt/graphslam/ERD/CLoopCloserERD.h>
Classes | |
| struct | TGenerateHypotsPoolAdParams |
| Struct for passing additional parameters to the generateHypotsPool call. More... | |
| struct | TGetICPEdgeAdParams |
| Struct for passing additional parameters to the getICPEdge call. More... | |
| struct | TLaserParams |
| Struct for storing together the parameters needed for ICP matching, laser scans visualization etc. More... | |
| struct | TLoopClosureParams |
| Struct for storing together the loop-closing related parameters. More... | |
Public Types | |
| typedef CRangeScanEdgeRegistrationDecider< GRAPH_T > | parent_t |
| Edge Registration Decider. More... | |
| typedef GRAPH_T::constraint_t | constraint_t |
| Handy typedefs. More... | |
| typedef GRAPH_T::constraint_t::type_value | pose_t |
| type of underlying poses (2D/3D). More... | |
| typedef GRAPH_T::global_pose_t | global_pose_t |
| typedef CLoopCloserERD< GRAPH_T > | decider_t |
| self type - Handy typedef More... | |
| typedef parent_t::range_ops_t | range_ops_t |
| typedef parent_t::nodes_to_scans2D_t | nodes_to_scans2D_t |
| typedef std::vector< mrpt::vector_uint > | partitions_t |
| Typedef for referring to a list of partitions. More... | |
| typedef GRAPH_T::edges_map_t::const_iterator | edges_citerator |
| typedef GRAPH_T::edges_map_t::iterator | edges_iterator |
| typedef mrpt::graphs::detail::THypothesis< GRAPH_T > | hypot_t |
| typedef std::vector< hypot_t > | hypots_t |
| typedef std::vector< hypot_t * > | hypotsp_t |
| typedef std::map< std::pair< hypot_t *, hypot_t * >, double > | hypotsp_to_consist_t |
| typedef mrpt::graphslam::TUncertaintyPath< GRAPH_T > | path_t |
| typedef std::vector< path_t > | paths_t |
| typedef mrpt::graphslam::detail::TNodeProps< GRAPH_T > | node_props_t |
| typedef mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T > | parent |
| Handy typedefs. More... | |
Public Member Functions | |
| CLoopCloserERD () | |
| virtual | ~CLoopCloserERD () |
| virtual bool | updateState (mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation) |
| Generic method for fetching the incremental action/observation readings from the calling function. More... | |
| void | setWindowManagerPtr (mrpt::graphslam::CWindowManager *win_manager) |
| Fetch a CWindowManager pointer. More... | |
| void | notifyOfWindowEvents (const std::map< std::string, bool > &events_occurred) |
| Get a list of the window events that happened since the last call. More... | |
| void | getEdgesStats (std::map< std::string, int > *edge_types_to_num) const |
| Fill the given map with the type of registered edges as well as the corresponding number of registration of each edge. More... | |
| void | initializeVisuals () |
| Initialize visual objects in CDisplayWindow (e.g. More... | |
| void | updateVisuals () |
| Update the relevant visual features in CDisplayWindow. More... | |
| void | loadParams (const std::string &source_fname) |
| Fetch the latest observation that the current instance received (most probably during a call to the updateState method. More... | |
| void | printParams () const |
| Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact way. More... | |
| void | getDescriptiveReport (std::string *report_str) const |
| Fill the provided string with a detailed report of the decider/optimizer state. More... | |
| void | getCurrPartitions (partitions_t *partitions_out) const |
| const partitions_t & | getCurrPartitions () const |
| size_t | getDijkstraExecutionThresh () const |
| Return the minimum number of nodes that should exist in the graph prior to running Dijkstra. More... | |
| void | setDijkstraExecutionThresh (size_t new_thresh) |
| void | generateHypotsPool (const vector_uint &groupA, const vector_uint &groupB, hypotsp_t *generated_hypots, const TGenerateHypotsPoolAdParams *ad_params=NULL) |
| Generate the hypothesis pool for all the inter-group constraints between two groups of nodes. More... | |
| void | generatePWConsistenciesMatrix (const vector_uint &groupA, const vector_uint &groupB, const hypotsp_t &hypots_pool, mrpt::math::CMatrixDouble *consist_matrix, const paths_t *groupA_opt_paths=NULL, const paths_t *groupB_opt_paths=NULL) |
| Compute the pair-wise consistencies Matrix. More... | |
| void | evalPWConsistenciesMatrix (const mrpt::math::CMatrixDouble &consist_matrix, const hypotsp_t &hypots_pool, hypotsp_t *valid_hypots) |
| Evalute the consistencies matrix, fill the valid hypotheses. More... | |
| virtual bool | justInsertedLoopClosure () const |
| Used by the caller to query for possible loop closures in the last edge registration procedure. More... | |
| virtual void | setCriticalSectionPtr (mrpt::synch::CCriticalSection *graph_section) |
| Fetch a mrpt::synch::CCriticalSection for locking the GRAPH_T resource. More... | |
| virtual void | setGraphPtr (GRAPH_T *graph) |
| Fetch the graph on which the decider/optimizer will work on. More... | |
| virtual void | initializeLoggers (const std::string &name) |
| Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand. More... | |
| virtual void | setClassName (const std::string &name) |
| bool | isMultiRobotSlamClass () |
| std::string | getClassName () const |
Protected Member Functions | |
| bool | fillNodePropsFromGroupParams (const mrpt::utils::TNodeID &nodeID, const std::map< mrpt::utils::TNodeID, node_props_t > &group_params, node_props_t *node_props) |
| Fill the TNodeProps instance using the parameters from the map. More... | |
| bool | getPropsOfNodeID (const mrpt::utils::TNodeID &nodeID, global_pose_t *pose, mrpt::obs::CObservation2DRangeScanPtr &scan, const node_props_t *node_props=NULL) const |
| Fill the pose and LaserScan for the given nodeID. More... | |
| bool | mahalanobisDistanceOdometryToICPEdge (const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, const constraint_t &rel_edge) |
| brief Compare the suggested ICP edge against the initial node difference. More... | |
| void | registerHypothesis (const hypot_t &h) |
| Wrapper around the registerNewEdge method which accepts a THypothesis object instead. More... | |
| void | registerNewEdge (const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, const constraint_t &rel_edge) |
| Register a new constraint/edge in the current graph. More... | |
| virtual void | fetchNodeIDsForScanMatching (const mrpt::utils::TNodeID &curr_nodeID, std::set< mrpt::utils::TNodeID > *nodes_set) |
| Fetch a list of nodes with regards prior to the given nodeID for which to try and add scan matching edges. More... | |
| virtual void | addScanMatchingEdges (const mrpt::utils::TNodeID &curr_nodeID) |
| Addd ICP constraints from X previous nodeIDs up to the given nodeID. More... | |
| void | initLaserScansVisualization () |
| void | updateLaserScansVisualization () |
| void | toggleLaserScansVisualization () |
| togle the LaserScans visualization on and off More... | |
| void | dumpVisibilityErrorMsg (std::string viz_flag, int sleep_time=500) |
| void | updateMapPartitions (bool full_update=false, bool is_first_time_node_reg=false) |
| Split the currently registered graph nodes into partitions. More... | |
| void | initMapPartitionsVisualization () |
| Initialize the visualization of the map partition objects. More... | |
| void | updateMapPartitionsVisualization () |
| Update the map partitions visualization. More... | |
| void | toggleMapPartitionsVisualization () |
| Toggle the map partitions visualization objects. More... | |
| void | initCurrCovarianceVisualization () |
| void | updateCurrCovarianceVisualization () |
| void | computeCentroidOfNodesVector (const vector_uint &nodes_list, std::pair< double, double > *centroid_coords) const |
| Compute the Centroid of a group of a vector of node positions. More... | |
| void | checkPartitionsForLC (partitions_t *partitions_for_LC) |
| Check the registered so far partitions for potential loop closures. More... | |
| void | evaluatePartitionsForLC (const partitions_t &partitions) |
| Evaluate the given partitions for loop closures. More... | |
| bool | computeDominantEigenVector (const mrpt::math::CMatrixDouble &consist_matrix, mrpt::math::dynamic_vector< double > *eigvec, bool use_power_method=false) |
| double | generatePWConsistencyElement (const mrpt::utils::TNodeID &a1, const mrpt::utils::TNodeID &a2, const mrpt::utils::TNodeID &b1, const mrpt::utils::TNodeID &b2, const hypotsp_t &hypots, const paths_t *opt_paths=NULL) |
| Return the pair-wise consistency between the observations of the given nodes. More... | |
| virtual bool | getICPEdge (const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, constraint_t *rel_edge, mrpt::slam::CICP::TReturnInfo *icp_info=NULL, const TGetICPEdgeAdParams *ad_params=NULL) |
| Get the ICP Edge between the provided nodes. More... | |
| void | execDijkstraProjection (mrpt::utils::TNodeID starting_node=0, mrpt::utils::TNodeID ending_node=INVALID_NODEID) |
| compute the minimum uncertainty of each node position with regards to the graph root. More... | |
| void | getMinUncertaintyPath (const mrpt::utils::TNodeID from, const mrpt::utils::TNodeID to, path_t *path) const |
| Given two nodeIDs compute and return the path connecting them. More... | |
| mrpt::graphslam::TUncertaintyPath< GRAPH_T > * | popMinUncertaintyPath (std::set< path_t *> *pool_of_paths) const |
| Find the minimum uncertainty path from te given pool of TUncertaintyPath instances. More... | |
| void | addToPaths (std::set< path_t *> *pool_of_paths, const path_t &curr_path, const std::set< mrpt::utils::TNodeID > &neibors) const |
| Append the paths starting from the current node. More... | |
| mrpt::graphslam::TUncertaintyPath< GRAPH_T > * | queryOptimalPath (const mrpt::utils::TNodeID node) const |
| Query for the optimal path of a nodeID. More... | |
| void | splitPartitionToGroups (vector_uint &partition, vector_uint *groupA, vector_uint *groupB, int max_nodes_in_group=5) |
| Split an existing partition to Groups. More... | |
| void | setLastLaserScan2D (mrpt::obs::CObservation2DRangeScanPtr scan) |
| Assign the last recorded 2D Laser scan. More... | |
| virtual void | assertVisualsVars () |
| Handy function for making all the visuals assertions in a compact manner. More... | |
| void | getICPEdge (const mrpt::obs::CObservation2DRangeScan &from, const mrpt::obs::CObservation2DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=NULL, mrpt::slam::CICP::TReturnInfo *icp_info=NULL) |
| Align the 2D range scans provided and fill the potential edge that can transform the one into the other. More... | |
| void | getICPEdge (const mrpt::obs::CObservation3DRangeScan &from, const mrpt::obs::CObservation3DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=NULL, mrpt::slam::CICP::TReturnInfo *icp_info=NULL) |
| Align the 3D range scans provided and find the potential edge that can transform the one into the other. More... | |
| void | decimatePointsMap (mrpt::maps::CPointsMap *m, size_t keep_point_every=4, size_t low_lim=0) |
| Reduce the size of the given CPointsMap by keeping one out of "keep_point_every" points. More... | |
| bool | convert3DTo2DRangeScan (mrpt::obs::CObservation3DRangeScanPtr &scan3D_in, mrpt::obs::CObservation2DRangeScanPtr *scan2D_out=NULL) |
| Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method. More... | |
Registration criteria checks | |
Check whether a new edge should be registered in the graph. If condition(s) for edge registration is satisfied, method should call the registerNewEdge method. | |
| virtual void | checkRegistrationCondition (mrpt::utils::TNodeID from, mrpt::utils::TNodeID to) |
| virtual void | checkRegistrationCondition (const std::set< mrpt::utils::TNodeID > &) |
Static Protected Member Functions | |
| static hypot_t * | findHypotByEnds (const hypotsp_t &vec_hypots, const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to, bool throw_exc=true) |
| Given a vector of THypothesis objects, find the one that has the given start and end nodes. More... | |
| static const path_t * | findPathByEnds (const paths_t &vec_paths, const mrpt::utils::TNodeID &src, const mrpt::utils::TNodeID &dst, bool throw_exc=true) |
| Given a vector of TUncertaintyPath objects, find the one that has the given source and destination nodeIDs. More... | |
| static hypot_t * | findHypotByID (const hypotsp_t &vec_hypots, const size_t &id, bool throw_exc=true) |
| Given a vector of THypothesis objects, find the one that has the given ID. More... | |
Protected Attributes | |
| TLaserParams | m_laser_params |
| TLoopClosureParams | m_lc_params |
| mrpt::slam::CIncrementalMapPartitioner | m_partitioner |
| Instance responsible for partitioning the map. More... | |
| bool | m_visualize_curr_node_covariance |
| const mrpt::utils::TColor | m_curr_node_covariance_color |
| double | m_offset_y_curr_node_covariance |
| int | m_text_index_curr_node_covariance |
| std::map< std::string, int > | m_edge_types_to_nums |
| Keep track of the registered edge types. More... | |
| mrpt::obs::CObservation2DRangeScanPtr | m_last_laser_scan2D |
| Keep the last laser scan for visualization purposes. More... | |
| bool | m_partitions_full_update |
| Indicate whether the partitions have been updated recently. More... | |
| std::map< int, vector_uint > | m_partitionID_to_prev_nodes_list |
| Keep track of the evaluated partitions so they are not checked again if nothing changed in them. More... | |
| std::map< mrpt::utils::TNodeID, path_t *> | m_node_optimal_paths |
| Map that stores the lowest uncertainty path towards a node. More... | |
| mrpt::obs::CObservation2DRangeScanPtr | m_first_laser_scan |
| Keep track of the first recorded laser scan so that it can be assigned to the root node when the NRD adds the first two nodes to the graph. More... | |
| bool | m_is_first_time_node_reg |
| Track the first node registration occurance. More... | |
| size_t | m_dijkstra_node_count_thresh |
| Node Count lower bound before executing dijkstra. More... | |
| double | m_consec_icp_constraint_factor |
| Factor used for accepting an ICP Constraint as valid. More... | |
| double | m_lc_icp_constraint_factor |
| Factor used for accepting an ICP Constraint in the loop closure proc. More... | |
| nodes_to_scans2D_t | m_nodes_to_laser_scans2D |
| Map for keeping track of the observation recorded at each graph position. More... | |
| size_t | m_last_total_num_nodes |
| Keep track of the total number of registered nodes since the last time class method was called. More... | |
| bool | m_just_inserted_lc |
| bool | m_override_registered_nodes_check |
| Indicates whether the ERD implementation expects, at most one single node to be registered, between successive calls to the updateState method. More... | |
| GRAPH_T * | m_graph |
| Pointer to the graph that is under construction. More... | |
| mrpt::synch::CCriticalSection * | m_graph_section |
| mrpt::utils::CTimeLogger | m_time_logger |
| Time logger instance. More... | |
| std::string | m_class_name |
| Name of the class instance. More... | |
| bool | is_mr_slam_class |
| Boolean indicating if the current class can be used in multi-robot SLAM operations. More... | |
| TParams | params |
Partition vectors | |
| partitions_t | m_last_partitions |
| Previous partitions vector. More... | |
| partitions_t | m_curr_partitions |
| Current partitions vector. More... | |
Visuals-related variables methods | |
| mrpt::graphslam::CWindowManager * | m_win_manager |
| Pointer to the CWindowManager object used to store visuals-related instances. More... | |
| mrpt::gui::CDisplayWindow3D * | m_win |
| Window to use. More... | |
| mrpt::graphslam::CWindowObserver * | m_win_observer |
| CWindowObserver object for monitoring various visual-oriented events. More... | |
| bool | m_initialized_visuals |
Static Protected Attributes | |
| static const std::string | header_sep = std::string(80, '-') |
| Separator string to be used in debugging messages. More... | |
| static const std::string | report_sep = std::string(2, '\n') |
Member Typedef Documentation
◆ constraint_t
| typedef GRAPH_T::constraint_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::constraint_t |
◆ decider_t
| typedef CLoopCloserERD<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::decider_t |
self type - Handy typedef
Definition at line 249 of file CLoopCloserERD.h.
◆ edges_citerator
| typedef GRAPH_T::edges_map_t::const_iterator mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::edges_citerator |
Definition at line 254 of file CLoopCloserERD.h.
◆ edges_iterator
| typedef GRAPH_T::edges_map_t::iterator mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::edges_iterator |
Definition at line 255 of file CLoopCloserERD.h.
◆ global_pose_t
| typedef GRAPH_T::global_pose_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::global_pose_t |
Definition at line 248 of file CLoopCloserERD.h.
◆ hypot_t
| typedef mrpt::graphs::detail::THypothesis<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypot_t |
Definition at line 256 of file CLoopCloserERD.h.
◆ hypots_t
| typedef std::vector<hypot_t> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypots_t |
Definition at line 257 of file CLoopCloserERD.h.
◆ hypotsp_t
| typedef std::vector<hypot_t*> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypotsp_t |
Definition at line 258 of file CLoopCloserERD.h.
◆ hypotsp_to_consist_t
| typedef std::map< std::pair<hypot_t*, hypot_t*>, double > mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypotsp_to_consist_t |
Definition at line 259 of file CLoopCloserERD.h.
◆ node_props_t
| typedef mrpt::graphslam::detail::TNodeProps<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::node_props_t |
Definition at line 262 of file CLoopCloserERD.h.
◆ nodes_to_scans2D_t
| typedef parent_t::nodes_to_scans2D_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::nodes_to_scans2D_t |
Definition at line 251 of file CLoopCloserERD.h.
◆ parent
|
inherited |
◆ parent_t
| typedef CRangeScanEdgeRegistrationDecider<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::parent_t |
Edge Registration Decider.
Definition at line 240 of file CLoopCloserERD.h.
◆ partitions_t
| typedef std::vector<mrpt::vector_uint> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::partitions_t |
Typedef for referring to a list of partitions.
Definition at line 253 of file CLoopCloserERD.h.
◆ path_t
| typedef mrpt::graphslam::TUncertaintyPath<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::path_t |
Definition at line 260 of file CLoopCloserERD.h.
◆ paths_t
| typedef std::vector<path_t> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::paths_t |
Definition at line 261 of file CLoopCloserERD.h.
◆ pose_t
| typedef GRAPH_T::constraint_t::type_value mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::pose_t |
type of underlying poses (2D/3D).
Definition at line 247 of file CLoopCloserERD.h.
◆ range_ops_t
| typedef parent_t::range_ops_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::range_ops_t |
Definition at line 250 of file CLoopCloserERD.h.
Constructor & Destructor Documentation
◆ CLoopCloserERD()
| mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::CLoopCloserERD | ( | ) |
Definition at line 19 of file CLoopCloserERD_impl.h.
References mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >::initializeLoggers(), mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_edge_types_to_nums, and MRPT_LOG_DEBUG_STREAM.
◆ ~CLoopCloserERD()
|
virtual |
Definition at line 33 of file CLoopCloserERD_impl.h.
References MRPT_LOG_DEBUG_STREAM.
Member Function Documentation
◆ addScanMatchingEdges()
|
protectedvirtual |
Addd ICP constraints from X previous nodeIDs up to the given nodeID.
X is set by the user in the .ini configuration file (see TLaserParams::prev_nodes_for_ICP)
- See also
- fetchNodeIDsForScanMatching
Definition at line 163 of file CLoopCloserERD_impl.h.
References mrpt::slam::CICP::TReturnInfo::goodness, mrpt::math::isNaN(), MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.
◆ addToPaths()
|
protected |
Append the paths starting from the current node.
- Parameters
-
[in] pool_of_paths Paths that are currently registered [in] curr_path Path that I am currently traversing. This path is already removed from pool_of_paths [in] neighbors std::set of neighboring nodes to the last node of the current path
Definition at line 1344 of file CLoopCloserERD_impl.h.
References mrpt::graphslam::TUncertaintyPath< GRAPH_T >::getDestination(), MRPT_END, MRPT_START, and mrpt::graphslam::TUncertaintyPath< GRAPH_T >::nodes_traversed.
◆ assertVisualsVars()
|
protectedvirtualinherited |
Handy function for making all the visuals assertions in a compact manner.
Definition at line 87 of file CRegistrationDeciderOrOptimizer_impl.h.
◆ checkPartitionsForLC()
|
protected |
Check the registered so far partitions for potential loop closures.
Practically checks whether there exist nodes in a single partition whose distance surpasses the minimum loop closure nodeID distance. The latter is read from a .ini external file, thus specified by the user (see TLoopClosureParams.LC_min_nodeid_diff.
- See also
- evaluatePartitionsForLC
Definition at line 382 of file CLoopCloserERD_impl.h.
References ASSERT_, mrpt::utils::find(), mrpt::utils::getSTLContainerAsString(), MRPT_END, MRPT_LOG_WARN_STREAM, and MRPT_START.
◆ checkRegistrationCondition() [1/2]
|
inlineprotectedvirtualinherited |
Definition at line 92 of file CEdgeRegistrationDecider.h.
◆ checkRegistrationCondition() [2/2]
|
inlineprotectedvirtualinherited |
Definition at line 95 of file CEdgeRegistrationDecider.h.
◆ computeCentroidOfNodesVector()
|
protected |
Compute the Centroid of a group of a vector of node positions.
- Parameters
-
[in] nodes_list List of node IDs whose positions are taken into account [out] centroid_coords Contains the Centroid coordinates as a pair [x,y]
- Note
- Method is used during the visualization of the map partitions.
Definition at line 1885 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
◆ computeDominantEigenVector()
|
protected |
limit =
Definition at line 812 of file CLoopCloserERD_impl.h.
References mrpt::math::approximatelyEqual(), ASSERT_, ASSERTMSG_, mrpt::mrpt::format(), MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_ERROR_STREAM, MRPT_START, and THROW_EXCEPTION.
◆ convert3DTo2DRangeScan()
|
protectedinherited |
Wrapper around the CObservation3DRangeScan::convertTo2DScan corresponding method.
- Returns
- True if operation was successful, false otherwise
Definition at line 139 of file CRangeScanOps_impl.h.
References mrpt::obs::CObservation2DRangeScan::Create(), MRPT_END, and MRPT_START.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateState().
◆ decimatePointsMap()
|
protectedinherited |
Reduce the size of the given CPointsMap by keeping one out of "keep_point_every" points.
- Note
- If low_lim is set then the PointsMap will contain at least low_lim measurements, regardless of keep_point_every value. Set low_lim to 0 if no lower limit is to be specified
Definition at line 110 of file CRangeScanOps_impl.h.
References mrpt::maps::CPointsMap::applyDeletionMask(), min, MRPT_END, MRPT_START, and mrpt::maps::CPointsMap::size().
◆ dumpVisibilityErrorMsg()
|
protected |
Definition at line 2128 of file CLoopCloserERD_impl.h.
References MRPT_END, MRPT_START, and mrpt::system::sleep().
◆ evalPWConsistenciesMatrix()
| void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::evalPWConsistenciesMatrix | ( | const mrpt::math::CMatrixDouble & | consist_matrix, |
| const hypotsp_t & | hypots_pool, | ||
| hypotsp_t * | valid_hypots | ||
| ) |
Evalute the consistencies matrix, fill the valid hypotheses.
Call to this method should be made right after generating the consistencies matrix using the generatePWConsistenciesMatrix method
- See also
- generatePWConsistenciesMatrix
Definition at line 546 of file CLoopCloserERD_impl.h.
References ASSERT_, mrpt::mrpt::format(), MRPT_END, and MRPT_START.
◆ evaluatePartitionsForLC()
|
protected |
Evaluate the given partitions for loop closures.
Call this method when you have identified potential loop closures - e.g. far away nodes in the same partitions - and you want to evaluate the potential hypotheses in the group. Comprises the main function that tests potential loop closures in partitions of nodes
- See also
- checkPartitionsForLC
Definition at line 479 of file CLoopCloserERD_impl.h.
References MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_WARN_STREAM, and MRPT_START.
◆ execDijkstraProjection()
|
protected |
compute the minimum uncertainty of each node position with regards to the graph root.
- Parameters
-
[in] starting_node Node from which I start the Dijkstra projection algorithm [in] ending_node Specify the nodeID whose uncertainty wrt the starting_node, we are interested in computing. If given, method execution ends when this path is computed.
Definition at line 1209 of file CLoopCloserERD_impl.h.
References ASSERT_, ASSERTMSG_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::getDestination(), INVALID_NODEID, MRPT_END, and MRPT_START.
◆ fetchNodeIDsForScanMatching()
|
protectedvirtual |
Fetch a list of nodes with regards prior to the given nodeID for which to try and add scan matching edges.
- See also
- addScanMatchingEdges
Definition at line 145 of file CLoopCloserERD_impl.h.
References ASSERT_.
◆ fillNodePropsFromGroupParams()
|
protected |
Fill the TNodeProps instance using the parameters from the map.
- Parameters
-
[in] nodeID ID of node corresponding to the TNodeProps struct that is to be filled [in] group_params Map of TNodeID to corresponding TNodeProps instance. [out] node_props Pointer to the TNodeProps struct to be filled.
- Returns
- True if operation was successful, false otherwise.
Definition at line 295 of file CLoopCloserERD_impl.h.
References ASSERT_.
◆ findHypotByEnds()
|
staticprotected |
Given a vector of THypothesis objects, find the one that has the given start and end nodes.
- Note
- If multiple hypothesis between the same start and end node exist, only the first one is returned.
- Parameters
-
[in] vec_hypots Vector of hypothesis to check [in] from Starting Node for hypothesis [in] to Ending Node for hypothesis [in] throw_exc If true and hypothesis is not found, throw a HypothesisNotFoundException
- Returns
- Pointer to the found hypothesis if that is found, otherwise NULL.
Definition at line 1152 of file CLoopCloserERD_impl.h.
◆ findHypotByID()
|
staticprotected |
Given a vector of THypothesis objects, find the one that has the given ID.
- Note
- If multiple hypothesis with the same ID exist, only the first one is returned.
- Parameters
-
[in] vec_hypots Vector of hypothesis to check [in] id,ID of the hypothesis to be returned [in] throw_exc If true and hypothesis is not found, throw a HypothesisNotFoundException
- Returns
- Pointer to the hypothesis with the given ID if that is found, otherwies NULL.
Definition at line 1183 of file CLoopCloserERD_impl.h.
◆ findPathByEnds()
|
staticprotected |
Given a vector of TUncertaintyPath objects, find the one that has the given source and destination nodeIDs.
- Note
- If multiple paths between the same start and end node exist, only the first one is returned.
- Returns
- NULL if a path with the given source and destination NodeIDs is not found, otherwise a pointer to the matching TUncertaintyPath.
- Exceptions
-
std::runtime_error if path was not found and throw_exc is set to true
Definition at line 1120 of file CLoopCloserERD_impl.h.
References ASSERT_, mrpt::format(), and THROW_EXCEPTION.
◆ generateHypotsPool()
| void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::generateHypotsPool | ( | const vector_uint & | groupA, |
| const vector_uint & | groupB, | ||
| hypotsp_t * | generated_hypots, | ||
| const TGenerateHypotsPoolAdParams * | ad_params = NULL |
||
| ) |
Generate the hypothesis pool for all the inter-group constraints between two groups of nodes.
- Parameters
-
[in] groupA First group to be tested [in] groupB Second group to be tested [out] generated_hypots Pool of generated hypothesis. Hypotheses are generated in the heap, so the caller is responsible of afterwards calling delete.
Definition at line 672 of file CLoopCloserERD_impl.h.
References ASSERTMSG_, mrpt::format(), mrpt::graphs::detail::THypothesis< GRAPH_T >::from, mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGetICPEdgeAdParams::from_params, mrpt::graphs::detail::THypothesis< GRAPH_T >::getAsString(), mrpt::utils::getSTLContainerAsString(), mrpt::graphs::detail::THypothesis< GRAPH_T >::goodness, mrpt::slam::CICP::TReturnInfo::goodness, mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGenerateHypotsPoolAdParams::groupA_params, mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGenerateHypotsPoolAdParams::groupB_params, mrpt::graphs::detail::THypothesis< GRAPH_T >::id, mrpt::graphs::detail::THypothesis< GRAPH_T >::is_valid, MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_START, mrpt::graphs::detail::THypothesis< GRAPH_T >::setEdge(), mrpt::graphs::detail::THypothesis< GRAPH_T >::to, and mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGetICPEdgeAdParams::to_params.
◆ generatePWConsistenciesMatrix()
| void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::generatePWConsistenciesMatrix | ( | const vector_uint & | groupA, |
| const vector_uint & | groupB, | ||
| const hypotsp_t & | hypots_pool, | ||
| mrpt::math::CMatrixDouble * | consist_matrix, | ||
| const paths_t * | groupA_opt_paths = NULL, |
||
| const paths_t * | groupB_opt_paths = NULL |
||
| ) |
Compute the pair-wise consistencies Matrix.
- Parameters
-
[in] groupA First group to be used [in] groupB Second group to be used [in] hypots_pool Pool of hypothesis that has been generated between the two groups [out] consist_matrix Pointer to Pair-wise consistencies matrix that is to be filled [in] groupA_opt_paths Pointer to vector of optimal paths that can be used instead of making queries to the m_node_optimal_paths class vector. See corresponding argument in generatePWConsistencyElement method [in] groupB_opt_paths
Definition at line 881 of file CLoopCloserERD_impl.h.
References mrpt::obs::gnss::a1, mrpt::obs::gnss::a2, ASSERTMSG_, mrpt::obs::gnss::b1, mrpt::obs::gnss::b2, mrpt::utils::getSTLContainerAsString(), mrpt::graphs::detail::THypothesis< GRAPH_T >::id, mrpt::graphs::detail::THypothesis< GRAPH_T >::is_valid, MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.
◆ generatePWConsistencyElement()
|
protected |
Return the pair-wise consistency between the observations of the given nodes.
For the tranformation matrix of the loop use the following edges
- a1=>a2 (Dijkstra Link)
- a2=>b1 (hypothesis - ICP edge)
- b1=>b2 (Dijkstra Link)
- b2=>a1 (hypothesis - ICP edge)
Given the transformation vector 
of the above composition (e.g. T) the pairwise consistency element would then be:
- Parameters
-
[in] hypots Hypothesis corresponding to the potential inter-group constraints [in] opt_paths Vector of optimal paths that can be used instead of making queries to the m_node_optimal_paths class vector. See corresponding argument in generatePWConsistenciesMatrix method - 1st element a1->a2 path
- 2nd element b1->b2 path
- Returns
- Pairwise consistency eleement of the composition of transformations
- See also
- generatePWConsistenciesMatrix
Definition at line 1010 of file CLoopCloserERD_impl.h.
References mrpt::obs::gnss::a1, mrpt::obs::gnss::a2, ASSERT_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::assertIsBetweenNodeIDs(), mrpt::obs::gnss::b1, mrpt::obs::gnss::b2, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::curr_pose_pdf, mrpt::graphs::detail::THypothesis< GRAPH_T >::getEdge(), mrpt::graphs::detail::THypothesis< GRAPH_T >::getInverseEdge(), mrpt::graphs::detail::THypothesis< GRAPH_T >::id, MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.
◆ getClassName()
|
inlineinherited |
Definition at line 138 of file CRegistrationDeciderOrOptimizer.h.
◆ getCurrPartitions() [1/2]
| void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getCurrPartitions | ( | partitions_t * | partitions_out | ) | const |
Definition at line 2228 of file CLoopCloserERD_impl.h.
References ASSERT_.
◆ getCurrPartitions() [2/2]
| const std::vector< mrpt::vector_uint > & mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getCurrPartitions | ( | ) | const |
Definition at line 2235 of file CLoopCloserERD_impl.h.
◆ getDescriptiveReport()
|
virtual |
Fill the provided string with a detailed report of the decider/optimizer state.
Report should include (part of) the following:
- Timing of important methods
- Properties fo class at the current time
- Logging of commands until current time
Reimplemented from mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T >.
Definition at line 2197 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
◆ getDijkstraExecutionThresh()
|
inline |
Return the minimum number of nodes that should exist in the graph prior to running Dijkstra.
Definition at line 292 of file CLoopCloserERD.h.
References mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_dijkstra_node_count_thresh.
◆ getEdgesStats()
|
virtual |
Fill the given map with the type of registered edges as well as the corresponding number of registration of each edge.
Reimplemented from mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T >.
Definition at line 2003 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
◆ getICPEdge() [1/3]
|
protectedinherited |
Align the 2D range scans provided and fill the potential edge that can transform the one into the other.
User can optionally ask that additional information be returned in a TReturnInfo struct
Definition at line 16 of file CRangeScanOps_impl.h.
References info, mrpt::maps::CMetricMap::insertObservation(), MRPT_END, and MRPT_START.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition2D(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::checkRegistrationCondition2D(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition3D().
◆ getICPEdge() [2/3]
|
protectedinherited |
Align the 3D range scans provided and find the potential edge that can transform the one into the other.
Fills the 2D part (rel_edge) of the 3D constraint between the scans, since we are interested in computing the 2D alignment. User can optionally ask that additional information be returned in a TReturnInfo struct
Definition at line 54 of file CRangeScanOps_impl.h.
References ASSERTMSG_, mrpt::mrpt::format(), mrpt::obs::CObservation3DRangeScan::hasRangeImage, info, mrpt::maps::CMetricMap::insertObservation(), MRPT_END, and MRPT_START.
◆ getICPEdge() [3/3]
|
protectedvirtual |
Get the ICP Edge between the provided nodes.
Handy for not having to manually fetch the laser scans, as the method takes care of this.
- Parameters
-
[out] icp_info Struct that will be filled with the results of the ICP operation [in] ad_params Pointer to additional parameters in the getICPEdge call
- Returns
- True if operation was successful, false otherwise (e.g. if the either of the nodes' CObservation2DRangeScan object does not contain valid data.
Definition at line 223 of file CLoopCloserERD_impl.h.
References ASSERT_, mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGetICPEdgeAdParams::from_params, mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGetICPEdgeAdParams::getAsString(), mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGetICPEdgeAdParams::init_estim, MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_START, and mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGetICPEdgeAdParams::to_params.
◆ getMinUncertaintyPath()
|
protected |
Given two nodeIDs compute and return the path connecting them.
Method takes care of multiple edges, as well as edges with 0 covariance matrices
Definition at line 1399 of file CLoopCloserERD_impl.h.
References mrpt::graphslam::TUncertaintyPath< GRAPH_T >::addToPath(), ASSERT_, ASSERTMSG_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::clear(), mrpt::mrpt::format(), mrpt::graphslam::TUncertaintyPath< GRAPH_T >::getDeterminant(), mrpt::pbmap::inverse(), mrpt::math::isNaN(), MRPT_END, and MRPT_START.
◆ getPropsOfNodeID()
|
protected |
Fill the pose and LaserScan for the given nodeID.
Pose and LaserScan are either fetched from the TNodeProps struct if it contains valid data, otherwise from the corresponding class vars
- Returns
- True if operation was successful and pose, scan contain valid data.
Definition at line 324 of file CLoopCloserERD_impl.h.
References ASSERT_, ASSERTMSG_, mrpt::format(), MRPT_LOG_WARN_STREAM, mrpt::graphslam::detail::TNodeProps< GRAPH_T >::pose, and mrpt::graphslam::detail::TNodeProps< GRAPH_T >::scan.
◆ initCurrCovarianceVisualization()
|
protected |
Definition at line 2055 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
◆ initializeLoggers()
|
virtualinherited |
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand.
Definition at line 38 of file CRegistrationDeciderOrOptimizer_impl.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::CICPCriteriaERD(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::CICPCriteriaNRD(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::CLevMarqGSO(), and mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::CLoopCloserERD().
◆ initializeVisuals()
|
virtual |
Initialize visual objects in CDisplayWindow (e.g.
add an object to scene).
- Exceptions
-
std::exception If the method is called without having first provided a CDisplayWindow3D* to the class instance
- See also
- setWindowManagerPtr, updateVisuals
Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.
Definition at line 2011 of file CLoopCloserERD_impl.h.
References ASSERTMSG_, MRPT_END, and MRPT_START.
◆ initLaserScansVisualization()
|
protected |
Definition at line 1910 of file CLoopCloserERD_impl.h.
References mrpt::opengl::CPlanarLaserScan::Create(), MRPT_END, and MRPT_START.
◆ initMapPartitionsVisualization()
|
protected |
Initialize the visualization of the map partition objects.
Definition at line 1647 of file CLoopCloserERD_impl.h.
◆ isMultiRobotSlamClass()
|
inherited |
Definition at line 132 of file CRegistrationDeciderOrOptimizer_impl.h.
◆ justInsertedLoopClosure()
|
inlinevirtualinherited |
Used by the caller to query for possible loop closures in the last edge registration procedure.
Definition at line 79 of file CEdgeRegistrationDecider.h.
◆ loadParams()
|
virtual |
Fetch the latest observation that the current instance received (most probably during a call to the updateState method.
Reimplemented from mrpt::graphslam::deciders::CRangeScanEdgeRegistrationDecider< GRAPH_T >.
Definition at line 2144 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
◆ mahalanobisDistanceOdometryToICPEdge()
|
protected |
brief Compare the suggested ICP edge against the initial node difference.
If this difference is significantly larger than the rest of of the recorded mahalanobis distances, reject the suggested ICP edge.
- Returns
- True if suggested ICP edge is accepted
- Note
- Method updates the Mahalanobis Distance TSlidingWindow which keep track of the recorded mahalanobis distance values.
- See also
- getICPEdge
Definition at line 1530 of file CLoopCloserERD_impl.h.
References mrpt::math::isNaN(), mrpt::math::mahalanobisDistance2(), MRPT_END, and MRPT_START.
◆ notifyOfWindowEvents()
|
virtual |
Get a list of the window events that happened since the last call.
Method in derived classes is automatically called from the CGraphSlamEngine_t instance. After that, decider/optimizer should just fetch the parameters that it is interested in.
Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.
Definition at line 1629 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
◆ popMinUncertaintyPath()
|
protected |
Find the minimum uncertainty path from te given pool of TUncertaintyPath instances.
Removes (and returns) the found path from the pool.
- Returns
- Minimum uncertainty path from the pool provided
Definition at line 1501 of file CLoopCloserERD_impl.h.
References ASSERT_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::getDeterminant(), MRPT_END, and MRPT_START.
◆ printParams()
|
virtual |
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact way.
Reimplemented from mrpt::graphslam::deciders::CRangeScanEdgeRegistrationDecider< GRAPH_T >.
Definition at line 2178 of file CLoopCloserERD_impl.h.
References MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.
◆ queryOptimalPath()
|
protected |
Query for the optimal path of a nodeID.
Method handles calls to out-of-bounds nodes as well as nodes whose paths have not yet been computed.
- Parameters
-
[in] node nodeID for which hte path is going to be returned
- Returns
- Optimal path corresponding to the given nodeID or NULL if the former is not found.
Definition at line 1382 of file CLoopCloserERD_impl.h.
◆ registerHypothesis()
|
protected |
Wrapper around the registerNewEdge method which accepts a THypothesis object instead.
Definition at line 1568 of file CLoopCloserERD_impl.h.
References mrpt::graphs::detail::THypothesis< GRAPH_T >::from, mrpt::graphs::detail::THypothesis< GRAPH_T >::getEdge(), and mrpt::graphs::detail::THypothesis< GRAPH_T >::to.
◆ registerNewEdge()
|
protectedvirtual |
Register a new constraint/edge in the current graph.
Implementations of this class should provide a wrapper around GRAPH_T::insertEdge method.
Reimplemented from mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T >.
Definition at line 1575 of file CLoopCloserERD_impl.h.
References mrpt::math::absDiff(), MRPT_END, and MRPT_START.
◆ setClassName()
|
virtualinherited |
Definition at line 52 of file CRegistrationDeciderOrOptimizer_impl.h.
◆ setCriticalSectionPtr()
|
virtualinherited |
Fetch a mrpt::synch::CCriticalSection for locking the GRAPH_T resource.
Handy for realising multithreading in the derived classes.
- Warning
- Beware that prior to the decider/optimizer public method call, the CCriticalSection will already be locked from CGraphSlamEngine_t instance, but this isn't effective in multithreaded implementations where the decider/optimizer itself has to lock the function at which the extra thread runs.
Definition at line 73 of file CRegistrationDeciderOrOptimizer_impl.h.
◆ setDijkstraExecutionThresh()
|
inline |
Definition at line 295 of file CLoopCloserERD.h.
References mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_dijkstra_node_count_thresh.
◆ setGraphPtr()
|
virtualinherited |
Fetch the graph on which the decider/optimizer will work on.
Definition at line 124 of file CRegistrationDeciderOrOptimizer_impl.h.
◆ setLastLaserScan2D()
|
protected |
Assign the last recorded 2D Laser scan.
- Note
- Compact way of assigning the last recorded laser scan for both MRPT rawlog formats.
Method takes into account the start of graphSLAM proc. when two nodes are added at the graph at the same time (root + node for 1st constraint)
◆ setWindowManagerPtr()
|
virtual |
Fetch a CWindowManager pointer.
CWindowManager instance should contain a CDisplayWindow3D* and, optionally, a CWindowObserver pointer so that interaction with the window is possible
Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.
Definition at line 1605 of file CLoopCloserERD_impl.h.
◆ splitPartitionToGroups()
|
protected |
Split an existing partition to Groups.
Have two groups A, B.
- Group A consists of the lower nodeIDs. They correspond to the start of the course
- Group B consists of the higher (more recent) nodeIDs. They correspond to the end of the course find where to split the current partition
- Note
- Method is used in single-robot graphSLAM for spliting a partition of nodes to lower and higher node IDs
- Parameters
-
[in] partition Partition to be split. [out] groupA First group of nodes. [out] groupB Second group of nodes. [in] max_nodes_in_group Max number of nodes that are to exist in each group (Use -1 to disable this threshold).
Definition at line 613 of file CLoopCloserERD_impl.h.
References ASSERT_, ASSERTMSG_, mrpt::format(), MRPT_END, and MRPT_START.
◆ toggleLaserScansVisualization()
|
protected |
togle the LaserScans visualization on and off
Definition at line 1977 of file CLoopCloserERD_impl.h.
References ASSERTMSG_, MRPT_END, and MRPT_START.
◆ toggleMapPartitionsVisualization()
|
protected |
Toggle the map partitions visualization objects.
To be called upon relevant keystroke press by the user (see TLoopClosureParams::keystroke_map_partitions)
Definition at line 1860 of file CLoopCloserERD_impl.h.
References ASSERTMSG_, MRPT_END, and MRPT_START.
◆ updateCurrCovarianceVisualization()
|
protected |
Definition at line 2088 of file CLoopCloserERD_impl.h.
References ASSERT_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::curr_pose_pdf, MRPT_END, and MRPT_START.
◆ updateLaserScansVisualization()
|
protected |
Definition at line 1940 of file CLoopCloserERD_impl.h.
References ASSERT_, mrpt::mrpt::utils::DEG2RAD(), MRPT_END, and MRPT_START.
◆ updateMapPartitions()
|
protected |
Split the currently registered graph nodes into partitions.
Definition at line 2240 of file CLoopCloserERD_impl.h.
References mrpt::obs::CSensoryFrame::Create(), MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_WARN_STREAM, and MRPT_START.
◆ updateMapPartitionsVisualization()
|
protected |
Update the map partitions visualization.
Definition at line 1672 of file CLoopCloserERD_impl.h.
References ASSERT_, mrpt::utils::find(), mrpt::mrpt::format(), MRPT_LOG_DEBUG_STREAM, and MRPT_LOG_ERROR_STREAM.
◆ updateState()
|
virtual |
Generic method for fetching the incremental action/observation readings from the calling function.
Implementations of this interface should use (part of) the specified parameters and call the checkRegistrationCondition to check for potential Edge registration
Implements mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_T >.
Definition at line 53 of file CLoopCloserERD_impl.h.
References mrpt::math::absDiff(), MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_ERROR_STREAM, MRPT_LOG_WARN_STREAM, MRPT_START, MRPT_UNUSED_PARAM, and THROW_EXCEPTION.
◆ updateVisuals()
|
virtual |
Update the relevant visual features in CDisplayWindow.
- Exceptions
-
std::exception If the method is called without having first provided a CDisplayWindow3D* to the class instance
- See also
- setWindowManagerPtr, initializeVisuals
Reimplemented from mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_T >.
Definition at line 2034 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
Member Data Documentation
◆ header_sep
|
staticprotectedinherited |
Separator string to be used in debugging messages.
Definition at line 175 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::getDescriptiveReport(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::getDescriptiveReport().
◆ is_mr_slam_class
|
protectedinherited |
Boolean indicating if the current class can be used in multi-robot SLAM operations.
Definition at line 171 of file CRegistrationDeciderOrOptimizer.h.
◆ m_class_name
|
protectedinherited |
Name of the class instance.
Definition at line 167 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::CRegistrationDeciderOrOptimizer< GRAPH_t >::getClassName().
◆ m_consec_icp_constraint_factor
|
protected |
Factor used for accepting an ICP Constraint as valid.
Definition at line 879 of file CLoopCloserERD.h.
◆ m_curr_node_covariance_color
|
protected |
Definition at line 835 of file CLoopCloserERD.h.
◆ m_curr_partitions
|
protected |
Current partitions vector.
Definition at line 851 of file CLoopCloserERD.h.
◆ m_dijkstra_node_count_thresh
|
protected |
Node Count lower bound before executing dijkstra.
Definition at line 876 of file CLoopCloserERD.h.
Referenced by mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getDijkstraExecutionThresh(), and mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::setDijkstraExecutionThresh().
◆ m_edge_types_to_nums
|
protected |
Keep track of the registered edge types.
Handy for displaying them in the Visualization window.
Definition at line 843 of file CLoopCloserERD.h.
Referenced by mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::CLoopCloserERD().
◆ m_first_laser_scan
|
protected |
Keep track of the first recorded laser scan so that it can be assigned to the root node when the NRD adds the first two nodes to the graph.
Definition at line 868 of file CLoopCloserERD.h.
◆ m_graph
|
protectedinherited |
Pointer to the graph that is under construction.
Definition at line 146 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::_optimizeGraph(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::checkForLoopClosures(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition2D(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition3D(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::getNearbyNodesOf(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::getNearbyNodesOf(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::registerNewEdge(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateGraphVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateOptDistanceVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateState(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateState(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateVisuals().
◆ m_graph_section
|
protectedinherited |
Definition at line 147 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::optimizeGraph().
◆ m_initialized_visuals
|
protectedinherited |
Definition at line 161 of file CRegistrationDeciderOrOptimizer.h.
◆ m_is_first_time_node_reg
|
protected |
Track the first node registration occurance.
Handy so that we can assign a measurement to the root node as well.
Definition at line 873 of file CLoopCloserERD.h.
◆ m_just_inserted_lc
|
protectedinherited |
Definition at line 108 of file CEdgeRegistrationDecider.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition2D(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition3D(), mrpt::graphslam::deciders::CEdgeRegistrationDecider< GRAPH_t >::justInsertedLoopClosure(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateState().
◆ m_laser_params
|
protected |
Definition at line 532 of file CLoopCloserERD.h.
◆ m_last_laser_scan2D
|
protected |
Keep the last laser scan for visualization purposes.
Definition at line 845 of file CLoopCloserERD.h.
◆ m_last_partitions
|
protected |
Previous partitions vector.
Definition at line 849 of file CLoopCloserERD.h.
◆ m_last_total_num_nodes
|
protectedinherited |
Keep track of the total number of registered nodes since the last time class method was called.
Definition at line 65 of file CRangeScanEdgeRegistrationDecider.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::CICPCriteriaERD(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateState().
◆ m_lc_icp_constraint_factor
|
protected |
Factor used for accepting an ICP Constraint in the loop closure proc.
Definition at line 882 of file CLoopCloserERD.h.
◆ m_lc_params
|
protected |
Definition at line 533 of file CLoopCloserERD.h.
◆ m_node_optimal_paths
|
protected |
Map that stores the lowest uncertainty path towards a node.
Starting node depends on the starting node as used in the execDijkstraProjection method
Definition at line 863 of file CLoopCloserERD.h.
◆ m_nodes_to_laser_scans2D
|
protectedinherited |
Map for keeping track of the observation recorded at each graph position.
Definition at line 61 of file CRangeScanEdgeRegistrationDecider.h.
◆ m_offset_y_curr_node_covariance
|
protected |
Definition at line 836 of file CLoopCloserERD.h.
◆ m_override_registered_nodes_check
|
protectedinherited |
Indicates whether the ERD implementation expects, at most one single node to be registered, between successive calls to the updateState method.
By default set to false.
Definition at line 115 of file CEdgeRegistrationDecider.h.
◆ m_partitioner
|
protected |
Instance responsible for partitioning the map.
Definition at line 832 of file CLoopCloserERD.h.
◆ m_partitionID_to_prev_nodes_list
|
protected |
Keep track of the evaluated partitions so they are not checked again if nothing changed in them.
Definition at line 858 of file CLoopCloserERD.h.
◆ m_partitions_full_update
|
protected |
Indicate whether the partitions have been updated recently.
Definition at line 854 of file CLoopCloserERD.h.
◆ m_text_index_curr_node_covariance
|
protected |
Definition at line 837 of file CLoopCloserERD.h.
◆ m_time_logger
|
protectedinherited |
Time logger instance.
Definition at line 165 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::_optimizeGraph(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition2D(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::checkRegistrationCondition3D(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::getDescriptiveReport(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::getDescriptiveReport(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::initializeVisuals(), mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::updateState(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateVisuals().
◆ m_visualize_curr_node_covariance
|
protected |
Definition at line 834 of file CLoopCloserERD.h.
◆ m_win
|
protectedinherited |
Window to use.
Definition at line 157 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::fitGraphInView(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::initializeVisuals(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initOptDistanceVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::toggleGraphVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::toggleLaserScansVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::toggleOptDistanceVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateGraphVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateOptDistanceVisualization(), and mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::updateVisuals().
◆ m_win_manager
|
protectedinherited |
Pointer to the CWindowManager object used to store visuals-related instances.
Definition at line 155 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initGraphVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::initializeVisuals(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initOptDistanceVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::toggleLaserScansVisualization(), mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateGraphVisualization(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::updateOptDistanceVisualization().
◆ m_win_observer
|
protectedinherited |
CWindowObserver object for monitoring various visual-oriented events.
Definition at line 160 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initGraphVisualization(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::initializeVisuals(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::initOptDistanceVisualization().
◆ params
|
protectedinherited |
Definition at line 155 of file CRangeScanOps.h.
◆ report_sep
|
staticprotectedinherited |
Definition at line 176 of file CRegistrationDeciderOrOptimizer.h.
Referenced by mrpt::graphslam::deciders::CICPCriteriaNRD< GRAPH_T >::getDescriptiveReport(), mrpt::graphslam::deciders::CICPCriteriaERD< GRAPH_T >::getDescriptiveReport(), and mrpt::graphslam::optimizers::CLevMarqGSO< GRAPH_T >::getDescriptiveReport().