Main MRPT website > C++ reference for MRPT 1.9.9
List of all members | Classes | Public Types | Public Member Functions | Protected Member Functions | Static Protected Member Functions | Protected Attributes
mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T > Class Template Reference
[mrpt-graphslam]

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:

Specifications

Loop Closing Strategy

The Loop closure registration strategy is described below:

Loop closing schematic. blue nodes belong to group A and have lower NodeIDs, while red nodes belong to group B and have higher NodeIDs. Nodes of both groups A and B belong to the same partition based on their corresponding 2DRangeScan observations.
Rigid body transformation of hypotheses and two corresponding dijkstra links
Note
Olson uses the following formula for evaluating the pairwise consistency between two hypotheses i,j:
$ A_{i,j} = e^{T \Sigma_T^{-1} T^T} $
Where:
  • T is the rigid body transformation using the two hypotheses and the two Dijkstra Links connecting the nodes that form the hypotheses
  • $ \Sigma_T $ 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 $ A_{i,j} $, 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), $ T T^T $ 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 $ \Sigma_T^{-1} $ 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:
    • $ \text{Correct loop closure} \Rightarrow T \rightarrow [0, 0, 0] \Rightarrow \text{exponent} \downarrow $
    • $ \text{Correct loop closure} \Rightarrow \text{diagonal\_terms}(\Sigma_T^{-1}) \uparrow \Rightarrow \text{exponent} \uparrow $
Based on the previous comments the following formula is used in the decider:
$ A_{i,j} = e^{-T \Sigma_T T^T} $

.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).
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
See also
mrpt::slam::CIncrementalMapPartitioner

Definition at line 243 of file CLoopCloserERD.h.

#include <mrpt/graphslam/ERD/CLoopCloserERD.h>

Inheritance diagram for mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >:
Inheritance graph

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
 

Public Member Functions

 CLoopCloserERD ()
 
virtual ~CLoopCloserERD ()
 
virtual bool updateState (mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
 
void setWindowManagerPtr (mrpt::graphslam::CWindowManager *win_manager)
 
void notifyOfWindowEvents (const std::map< std::string, bool > &events_occurred)
 
void getEdgesStats (std::map< std::string, int > *edge_types_to_num) const
 
void initializeVisuals ()
 
void updateVisuals ()
 
void loadParams (const std::string &source_fname)
 
void printParams () const
 
void getDescriptiveReport (std::string *report_str) const
 
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...
 

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::CObservation2DRangeScan::Ptr &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)
 
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::CObservation2DRangeScan::Ptr scan)
 Assign the last recorded 2D Laser scan. More...
 

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::CObservation2DRangeScan::Ptr 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::CObservation2DRangeScan::Ptr 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...
 
Partition vectors
partitions_t m_last_partitions
 Previous partitions vector. More...
 
partitions_t m_curr_partitions
 Current partitions vector. More...
 

Member Typedef Documentation

◆ constraint_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef GRAPH_T::constraint_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::constraint_t

Handy typedefs.

type of graph constraints

Definition at line 253 of file CLoopCloserERD.h.

◆ decider_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef CLoopCloserERD<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::decider_t

self type - Handy typedef

Definition at line 257 of file CLoopCloserERD.h.

◆ edges_citerator

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef GRAPH_T::edges_map_t::const_iterator mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::edges_citerator

Definition at line 262 of file CLoopCloserERD.h.

◆ edges_iterator

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef GRAPH_T::edges_map_t::iterator mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::edges_iterator

Definition at line 263 of file CLoopCloserERD.h.

◆ global_pose_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef GRAPH_T::global_pose_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::global_pose_t

Definition at line 256 of file CLoopCloserERD.h.

◆ hypot_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef mrpt::graphs::detail::THypothesis<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypot_t

Definition at line 264 of file CLoopCloserERD.h.

◆ hypots_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef std::vector<hypot_t> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypots_t

Definition at line 265 of file CLoopCloserERD.h.

◆ hypotsp_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef std::vector<hypot_t*> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypotsp_t

Definition at line 266 of file CLoopCloserERD.h.

◆ hypotsp_to_consist_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef std::map<std::pair<hypot_t*, hypot_t*>, double> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypotsp_to_consist_t

Definition at line 268 of file CLoopCloserERD.h.

◆ node_props_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef mrpt::graphslam::detail::TNodeProps<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::node_props_t

Definition at line 271 of file CLoopCloserERD.h.

◆ nodes_to_scans2D_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef parent_t::nodes_to_scans2D_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::nodes_to_scans2D_t

Definition at line 259 of file CLoopCloserERD.h.

◆ parent_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef CRangeScanEdgeRegistrationDecider<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::parent_t

Edge Registration Decider.

Definition at line 248 of file CLoopCloserERD.h.

◆ partitions_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
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 261 of file CLoopCloserERD.h.

◆ path_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef mrpt::graphslam::TUncertaintyPath<GRAPH_T> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::path_t

Definition at line 269 of file CLoopCloserERD.h.

◆ paths_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef std::vector<path_t> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::paths_t

Definition at line 270 of file CLoopCloserERD.h.

◆ pose_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef GRAPH_T::constraint_t::type_value mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::pose_t

type of underlying poses (2D/3D).

Definition at line 255 of file CLoopCloserERD.h.

◆ range_ops_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
typedef parent_t::range_ops_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::range_ops_t

Definition at line 258 of file CLoopCloserERD.h.

Constructor & Destructor Documentation

◆ CLoopCloserERD()

template<class GRAPH_T >
mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::CLoopCloserERD ( )

Definition at line 23 of file CLoopCloserERD_impl.h.

References mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_edge_types_to_nums, and MRPT_LOG_DEBUG_STREAM.

◆ ~CLoopCloserERD()

template<class GRAPH_T >
mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::~CLoopCloserERD ( )
virtual

Definition at line 37 of file CLoopCloserERD_impl.h.

References MRPT_LOG_DEBUG_STREAM.

Member Function Documentation

◆ addScanMatchingEdges()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::addScanMatchingEdges ( const mrpt::utils::TNodeID &  curr_nodeID)
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 189 of file CLoopCloserERD_impl.h.

References mrpt::math::approximatelyEqual(), mrpt::slam::CICP::TReturnInfo::goodness, MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.

Here is the call graph for this function:

◆ addToPaths()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::addToPaths ( std::set< path_t *> *  pool_of_paths,
const path_t &  curr_path,
const std::set< mrpt::utils::TNodeID > &  neibors 
) const
protected

Append the paths starting from the current node.

Parameters
[in]pool_of_pathsPaths that are currently registered
[in]curr_pathPath that I am currently traversing. This path is already removed from pool_of_paths
[in]neighborsstd::set of neighboring nodes to the last node of the current path

Definition at line 1488 of file CLoopCloserERD_impl.h.

References mrpt::graphslam::TUncertaintyPath< GRAPH_T >::getDestination(), MRPT_END, MRPT_START, and mrpt::graphslam::TUncertaintyPath< GRAPH_T >::nodes_traversed.

Here is the call graph for this function:

◆ checkPartitionsForLC()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::checkPartitionsForLC ( partitions_t *  partitions_for_LC)
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 426 of file CLoopCloserERD_impl.h.

References ASSERT_, mrpt::utils::find(), mrpt::utils::getSTLContainerAsString(), MRPT_END, MRPT_LOG_WARN_STREAM, and MRPT_START.

Here is the call graph for this function:

◆ computeCentroidOfNodesVector()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::computeCentroidOfNodesVector ( const vector_uint &  nodes_list,
std::pair< double, double > *  centroid_coords 
) const
protected

Compute the Centroid of a group of a vector of node positions.

Parameters
[in]nodes_listList of node IDs whose positions are taken into account
[out]centroid_coordsContains the Centroid coordinates as a pair [x,y]
Note
Method is used during the visualization of the map partitions.

Definition at line 2098 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

◆ computeDominantEigenVector()

template<class GRAPH_T >
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::computeDominantEigenVector ( const mrpt::math::CMatrixDouble &  consist_matrix,
mrpt::math::dynamic_vector< double > *  eigvec,
bool  use_power_method = false 
)
protected

limit =

Definition at line 899 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.

Here is the call graph for this function:

◆ dumpVisibilityErrorMsg()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::dumpVisibilityErrorMsg ( std::string  viz_flag,
int  sleep_time = 500 
)
protected

Definition at line 2369 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

◆ evalPWConsistenciesMatrix()

template<class GRAPH_T >
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 607 of file CLoopCloserERD_impl.h.

References ASSERT_, mrpt::mrpt::format(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ evaluatePartitionsForLC()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::evaluatePartitionsForLC ( const partitions_t &  partitions)
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 538 of file CLoopCloserERD_impl.h.

References MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_WARN_STREAM, and MRPT_START.

◆ execDijkstraProjection()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::execDijkstraProjection ( mrpt::utils::TNodeID  starting_node = 0,
mrpt::utils::TNodeID  ending_node = INVALID_NODEID 
)
protected

compute the minimum uncertainty of each node position with regards to the graph root.

Parameters
[in]starting_nodeNode from which I start the Dijkstra projection algorithm
[in]ending_nodeSpecify the nodeID whose uncertainty wrt the starting_node, we are interested in computing. If given, method execution ends when this path is computed.

TODO Remove these - >>>>>>>>>>>>>>>>>>>> ^^^UNCOMMENT ABOVE AS WELL^^^

Definition at line 1337 of file CLoopCloserERD_impl.h.

References ASSERT_, ASSERTMSG_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::getDestination(), INVALID_NODEID, MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ fetchNodeIDsForScanMatching()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::fetchNodeIDsForScanMatching ( const mrpt::utils::TNodeID &  curr_nodeID,
std::set< mrpt::utils::TNodeID > *  nodes_set 
)
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 168 of file CLoopCloserERD_impl.h.

References ASSERT_.

◆ fillNodePropsFromGroupParams()

template<class GRAPH_T >
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::fillNodePropsFromGroupParams ( const mrpt::utils::TNodeID &  nodeID,
const std::map< mrpt::utils::TNodeID, node_props_t > &  group_params,
node_props_t *  node_props 
)
protected

Fill the TNodeProps instance using the parameters from the map.

Parameters
[in]nodeIDID of node corresponding to the TNodeProps struct that is to be filled
[in]group_paramsMap of TNodeID to corresponding TNodeProps instance.
[out]node_propsPointer to the TNodeProps struct to be filled.
Returns
True if operation was successful, false otherwise.

Definition at line 325 of file CLoopCloserERD_impl.h.

References ASSERT_.

◆ findHypotByEnds()

template<class GRAPH_T >
mrpt::graphs::detail::THypothesis< GRAPH_T > * mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::findHypotByEnds ( const hypotsp_t &  vec_hypots,
const mrpt::utils::TNodeID &  from,
const mrpt::utils::TNodeID &  to,
bool  throw_exc = true 
)
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_hypotsVector of hypothesis to check
[in]fromStarting Node for hypothesis
[in]toEnding Node for hypothesis
[in]throw_excIf true and hypothesis is not found, throw a HypothesisNotFoundException
Returns
Pointer to the found hypothesis if that is found, otherwise NULL.

Definition at line 1280 of file CLoopCloserERD_impl.h.

◆ findHypotByID()

template<class GRAPH_T >
mrpt::graphs::detail::THypothesis< GRAPH_T > * mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::findHypotByID ( const hypotsp_t &  vec_hypots,
const size_t &  id,
bool  throw_exc = true 
)
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_hypotsVector of hypothesis to check
[in]id,IDof the hypothesis to be returned
[in]throw_excIf 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 1311 of file CLoopCloserERD_impl.h.

◆ findPathByEnds()

template<class GRAPH_T >
const mrpt::graphslam::TUncertaintyPath< GRAPH_T > * mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::findPathByEnds ( const paths_t &  vec_paths,
const mrpt::utils::TNodeID &  src,
const mrpt::utils::TNodeID &  dst,
bool  throw_exc = true 
)
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_errorif path was not found and throw_exc is set to true

Definition at line 1248 of file CLoopCloserERD_impl.h.

References ASSERT_, mrpt::format(), and THROW_EXCEPTION.

Here is the call graph for this function:

◆ generateHypotsPool()

template<class GRAPH_T >
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]groupAFirst group to be tested
[in]groupBSecond group to be tested
[out]generated_hypotsPool of generated hypothesis. Hypotheses are generated in the heap, so the caller is responsible of afterwards calling delete.

Definition at line 747 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.

Here is the call graph for this function:

◆ generatePWConsistenciesMatrix()

template<class GRAPH_T >
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]groupAFirst group to be used
[in]groupBSecond group to be used
[in]hypots_poolPool 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_pathsPointer 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
See also
generatePWConsistencyElement
evalPWConsistenciesMatrix

Definition at line 975 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.

Here is the call graph for this function:

◆ generatePWConsistencyElement()

template<class GRAPH_T >
double mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::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 
)
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 $ (x,y,\phi)$ of the above composition (e.g. T) the pairwise consistency element would then be:

$ A_{i,j} = e^{-T \Sigma_T T^T} $
Parameters
[in]hypotsHypothesis corresponding to the potential inter-group constraints
[in]opt_pathsVector 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 1124 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.

Here is the call graph for this function:

◆ getCurrPartitions() [1/2]

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getCurrPartitions ( partitions_t *  partitions_out) const

Definition at line 2472 of file CLoopCloserERD_impl.h.

References ASSERT_.

◆ getCurrPartitions() [2/2]

template<class GRAPH_T >
const std::vector< mrpt::vector_uint > & mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getCurrPartitions ( ) const

Definition at line 2481 of file CLoopCloserERD_impl.h.

◆ getDescriptiveReport()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getDescriptiveReport ( std::string *  report_str) const

Definition at line 2439 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

◆ getDijkstraExecutionThresh()

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
size_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getDijkstraExecutionThresh ( ) const
inline

Return the minimum number of nodes that should exist in the graph prior to running Dijkstra.

Definition at line 300 of file CLoopCloserERD.h.

References mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_dijkstra_node_count_thresh.

◆ getEdgesStats()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getEdgesStats ( std::map< std::string, int > *  edge_types_to_num) const

Definition at line 2228 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

◆ getICPEdge()

template<class GRAPH_T >
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::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 
)
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_infoStruct that will be filled with the results of the ICP operation
[in]ad_paramsPointer 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 252 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.

Here is the call graph for this function:

◆ getMinUncertaintyPath()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getMinUncertaintyPath ( const mrpt::utils::TNodeID  from,
const mrpt::utils::TNodeID  to,
path_t *  path 
) const
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 1545 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_END, and MRPT_START.

Here is the call graph for this function:

◆ getPropsOfNodeID()

template<class GRAPH_T >
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getPropsOfNodeID ( const mrpt::utils::TNodeID &  nodeID,
global_pose_t *  pose,
mrpt::obs::CObservation2DRangeScan::Ptr &  scan,
const node_props_t *  node_props = NULL 
) const
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 358 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.

Here is the call graph for this function:

◆ initCurrCovarianceVisualization()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::initCurrCovarianceVisualization ( )
protected

Definition at line 2290 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

◆ initializeVisuals()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::initializeVisuals ( )

Definition at line 2237 of file CLoopCloserERD_impl.h.

References ASSERTMSG_, MRPT_END, and MRPT_START.

◆ initLaserScansVisualization()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::initLaserScansVisualization ( )
protected

Definition at line 2126 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

◆ initMapPartitionsVisualization()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::initMapPartitionsVisualization ( )
protected

Initialize the visualization of the map partition objects.

Definition at line 1822 of file CLoopCloserERD_impl.h.

◆ loadParams()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::loadParams ( const std::string &  source_fname)

Definition at line 2386 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

◆ mahalanobisDistanceOdometryToICPEdge()

template<class GRAPH_T >
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::mahalanobisDistanceOdometryToICPEdge ( const mrpt::utils::TNodeID &  from,
const mrpt::utils::TNodeID &  to,
const constraint_t &  rel_edge 
)
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 1686 of file CLoopCloserERD_impl.h.

References mrpt::math::mahalanobisDistance2(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ notifyOfWindowEvents()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::notifyOfWindowEvents ( const std::map< std::string, bool > &  events_occurred)

Definition at line 1801 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

◆ popMinUncertaintyPath()

template<class GRAPH_T >
TUncertaintyPath< GRAPH_T > * mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::popMinUncertaintyPath ( std::set< path_t *> *  pool_of_paths) const
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 1656 of file CLoopCloserERD_impl.h.

References ASSERT_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::getDeterminant(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ printParams()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::printParams ( ) const

Definition at line 2415 of file CLoopCloserERD_impl.h.

References MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.

◆ queryOptimalPath()

template<class GRAPH_T >
mrpt::graphslam::TUncertaintyPath< GRAPH_T > * mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::queryOptimalPath ( const mrpt::utils::TNodeID  node) const
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]nodenodeID for which hte path is going to be returned
Returns
Optimal path corresponding to the given nodeID or nullptr if the former is not found.

Definition at line 1527 of file CLoopCloserERD_impl.h.

◆ registerHypothesis()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::registerHypothesis ( const hypot_t &  h)
protected

Wrapper around the registerNewEdge method which accepts a THypothesis object instead.

Definition at line 1734 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.

Here is the call graph for this function:

◆ registerNewEdge()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::registerNewEdge ( const mrpt::utils::TNodeID &  from,
const mrpt::utils::TNodeID &  to,
const constraint_t &  rel_edge 
)
protected

Definition at line 1742 of file CLoopCloserERD_impl.h.

References mrpt::math::absDiff(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ setDijkstraExecutionThresh()

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::setDijkstraExecutionThresh ( size_t  new_thresh)
inline

Definition at line 304 of file CLoopCloserERD.h.

References mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_dijkstra_node_count_thresh.

◆ setLastLaserScan2D()

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::setLastLaserScan2D ( mrpt::obs::CObservation2DRangeScan::Ptr  scan)
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()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::setWindowManagerPtr ( mrpt::graphslam::CWindowManager *  win_manager)

Definition at line 1774 of file CLoopCloserERD_impl.h.

◆ splitPartitionToGroups()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::splitPartitionToGroups ( vector_uint &  partition,
vector_uint *  groupA,
vector_uint *  groupB,
int  max_nodes_in_group = 5 
)
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]partitionPartition to be split.
[out]groupAFirst group of nodes.
[out]groupBSecond group of nodes.
[in]max_nodes_in_groupMax number of nodes that are to exist in each group (Use -1 to disable this threshold).

Definition at line 682 of file CLoopCloserERD_impl.h.

References ASSERT_, ASSERTMSG_, mrpt::format(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ toggleLaserScansVisualization()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::toggleLaserScansVisualization ( )
protected

togle the LaserScans visualization on and off

Definition at line 2199 of file CLoopCloserERD_impl.h.

References ASSERTMSG_, MRPT_END, and MRPT_START.

◆ toggleMapPartitionsVisualization()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::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 2069 of file CLoopCloserERD_impl.h.

References ASSERTMSG_, MRPT_END, and MRPT_START.

◆ updateCurrCovarianceVisualization()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::updateCurrCovarianceVisualization ( )
protected

Definition at line 2323 of file CLoopCloserERD_impl.h.

References ASSERT_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::curr_pose_pdf, MRPT_END, MRPT_START, and mrpt::opengl::CRenderizable::setLocation().

Here is the call graph for this function:

◆ updateLaserScansVisualization()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::updateLaserScansVisualization ( )
protected

Definition at line 2158 of file CLoopCloserERD_impl.h.

References ASSERT_, mrpt::mrpt::utils::DEG2RAD(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ updateMapPartitions()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::updateMapPartitions ( bool  full_update = false,
bool  is_first_time_node_reg = false 
)
protected

Split the currently registered graph nodes into partitions.

Definition at line 2487 of file CLoopCloserERD_impl.h.

References MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_WARN_STREAM, and MRPT_START.

◆ updateMapPartitionsVisualization()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::updateMapPartitionsVisualization ( )
protected

Update the map partitions visualization.

Definition at line 1849 of file CLoopCloserERD_impl.h.

References ASSERT_, mrpt::opengl::CSetOfLines::clear(), mrpt::utils::find(), mrpt::mrpt::format(), MRPT_LOG_DEBUG_STREAM, MRPT_LOG_ERROR_STREAM, mrpt::opengl::CRenderizable::setColor_u8(), mrpt::opengl::CRenderizable::setLocation(), and mrpt::opengl::CRenderizable::setVisibility().

Here is the call graph for this function:

◆ updateState()

template<class GRAPH_T >
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::updateState ( mrpt::obs::CActionCollection::Ptr  action,
mrpt::obs::CSensoryFrame::Ptr  observations,
mrpt::obs::CObservation::Ptr  observation 
)
virtual

Definition at line 55 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.

Here is the call graph for this function:

◆ updateVisuals()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::updateVisuals ( )

Definition at line 2265 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

Member Data Documentation

◆ m_consec_icp_constraint_factor

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
double mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_consec_icp_constraint_factor
protected

Factor used for accepting an ICP Constraint as valid.

Definition at line 876 of file CLoopCloserERD.h.

◆ m_curr_node_covariance_color

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
const mrpt::utils::TColor mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_curr_node_covariance_color
protected

Definition at line 832 of file CLoopCloserERD.h.

◆ m_curr_partitions

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
partitions_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_curr_partitions
protected

Current partitions vector.

Definition at line 848 of file CLoopCloserERD.h.

◆ m_dijkstra_node_count_thresh

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
size_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_dijkstra_node_count_thresh
protected

Node Count lower bound before executing dijkstra.

Definition at line 873 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

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
std::map<std::string, int> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_edge_types_to_nums
protected

Keep track of the registered edge types.

Handy for displaying them in the Visualization window.

Definition at line 840 of file CLoopCloserERD.h.

Referenced by mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::CLoopCloserERD().

◆ m_first_laser_scan

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
mrpt::obs::CObservation2DRangeScan::Ptr mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::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 865 of file CLoopCloserERD.h.

◆ m_is_first_time_node_reg

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::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 870 of file CLoopCloserERD.h.

◆ m_laser_params

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
TLaserParams mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_laser_params
protected

Definition at line 543 of file CLoopCloserERD.h.

◆ m_last_laser_scan2D

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
mrpt::obs::CObservation2DRangeScan::Ptr mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_last_laser_scan2D
protected

Keep the last laser scan for visualization purposes.

Definition at line 842 of file CLoopCloserERD.h.

◆ m_last_partitions

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
partitions_t mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_last_partitions
protected

Previous partitions vector.

Definition at line 846 of file CLoopCloserERD.h.

◆ m_lc_icp_constraint_factor

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
double mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_lc_icp_constraint_factor
protected

Factor used for accepting an ICP Constraint in the loop closure proc.

Definition at line 880 of file CLoopCloserERD.h.

◆ m_lc_params

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
TLoopClosureParams mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_lc_params
protected

Definition at line 544 of file CLoopCloserERD.h.

◆ m_node_optimal_paths

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
std::map<mrpt::utils::TNodeID, path_t*> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::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 860 of file CLoopCloserERD.h.

◆ m_offset_y_curr_node_covariance

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
double mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_offset_y_curr_node_covariance
protected

Definition at line 833 of file CLoopCloserERD.h.

◆ m_partitioner

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
mrpt::slam::CIncrementalMapPartitioner mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_partitioner
protected

Instance responsible for partitioning the map.

Definition at line 829 of file CLoopCloserERD.h.

◆ m_partitionID_to_prev_nodes_list

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
std::map<int, vector_uint> mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::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 855 of file CLoopCloserERD.h.

◆ m_partitions_full_update

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_partitions_full_update
protected

Indicate whether the partitions have been updated recently.

Definition at line 851 of file CLoopCloserERD.h.

◆ m_text_index_curr_node_covariance

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
int mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_text_index_curr_node_covariance
protected

Definition at line 834 of file CLoopCloserERD.h.

◆ m_visualize_curr_node_covariance

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_visualize_curr_node_covariance
protected

Definition at line 831 of file CLoopCloserERD.h.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019