Template Class CLoopCloserERD

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

template<class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
class CLoopCloserERD : public virtual mrpt::graphslam::deciders::CRangeScanEdgeRegistrationDecider<typename mrpt::graphs::CNetworkOfPoses2DInf>

Edge Registration Decider scheme specialized in Loop Closing.

Partition vectors

partitions_t m_last_partitions

Previous partitions vector.

partitions_t m_curr_partitions

Current partitions vector.

Public Types

using parent_t = CRangeScanEdgeRegistrationDecider<GRAPH_T>

Edge Registration Decider.

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

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 Functions

CLoopCloserERD()
~CLoopCloserERD() override
virtual bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override

Generic method for fetching the incremental action-observations (or observation-only) measurements.

Returns:

True if operation was successful. Criteria for Success depend on the decider/optimizer implementing this method

virtual void setWindowManagerPtr(mrpt::graphslam::CWindowManager *win_manager) override

Fetch a CWindowManager pointer.

CWindowManager instance should contain a CDisplayWindow3D* and, optionally, a CWindowObserver pointer so that interaction with the window is possible

virtual void notifyOfWindowEvents(const std::map<std::string, bool> &events_occurred) override

Get a list of the window events that happened since the last call.

Method in derived classes is automatically called from the CGraphSlamEngine_t instance. After that, decider/optimizer should just fetch the parameters that it is interested in.

virtual void getEdgesStats(std::map<std::string, int> *edge_types_to_num) const override

Fill the given map with the type of registered edges as well as the corresponding number of registration of each edge.

virtual void initializeVisuals() override

Initialize visual objects in CDisplayWindow (e.g. add an object to scene).

See also

setWindowManagerPtr, updateVisuals

Throws:

std::exception – If the method is called without having first provided a CDisplayWindow3D* to the class instance

virtual void updateVisuals() override

Update the relevant visual features in CDisplayWindow.

See also

setWindowManagerPtr, initializeVisuals

Throws:

std::exception – If the method is called without having first provided a CDisplayWindow3D* to the class instance

virtual void loadParams(const std::string &source_fname) override

Load the necessary for the decider/optimizer configuration parameters.

virtual void printParams() const override

Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact way.

virtual void getDescriptiveReport(std::string *report_str) const override

Fill the provided string with a detailed report of the decider/optimizer state.

Report should include (part of) the following:

  • Timing of important methods

  • Properties fo class at the current time

  • Logging of commands until current time

void getCurrentPartitions(partitions_t &partitions_out) const
const partitions_t &getCurrentPartitions() const
inline size_t getDijkstraExecutionThresh() const

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

inline 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.

Parameters:

  • groupA – [in] First group to be tested

  • groupB – [in] Second group to be tested

  • generated_hypots – [out] Pool of generated hypothesis. Hypotheses are generated in the heap, so the caller is responsible of afterwards calling delete.

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.

See also

generatePWConsistencyElement

See also

evalPWConsistenciesMatrix

Parameters:

  • groupA – [in] First group to be used

  • groupB – [in] Second group to be used

  • hypots_pool – [in] Pool of hypothesis that has been generated between the two groups \pram[out] consist_matrix Pointer to Pair-wise consistencies matrix that is to be filled

  • groupA_opt_paths – [in] Pointer to vector of optimal paths that can be used instead of making queries to the m_node_optimal_paths class vector. See corresponding argument in generatePWConsistencyElement method

  • groupB_opt_paths – [in]

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.

Call to this method should be made right after generating the consistencies matrix using the generatePWConsistenciesMatrix method

See also

generatePWConsistenciesMatrix

Protected 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.

Parameters:

  • nodeID – [in] ID of node corresponding to the TNodeProps struct that is to be filled

  • group_params – [in] Map of TNodeID to corresponding TNodeProps instance.

  • node_props – [out] Pointer to the TNodeProps struct to be filled.

Returns:

True if operation was successful, false otherwise.

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

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.

If this difference is significantly larger than the rest of of the recorded mahalanobis distances, reject the suggested ICP edge.

See also

getICPEdge

Note

Method updates the Mahalanobis Distance TSlidingWindow which keep track of the recorded mahalanobis distance values.

Returns:

True if suggested ICP edge is accepted

void registerHypothesis(const hypot_t &h)

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

virtual void registerNewEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge) override

Register a new constraint/edge in the current graph.

Implementations of this class should provide a wrapper around GRAPH_T::insertEdge method.

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.

See also

addScanMatchingEdges

virtual void addScanMatchingEdges(const mrpt::graphs::TNodeID &curr_nodeID)

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

void initLaserScansVisualization()
void updateLaserScansVisualization()
void toggleLaserScansVisualization()

togle the LaserScans visualization on and off

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.

void initMapPartitionsVisualization()

Initialize the visualization of the map partition objects.

void updateMapPartitionsVisualization()

Update the map partitions visualization.

void toggleMapPartitionsVisualization()

Toggle the map partitions visualization objects.

To be called upon relevant keystroke press by the user (see TLoopClosureParams::keystroke_map_partitions)

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.

Note

Method is used during the visualization of the map partitions.

Parameters:

  • nodes_list – [in] List of node IDs whose positions are taken into account

  • centroid_coords – [out] Contains the Centroid coordinates as a pair [x,y]

void checkPartitionsForLC(partitions_t *partitions_for_LC)

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

void evaluatePartitionsForLC(const partitions_t &partitions)

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

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.

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:

See also

generatePWConsistenciesMatrix

Parameters:

  • hypots – [in] Hypothesis corresponding to the potential inter-group constraints

  • opt_paths – [in] Vector of optimal paths that can be used instead of making queries to the m_node_optimal_paths class vector. See corresponding argument in generatePWConsistenciesMatrix method

    • 1st element \rightarrow a1->a2 path

    • 2nd element \rightarrow b1->b2 path

Returns:

Pairwise consistency eleement of the composition of transformations

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.

Handy for not having to manually fetch the laser scans, as the method takes care of this.

Parameters:

  • icp_info – [out] Struct that will be filled with the results of the ICP operation

  • ad_params – [in] Pointer to additional parameters in the getICPEdge call

Returns:

True if operation was successful, false otherwise (e.g. if the either of the nodes’ CObservation2DRangeScan object does not contain valid data.

void execDijkstraProjection(mrpt::graphs::TNodeID starting_node = 0, mrpt::graphs::TNodeID ending_node = mrpt::graphs::INVALID_NODEID)

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

Parameters:

  • starting_node – [in] Node from which I start the Dijkstra projection algorithm

  • ending_node – [in] Specify the nodeID whose uncertainty wrt the starting_node, we are interested in computing. If given, method execution ends when this path is computed.

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.

Method takes care of multiple edges, as well as edges with 0 covariance matrices

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.

Removes (and returns) the found path from the pool.

Returns:

Minimum uncertainty path from the pool provided

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.

Parameters:

  • pool_of_paths – [in] Paths that are currently registered

  • curr_path – [in] Path that I am currently traversing. This path is already removed from pool_of_paths

  • neighbors – [in] std::set of neighboring nodes to the last node of the current path

mrpt::graphslam::TUncertaintyPath<GRAPH_T> *queryOptimalPath(const mrpt::graphs::TNodeID node) const

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:

node – [in] nodeID 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.

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. 

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 splitting a
partition of nodes to lower and higher node IDs

\param[in] partition Partition to be split.
\param[out] groupA First group of nodes.
\param[out] groupB Second group of nodes.
\param[in] max_nodes_in_group Max number of nodes that are to exist in
each group (Use -1 to disable this threshold).
void setLastLaserScan2D(mrpt::obs::CObservation2DRangeScan::Ptr scan)

Assign the last recorded 2D Laser scan.

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)

Note

Compact way of assigning the last recorded laser scan for both MRPT rawlog formats.

Protected Attributes

TLaserParams m_laser_params
TLoopClosureParams m_lc_params
mrpt::slam::CIncrementalMapPartitioner m_partitioner

Instance responsible for partitioning the map.

bool m_visualize_curr_node_covariance = false
const mrpt::img::TColor m_curr_node_covariance_color = mrpt::img::TColor(160, 160, 160, 255)
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.

Handy for displaying them in the Visualization window.

mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D

Keep the last laser scan for visualization purposes.

bool m_partitions_full_update = false

Indicate whether the partitions have been updated recently.

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.

std::map<mrpt::graphs::TNodeID, path_t*> m_node_optimal_paths

Map that stores the lowest uncertainty path towards a node. Starting node depends on the starting node as used in the execDijkstraProjection method.

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.

bool m_is_first_time_node_reg = true

Track the first node registration occurance.

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

size_t m_dijkstra_node_count_thresh = 3

Node Count lower bound before executing dijkstra.

double m_consec_icp_constraint_factor

Factor used for accepting an ICP Constraint as valid.

double m_lc_icp_constraint_factor

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

Protected Static 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.

Note

If multiple hypothesis between the same start and end node exist, only the first one is returned.

Parameters:

  • vec_hypots – [in] Vector of hypothesis to check

  • from – [in] Starting Node for hypothesis

  • to – [in] Ending Node for hypothesis

  • throw_exc – [in] If true and hypothesis is not found, throw a HypothesisNotFoundException

Returns:

Pointer to the found hypothesis if that is found, otherwise nullptr.

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.

Note

If multiple paths between the same start and end node exist, only the first one is returned.

Throws:

std::runtime_error – if path was not found and throw_exc is set to true

Returns:

nullptr if a path with the given source and destination NodeIDs is not found, otherwise a pointer to the matching TUncertaintyPath.

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.

Note

If multiple hypothesis with the same ID exist, only the first one is returned.

Parameters:

  • vec_hypots – [in] Vector of hypothesis to check

  • id, ID – [in] of the hypothesis to be returned

  • throw_exc – [in] If true and hypothesis is not found, throw a HypothesisNotFoundException

Returns:

Pointer to the hypothesis with the given ID if that is found, otherwies nullptr.

struct TGenerateHypotsPoolAdParams

Struct for passing additional parameters to the generateHypotsPool call.

Public Types

using group_t = std::map<mrpt::graphs::TNodeID, node_props_t>

Public Members

group_t groupA_params

Ad. params for groupA.

group_t groupB_params

Ad. params for groupB.

struct TGetICPEdgeAdParams

Struct for passing additional parameters to the getICPEdge call.

Handy for overriding the search to the GRAPH_T::nodes map or the search for the node’s LaserScan

Public Types

using self_t = TGetICPEdgeAdParams

Public Functions

inline void getAsString(std::string *str) const
inline std::string getAsString() const

Public Members

node_props_t from_params

Ad. params for the from_node

node_props_t to_params

Ad. params for the to_node

pose_t init_estim

Initial ICP estimation

Friends

inline friend std::ostream &operator<<(std::ostream &o, const self_t &params)
struct TLaserParams : public mrpt::config::CLoadableOptions

Struct for storing together the parameters needed for ICP matching, laser scans visualization etc.

Public Functions

TLaserParams()
~TLaserParams() override
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
virtual void dumpToTextStream(std::ostream &out) const override

Public Members

mrpt::slam::CICP icp
int prev_nodes_for_ICP

How many nodes back to check ICP against?

const mrpt::img::TColor laser_scans_color = mrpt::img::TColor(0, 20, 255)

see Constructor for initialization

bool visualize_laser_scans
std::string keystroke_laser_scans = "l"
bool use_scan_matching

Indicate whethet to use scan-matching at all during graphSLAM [on by default].

Warning

It is strongly recommended that the user does not set this to false (via the .ini file). graphSLAM may diverge significantly if no scan-matching is not used.

bool has_read_config = false
TSlidingWindow mahal_distance_ICP_odom_win

Keep track of the mahalanobis distance between the initial pose difference and the suggested new edge for the pairs of checked nodes.

TSlidingWindow goodness_threshold_win

Keep track of ICP Goodness values for ICP between nearby nodes and adapt the Goodness threshold based on the median of the recorded Goodness values.

struct TLoopClosureParams : public mrpt::config::CLoadableOptions

Struct for storing together the loop-closing related parameters.

Public Functions

TLoopClosureParams()
~TLoopClosureParams() override
virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
virtual void dumpToTextStream(std::ostream &out) const override

Public Members

bool LC_check_curr_partition_only

flag indicating whether to check only the partition of the last registered node for potential loop closures

size_t LC_min_nodeid_diff

nodeID difference for detecting potential loop closure in a partition.

If this difference is surpassed then the partition should be investigated for loop closures using Olson’s strategy.

double LC_eigenvalues_ratio_thresh

Eigenvalues ratio for accepting/rejecting a hypothesis set.

By default this is set to 2.

int LC_min_remote_nodes

how many remote nodes (large nodID difference should there be before I consider the potential loop closure.

int full_partition_per_nodes

Full partition of map only afer X new nodes have been registered.

bool visualize_map_partitions
std::string keystroke_map_partitions
double offset_y_map_partitions
int text_index_map_partitions
const double balloon_elevation = {3}
const double balloon_radius = {0.5}
const mrpt::img::TColor balloon_std_color
const mrpt::img::TColor balloon_curr_color
const mrpt::img::TColor connecting_lines_color
bool has_read_config = {false}