MRPT
2.0.1
|
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:
"This quantity is proportional to the probability density that the rigid-body transformation is the identity matrix (i.e., T = [0 0 0])"This is inconsistent with the given formula. Suppose that a wrong loop closure is included in the , therefore the pairwise-consistency element should have a low value. For this to hold true the exponent of the consistency element shoud be small and neglecting the covariance matrix of rigid-body transformation (e.g. unit covariance matrix), should be small. When a wrong loop closure is evaluated the aforementioned quantity increases since the hypotheses do not form a correct loop. Therefore the worse the rigid-body transformation the higher the exponent term, therefore the higher the consistency element
Definition at line 213 of file CLoopCloserERD.h.
#include <mrpt/graphslam/ERD/CLoopCloserERD.h>
Classes | |
struct | TGenerateHypotsPoolAdParams |
Struct for passing additional parameters to the generateHypotsPool call. More... | |
struct | TGetICPEdgeAdParams |
Struct for passing additional parameters to the getICPEdge call. More... | |
struct | TLaserParams |
Struct for storing together the parameters needed for ICP matching, laser scans visualization etc. More... | |
struct | TLoopClosureParams |
Struct for storing together the loop-closing related parameters. More... | |
Public Types | |
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... | |
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::constraint_t = typename GRAPH_T::constraint_t |
Definition at line 220 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::decider_t = CLoopCloserERD<GRAPH_T> |
self type
Definition at line 223 of file CLoopCloserERD.h.
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.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::edges_iterator = typename GRAPH_T::edges_map_t::iterator |
Definition at line 228 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::global_pose_t = typename GRAPH_T::global_pose_t |
Definition at line 222 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypot_t = typename mrpt::graphs::detail::THypothesis<GRAPH_T> |
Definition at line 229 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypots_t = std::vector<hypot_t> |
Definition at line 230 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::hypotsp_t = std::vector<hypot_t*> |
Definition at line 231 of file CLoopCloserERD.h.
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.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::node_props_t = mrpt::graphslam::detail::TNodeProps<GRAPH_T> |
Definition at line 236 of file CLoopCloserERD.h.
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.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::parent_t = CRangeScanEdgeRegistrationDecider<GRAPH_T> |
Edge Registration Decider.
Definition at line 218 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::partitions_t = std::vector<std::vector<uint32_t> > |
Definition at line 226 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::path_t = mrpt::graphslam::TUncertaintyPath<GRAPH_T> |
Definition at line 234 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::paths_t = std::vector<path_t> |
Definition at line 235 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::pose_t = typename GRAPH_T::constraint_t::type_value |
Definition at line 221 of file CLoopCloserERD.h.
using mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::range_ops_t = typename parent_t::range_ops_t |
Definition at line 224 of file CLoopCloserERD.h.
mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::CLoopCloserERD | ( | ) |
Definition at line 22 of file CLoopCloserERD_impl.h.
References MRPT_LOG_DEBUG_STREAM.
|
override |
Definition at line 31 of file CLoopCloserERD_impl.h.
|
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)
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.
|
protected |
Append the paths starting from the current node.
[in] | pool_of_paths | Paths that are currently registered |
[in] | curr_path | Path that I am currently traversing. This path is already removed from pool_of_paths |
[in] | neighbors | std::set of neighboring nodes to the last node of the current path |
Definition at line 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.
|
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.
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.
|
protected |
Compute the Centroid of a group of a vector of node positions.
[in] | nodes_list | List of node IDs whose positions are taken into account |
[out] | centroid_coords | Contains the Centroid coordinates as a pair [x,y] |
Definition at line 2023 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
|
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.
|
protected |
Definition at line 2286 of file CLoopCloserERD_impl.h.
References mrpt::system::LVL_ERROR, MRPT_END, and MRPT_START.
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
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().
|
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
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.
|
protected |
compute the minimum uncertainty of each node position with regards to the graph root.
[in] | starting_node | Node from which I start the Dijkstra projection algorithm |
[in] | ending_node | Specify the nodeID whose uncertainty wrt the starting_node, we are interested in computing. If given, method execution ends when this path is computed. |
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.
|
protectedvirtual |
Fetch a list of nodes with regards prior to the given nodeID for which to try and add scan matching edges.
Definition at line 151 of file CLoopCloserERD_impl.h.
References ASSERTDEB_.
|
protected |
Fill the TNodeProps instance using the parameters from the map.
[in] | nodeID | ID of node corresponding to the TNodeProps struct that is to be filled |
[in] | group_params | Map of TNodeID to corresponding TNodeProps instance. |
[out] | node_props | Pointer to the TNodeProps struct to be filled. |
Definition at line 306 of file CLoopCloserERD_impl.h.
References ASSERTDEB_.
|
staticprotected |
Given a vector of THypothesis objects, find the one that has the given start and end nodes.
[in] | vec_hypots | Vector of hypothesis to check |
[in] | from | Starting Node for hypothesis |
[in] | to | Ending Node for hypothesis |
[in] | throw_exc | If true and hypothesis is not found, throw a HypothesisNotFoundException |
Definition at line 1232 of file CLoopCloserERD_impl.h.
|
staticprotected |
Given a vector of THypothesis objects, find the one that has the given ID.
[in] | vec_hypots | Vector of hypothesis to check |
[in] | id,ID | of the hypothesis to be returned |
[in] | throw_exc | If true and hypothesis is not found, throw a HypothesisNotFoundException |
Definition at line 1262 of file CLoopCloserERD_impl.h.
|
staticprotected |
Given a vector of TUncertaintyPath objects, find the one that has the given source and destination nodeIDs.
std::runtime_error | if 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.
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.
[in] | groupA | First group to be tested |
[in] | groupB | Second group to be tested |
[out] | generated_hypots | Pool of generated hypothesis. Hypotheses are generated in the heap, so the caller is responsible of afterwards calling delete. |
Definition at line 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.
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.
[in] | groupA | First group to be used |
[in] | groupB | Second group to be used |
[in] | hypots_pool | Pool of hypothesis that has been generated between the two groups [out] consist_matrix Pointer to Pair-wise consistencies matrix that is to be filled |
[in] | groupA_opt_paths | Pointer to vector of optimal paths that can be used instead of making queries to the m_node_optimal_paths class vector. See corresponding argument in generatePWConsistencyElement method |
[in] | groupB_opt_paths |
Definition at line 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().
|
protected |
Return the pair-wise consistency between the observations of the given nodes.
For the tranformation matrix of the loop use the following edges
Given the transformation vector of the above composition (e.g. T) the pairwise consistency element would then be:
[in] | hypots | Hypothesis corresponding to the potential inter-group constraints |
[in] | opt_paths | Vector of optimal paths that can be used instead of making queries to the m_node_optimal_paths class vector. See corresponding argument in generatePWConsistenciesMatrix method
|
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().
void mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getCurrentPartitions | ( | partitions_t & | partitions_out | ) | const |
Definition at line 2389 of file CLoopCloserERD_impl.h.
const std::vector< std::vector< uint32_t > > & mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::getCurrentPartitions | ( | ) | const |
Definition at line 2397 of file CLoopCloserERD_impl.h.
|
override |
Definition at line 2356 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
|
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.
|
override |
Definition at line 2149 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
|
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.
[out] | icp_info | Struct that will be filled with the results of the ICP operation |
[in] | ad_params | Pointer to additional parameters in the getICPEdge call |
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.
|
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().
|
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
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.
|
protected |
Definition at line 2211 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
|
override |
Definition at line 2158 of file CLoopCloserERD_impl.h.
References ASSERTDEBMSG_, MRPT_END, and MRPT_START.
|
protected |
Definition at line 2050 of file CLoopCloserERD_impl.h.
References mrpt::opengl::CPlanarLaserScan::Create(), MRPT_END, and MRPT_START.
|
protected |
Initialize the visualization of the map partition objects.
Definition at line 1761 of file CLoopCloserERD_impl.h.
|
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().
|
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.
Definition at line 1630 of file CLoopCloserERD_impl.h.
References mrpt::math::mahalanobisDistance2(), MRPT_END, and MRPT_START.
|
override |
Definition at line 1740 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
|
protected |
Find the minimum uncertainty path from te given pool of TUncertaintyPath instances.
Removes (and returns) the found path from the pool.
Definition at line 1601 of file CLoopCloserERD_impl.h.
References ASSERTDEB_, mrpt::graphslam::TUncertaintyPath< GRAPH_T >::getDeterminant(), MRPT_END, and MRPT_START.
|
override |
Definition at line 2332 of file CLoopCloserERD_impl.h.
References MRPT_END, MRPT_LOG_DEBUG_STREAM, and MRPT_START.
|
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.
[in] | node | nodeID for which hte path is going to be returned |
Definition at line 1472 of file CLoopCloserERD_impl.h.
|
protected |
Wrapper around the registerNewEdge method which accepts a THypothesis object instead.
Definition at line 1677 of file CLoopCloserERD_impl.h.
|
overrideprotected |
Definition at line 1685 of file CLoopCloserERD_impl.h.
References mrpt::math::absDiff(), MRPT_END, MRPT_LOG_INFO, and MRPT_START.
|
inline |
Definition at line 269 of file CLoopCloserERD.h.
References mrpt::graphslam::deciders::CLoopCloserERD< GRAPH_T >::m_dijkstra_node_count_thresh.
|
protected |
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)
|
override |
Definition at line 1716 of file CLoopCloserERD_impl.h.
References MRPT_LOG_DEBUG.
|
protected |
Split an existing partition to Groups.
Have two groups A, B.
[in] | partition | Partition to be split. |
[out] | groupA | First group of nodes. |
[out] | groupB | Second group of nodes. |
[in] | max_nodes_in_group | Max number of nodes that are to exist in each group (Use -1 to disable this threshold). |
Definition at line 652 of file CLoopCloserERD_impl.h.
References ASSERTDEB_, ASSERTDEBMSG_, mrpt::format(), MRPT_END, and MRPT_START.
|
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.
|
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.
|
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.
|
protected |
Definition at line 2082 of file CLoopCloserERD_impl.h.
References mrpt::DEG2RAD(), MRPT_END, and MRPT_START.
|
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.
|
protected |
Update the map partitions visualization.
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.
|
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.
|
override |
Definition at line 2186 of file CLoopCloserERD_impl.h.
References MRPT_END, and MRPT_START.
|
protected |
Factor used for accepting an ICP Constraint as valid.
Definition at line 841 of file CLoopCloserERD.h.
|
protected |
Definition at line 796 of file CLoopCloserERD.h.
|
protected |
Current partitions vector.
Definition at line 813 of file CLoopCloserERD.h.
|
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().
|
protected |
Keep track of the registered edge types.
Handy for displaying them in the Visualization window.
Definition at line 805 of file CLoopCloserERD.h.
|
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.
|
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.
|
protected |
Definition at line 508 of file CLoopCloserERD.h.
|
protected |
Keep the last laser scan for visualization purposes.
Definition at line 807 of file CLoopCloserERD.h.
|
protected |
Previous partitions vector.
Definition at line 811 of file CLoopCloserERD.h.
|
protected |
Factor used for accepting an ICP Constraint in the loop closure proc.
Definition at line 845 of file CLoopCloserERD.h.
|
protected |
Definition at line 509 of file CLoopCloserERD.h.
|
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.
|
protected |
Definition at line 798 of file CLoopCloserERD.h.
|
protected |
Instance responsible for partitioning the map.
Definition at line 793 of file CLoopCloserERD.h.
|
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.
|
protected |
Indicate whether the partitions have been updated recently.
Definition at line 816 of file CLoopCloserERD.h.
|
protected |
Definition at line 799 of file CLoopCloserERD.h.
|
protected |
Definition at line 795 of file CLoopCloserERD.h.
Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020 |