MRPT  2.0.0
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.

Current decider is implemented based on the following papers:

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} $
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 213 of file CLoopCloserERD.h.

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

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

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

using parent_t = CRangeScanEdgeRegistrationDecider< GRAPH_T >
 Edge Registration Decider. More...
 
using constraint_t = typename GRAPH_T::constraint_t
 
using pose_t = typename GRAPH_T::constraint_t::type_value
 
using global_pose_t = typename GRAPH_T::global_pose_t
 
using decider_t = CLoopCloserERD< GRAPH_T >
 self type More...
 
using range_ops_t = typename parent_t::range_ops_t
 
using nodes_to_scans2D_t = typename parent_t::nodes_to_scans2D_t
 
using partitions_t = std::vector< std::vector< uint32_t > >
 
using edges_citerator = typename GRAPH_T::edges_map_t::const_iterator
 
using edges_iterator = typename GRAPH_T::edges_map_t::iterator
 
using hypot_t = typename mrpt::graphs::detail::THypothesis< GRAPH_T >
 
using hypots_t = std::vector< hypot_t >
 
using hypotsp_t = std::vector< hypot_t * >
 
using hypotsp_to_consist_t = std::map< std::pair< hypot_t *, hypot_t * >, double >
 
using path_t = mrpt::graphslam::TUncertaintyPath< GRAPH_T >
 
using paths_t = std::vector< path_t >
 
using node_props_t = mrpt::graphslam::detail::TNodeProps< GRAPH_T >
 

Public Member Functions

 CLoopCloserERD ()
 
 ~CLoopCloserERD () override
 
bool updateState (mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
 
void setWindowManagerPtr (mrpt::graphslam::CWindowManager *win_manager) override
 
void notifyOfWindowEvents (const std::map< std::string, bool > &events_occurred) override
 
void getEdgesStats (std::map< std::string, int > *edge_types_to_num) const override
 
void initializeVisuals () override
 
void updateVisuals () override
 
void loadParams (const std::string &source_fname) override
 
void printParams () const override
 
void getDescriptiveReport (std::string *report_str) const override
 
void getCurrentPartitions (partitions_t &partitions_out) const
 
const partitions_t & getCurrentPartitions () 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 std::vector< uint32_t > &groupA, const std::vector< uint32_t > &groupB, hypotsp_t *generated_hypots, const TGenerateHypotsPoolAdParams *ad_params=nullptr)
 Generate the hypothesis pool for all the inter-group constraints between two groups of nodes. More...
 
void generatePWConsistenciesMatrix (const std::vector< uint32_t > &groupA, const std::vector< uint32_t > &groupB, const hypotsp_t &hypots_pool, mrpt::math::CMatrixDouble *consist_matrix, const paths_t *groupA_opt_paths=nullptr, const paths_t *groupB_opt_paths=nullptr)
 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::graphs::TNodeID &nodeID, const std::map< mrpt::graphs::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::graphs::TNodeID &nodeID, global_pose_t *pose, mrpt::obs::CObservation2DRangeScan::Ptr &scan, const node_props_t *node_props=nullptr) const
 Fill the pose and LaserScan for the given nodeID. More...
 
bool mahalanobisDistanceOdometryToICPEdge (const mrpt::graphs::TNodeID &from, const mrpt::graphs::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::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge) override
 
virtual void fetchNodeIDsForScanMatching (const mrpt::graphs::TNodeID &curr_nodeID, std::set< mrpt::graphs::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::graphs::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 std::vector< uint32_t > &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::CVectorDouble *eigvec, bool use_power_method=false)
 
double generatePWConsistencyElement (const mrpt::graphs::TNodeID &a1, const mrpt::graphs::TNodeID &a2, const mrpt::graphs::TNodeID &b1, const mrpt::graphs::TNodeID &b2, const hypotsp_t &hypots, const paths_t *opt_paths=nullptr)
 Return the pair-wise consistency between the observations of the given nodes. More...
 
virtual bool getICPEdge (const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, constraint_t *rel_edge, mrpt::slam::CICP::TReturnInfo *icp_info=nullptr, const TGetICPEdgeAdParams *ad_params=nullptr)
 Get the ICP Edge between the provided nodes. More...
 
void execDijkstraProjection (mrpt::graphs::TNodeID starting_node=0, mrpt::graphs::TNodeID ending_node=INVALID_NODEID)
 compute the minimum uncertainty of each node position with regards to the graph root. More...
 
void getMinUncertaintyPath (const mrpt::graphs::TNodeID from, const mrpt::graphs::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::graphs::TNodeID > &neibors) const
 Append the paths starting from the current node. More...
 
mrpt::graphslam::TUncertaintyPath< GRAPH_T > * queryOptimalPath (const mrpt::graphs::TNodeID node) const
 Query for the optimal path of a nodeID. More...
 
void splitPartitionToGroups (std::vector< uint32_t > &partition, std::vector< uint32_t > *groupA, std::vector< uint32_t > *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::graphs::TNodeID &from, const mrpt::graphs::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::graphs::TNodeID &src, const mrpt::graphs::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, 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 = false
 
const mrpt::img::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 = false
 Indicate whether the partitions have been updated recently. More...
 
std::map< int, std::vector< uint32_t > > 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::graphs::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 = true
 Track the first node registration occurance. More...
 
size_t m_dijkstra_node_count_thresh = 3
 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>
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::constraint_t = typename GRAPH_T::constraint_t

Definition at line 220 of file CLoopCloserERD.h.

◆ decider_t

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

self type

Definition at line 223 of file CLoopCloserERD.h.

◆ edges_citerator

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

Definition at line 227 of file CLoopCloserERD.h.

◆ edges_iterator

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

Definition at line 228 of file CLoopCloserERD.h.

◆ global_pose_t

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

Definition at line 222 of file CLoopCloserERD.h.

◆ hypot_t

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

Definition at line 229 of file CLoopCloserERD.h.

◆ hypots_t

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

Definition at line 230 of file CLoopCloserERD.h.

◆ hypotsp_t

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

Definition at line 231 of file CLoopCloserERD.h.

◆ hypotsp_to_consist_t

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

Definition at line 233 of file CLoopCloserERD.h.

◆ node_props_t

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

Definition at line 236 of file CLoopCloserERD.h.

◆ nodes_to_scans2D_t

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

Definition at line 225 of file CLoopCloserERD.h.

◆ parent_t

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

Edge Registration Decider.

Definition at line 218 of file CLoopCloserERD.h.

◆ partitions_t

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::partitions_t = std::vector<std::vector<uint32_t> >

Definition at line 226 of file CLoopCloserERD.h.

◆ path_t

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

Definition at line 234 of file CLoopCloserERD.h.

◆ paths_t

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

Definition at line 235 of file CLoopCloserERD.h.

◆ pose_t

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

Definition at line 221 of file CLoopCloserERD.h.

◆ range_ops_t

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

Definition at line 224 of file CLoopCloserERD.h.

Constructor & Destructor Documentation

◆ CLoopCloserERD()

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

Definition at line 22 of file CLoopCloserERD_impl.h.

References MRPT_LOG_DEBUG_STREAM.

◆ ~CLoopCloserERD()

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

Definition at line 31 of file CLoopCloserERD_impl.h.

Member Function Documentation

◆ addScanMatchingEdges()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::addScanMatchingEdges ( const mrpt::graphs::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 172 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::graphs::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 1434 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 404 of file CLoopCloserERD_impl.h.

References ASSERTDEB_, mrpt::containers::find(), mrpt::containers::getSTLContainerAsString(), MRPT_END, MRPT_LOG_DEBUG_STREAM, 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 std::vector< uint32_t > &  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 2023 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::CVectorDouble *  eigvec,
bool  use_power_method = false 
)
protected

Definition at line 864 of file CLoopCloserERD_impl.h.

References mrpt::math::approximatelyEqual(), ASSERTDEB_, ASSERTDEBMSG_, mrpt::math::CMatrixDynamic< T >::cols(), mrpt::math::MatrixBase< Scalar, Derived >::eig(), mrpt::format(), MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_ERROR_STREAM, MRPT_START, mrpt::math::CVectorDynamic< T >::size(), mrpt::math::CMatrixDynamic< T >::size(), 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 2286 of file CLoopCloserERD_impl.h.

References mrpt::system::LVL_ERROR, 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 578 of file CLoopCloserERD_impl.h.

References mrpt::math::CVectorDynamic< T >::asEigen(), ASSERTDEB_, mrpt::format(), MRPT_END, MRPT_START, mrpt::math::CVectorDynamic< T >::size(), and mrpt::square().

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 517 of file CLoopCloserERD_impl.h.

References MRPT_END, MRPT_LOG_DEBUG_FMT, 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::graphs::TNodeID  starting_node = 0,
mrpt::graphs::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 1287 of file CLoopCloserERD_impl.h.

References ASSERTDEB_, ASSERTDEBMSG_, 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::graphs::TNodeID &  curr_nodeID,
std::set< mrpt::graphs::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 151 of file CLoopCloserERD_impl.h.

References ASSERTDEB_.

◆ fillNodePropsFromGroupParams()

template<class GRAPH_T >
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::fillNodePropsFromGroupParams ( const mrpt::graphs::TNodeID &  nodeID,
const std::map< mrpt::graphs::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 306 of file CLoopCloserERD_impl.h.

References ASSERTDEB_.

◆ 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::graphs::TNodeID &  from,
const mrpt::graphs::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 nullptr.

Definition at line 1232 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,
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 nullptr.

Definition at line 1262 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::graphs::TNodeID &  src,
const mrpt::graphs::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
nullptr 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 1203 of file CLoopCloserERD_impl.h.

References ASSERTDEB_, 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 std::vector< uint32_t > &  groupA,
const std::vector< uint32_t > &  groupB,
hypotsp_t *  generated_hypots,
const TGenerateHypotsPoolAdParams *  ad_params = nullptr 
)

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 718 of file CLoopCloserERD_impl.h.

References ASSERTDEBMSG_, mrpt::format(), mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGetICPEdgeAdParams::from_params, mrpt::containers::getSTLContainerAsString(), mrpt::slam::CICP::TReturnInfo::goodness, mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGenerateHypotsPoolAdParams::groupA_params, mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::TGenerateHypotsPoolAdParams::groupB_params, 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:

◆ generatePWConsistenciesMatrix()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::generatePWConsistenciesMatrix ( const std::vector< uint32_t > &  groupA,
const std::vector< uint32_t > &  groupB,
const hypotsp_t &  hypots_pool,
mrpt::math::CMatrixDouble *  consist_matrix,
const paths_t *  groupA_opt_paths = nullptr,
const paths_t *  groupB_opt_paths = nullptr 
)

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 936 of file CLoopCloserERD_impl.h.

References mrpt::obs::gnss::a1, mrpt::obs::gnss::a2, ASSERTDEBMSG_, mrpt::obs::gnss::b1, mrpt::obs::gnss::b2, mrpt::containers::getSTLContainerAsString(), MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_START, and mrpt::math::CMatrixDynamic< T >::rows().

Here is the call graph for this function:

◆ generatePWConsistencyElement()

template<class GRAPH_T >
double mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::generatePWConsistencyElement ( const mrpt::graphs::TNodeID &  a1,
const mrpt::graphs::TNodeID &  a2,
const mrpt::graphs::TNodeID &  b1,
const mrpt::graphs::TNodeID &  b2,
const hypotsp_t &  hypots,
const paths_t *  opt_paths = nullptr 
)
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 1080 of file CLoopCloserERD_impl.h.

References mrpt::obs::gnss::a1, mrpt::obs::gnss::a2, ASSERTDEB_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::assertIsBetweenNodeIDs(), mrpt::obs::gnss::b1, mrpt::obs::gnss::b2, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::curr_pose_pdf, MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_START, and mrpt::math::multiply_HtCH_scalar().

Here is the call graph for this function:

◆ getCurrentPartitions() [1/2]

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

Definition at line 2389 of file CLoopCloserERD_impl.h.

◆ getCurrentPartitions() [2/2]

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

Definition at line 2397 of file CLoopCloserERD_impl.h.

◆ getDescriptiveReport()

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

Definition at line 2356 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 265 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
override

Definition at line 2149 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::graphs::TNodeID &  from,
const mrpt::graphs::TNodeID &  to,
constraint_t *  rel_edge,
mrpt::slam::CICP::TReturnInfo *  icp_info = nullptr,
const TGetICPEdgeAdParams *  ad_params = nullptr 
)
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 234 of file CLoopCloserERD_impl.h.

References ASSERTDEB_, 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::graphs::TNodeID  from,
const mrpt::graphs::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 1491 of file CLoopCloserERD_impl.h.

References mrpt::graphslam::TUncertaintyPath< GRAPH_T >::addToPath(), ASSERTDEB_, ASSERTDEBMSG_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::clear(), mrpt::format(), mrpt::graphslam::TUncertaintyPath< GRAPH_T >::getDeterminant(), MRPT_END, MRPT_START, and mrpt::math::MatrixBase< Scalar, Derived >::setIdentity().

Here is the call graph for this function:

◆ getPropsOfNodeID()

template<class GRAPH_T >
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getPropsOfNodeID ( const mrpt::graphs::TNodeID &  nodeID,
global_pose_t *  pose,
mrpt::obs::CObservation2DRangeScan::Ptr &  scan,
const node_props_t *  node_props = nullptr 
) 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 338 of file CLoopCloserERD_impl.h.

References ASSERTDEB_, ASSERTDEBMSG_, 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 2211 of file CLoopCloserERD_impl.h.

References MRPT_END, and MRPT_START.

◆ initializeVisuals()

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

Definition at line 2158 of file CLoopCloserERD_impl.h.

References ASSERTDEBMSG_, MRPT_END, and MRPT_START.

◆ initLaserScansVisualization()

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

Definition at line 2050 of file CLoopCloserERD_impl.h.

References mrpt::opengl::CPlanarLaserScan::Create(), MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ 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 1761 of file CLoopCloserERD_impl.h.

◆ loadParams()

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

Definition at line 2303 of file CLoopCloserERD_impl.h.

References MRPT_END, MRPT_START, mrpt::config::CConfigFileBase::read_double(), mrpt::config::CConfigFileBase::read_int(), and setMinLoggingLevel().

Here is the call graph for this function:

◆ mahalanobisDistanceOdometryToICPEdge()

template<class GRAPH_T >
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::mahalanobisDistanceOdometryToICPEdge ( const mrpt::graphs::TNodeID &  from,
const mrpt::graphs::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 1630 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)
override

Definition at line 1740 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 1601 of file CLoopCloserERD_impl.h.

References ASSERTDEB_, 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
override

Definition at line 2332 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::graphs::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 1472 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 1677 of file CLoopCloserERD_impl.h.

◆ registerNewEdge()

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

Definition at line 1685 of file CLoopCloserERD_impl.h.

References mrpt::math::absDiff(), MRPT_END, MRPT_LOG_INFO, 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 269 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)
override

Definition at line 1716 of file CLoopCloserERD_impl.h.

References MRPT_LOG_DEBUG.

◆ splitPartitionToGroups()

template<class GRAPH_T >
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::splitPartitionToGroups ( std::vector< uint32_t > &  partition,
std::vector< uint32_t > *  groupA,
std::vector< uint32_t > *  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 652 of file CLoopCloserERD_impl.h.

References ASSERTDEB_, ASSERTDEBMSG_, 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 2121 of file CLoopCloserERD_impl.h.

References ASSERTDEBMSG_, MRPT_END, MRPT_LOG_INFO, 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 1995 of file CLoopCloserERD_impl.h.

References ASSERTDEBMSG_, MRPT_END, MRPT_LOG_INFO, and MRPT_START.

◆ updateCurrCovarianceVisualization()

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

Definition at line 2243 of file CLoopCloserERD_impl.h.

References mrpt::graphslam::TUncertaintyPath< GRAPH_T >::curr_pose_pdf, mrpt::math::MatrixBase< Scalar, Derived >::det(), MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.

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 2082 of file CLoopCloserERD_impl.h.

References mrpt::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 2403 of file CLoopCloserERD_impl.h.

References mrpt::poses::CPose3DPDF::createFrom2D(), mrpt::obs::CSensoryFrame::insert(), MRPT_END, MRPT_LOG_DEBUG_STREAM, MRPT_LOG_INFO, MRPT_LOG_WARN_STREAM, and MRPT_START.

Here is the call graph for this function:

◆ updateMapPartitionsVisualization()

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

Update the map partitions visualization.

Todo:
Have some sorts of a string_view instead

Definition at line 1784 of file CLoopCloserERD_impl.h.

References mrpt::containers::find(), mrpt::format(), MRPT_LOG_DEBUG_STREAM, THROW_EXCEPTION, and THROW_EXCEPTION_FMT.

Here is the call graph for this function:

◆ updateState()

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
bool mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::updateState ( mrpt::obs::CActionCollection::Ptr  action,
mrpt::obs::CSensoryFrame::Ptr  observations,
mrpt::obs::CObservation::Ptr  observation 
)
override

Definition at line 41 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, and THROW_EXCEPTION.

Here is the call graph for this function:

◆ updateVisuals()

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

Definition at line 2186 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 841 of file CLoopCloserERD.h.

◆ m_curr_node_covariance_color

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
const mrpt::img::TColor mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_curr_node_covariance_color
protected
Initial value:
=
mrpt::img::TColor(160, 160, 160, 255)

Definition at line 796 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 813 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 = 3
protected

Node Count lower bound before executing dijkstra.

Definition at line 838 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 805 of file CLoopCloserERD.h.

◆ 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 830 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 = true
protected

Track the first node registration occurance.

Handy so that we can assign a measurement to the root node as well.

Definition at line 835 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 508 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 807 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 811 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 845 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 509 of file CLoopCloserERD.h.

◆ m_node_optimal_paths

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
std::map<mrpt::graphs::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 825 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 798 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 793 of file CLoopCloserERD.h.

◆ m_partitionID_to_prev_nodes_list

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
std::map<int, std::vector<uint32_t> > 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 820 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 = false
protected

Indicate whether the partitions have been updated recently.

Definition at line 816 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 799 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 = false
protected

Definition at line 795 of file CLoopCloserERD.h.



Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at mié abr 1 00:50:30 CEST 2020