Go to the documentation of this file.
212 template <
class GRAPH_T =
typename mrpt::graphs::CNetworkOfPoses2DInf>
221 using pose_t =
typename GRAPH_T::constraint_t::type_value;
233 std::map<std::pair<hypot_t*, hypot_t*>,
double>;
250 const std::map<std::string, bool>& events_occurred)
override;
252 std::map<std::string, int>* edge_types_to_num)
const override;
256 void loadParams(
const std::string& source_fname)
override;
309 o <<
params.getAsString() << endl;
318 using group_t = std::map<mrpt::graphs::TNodeID, node_props_t>;
337 const std::vector<uint32_t>& groupA,
338 const std::vector<uint32_t>& groupB,
hypotsp_t* generated_hypots,
359 const std::vector<uint32_t>& groupA,
360 const std::vector<uint32_t>& groupB,
const hypotsp_t& hypots_pool,
362 const paths_t* groupA_opt_paths =
nullptr,
363 const paths_t* groupB_opt_paths =
nullptr);
393 const std::map<mrpt::graphs::TNodeID, node_props_t>& group_params,
418 const std::string& section)
override;
466 const std::string& section)
override;
539 std::set<mrpt::graphs::TNodeID>* nodes_set);
555 std::string viz_flag,
int sleep_time = 500 );
558 bool full_update =
false,
bool is_first_time_node_reg =
false);
581 const std::vector<uint32_t>& nodes_list,
582 std::pair<double, double>* centroid_coords)
const;
688 const hypotsp_t& vec_hypots,
size_t id,
bool throw_exc =
true);
737 std::set<path_t*>* pool_of_paths)
const;
747 std::set<path_t*>* pool_of_paths,
const path_t& curr_path,
748 const std::set<mrpt::graphs::TNodeID>& neibors)
const;
780 std::vector<uint32_t>& partition, std::vector<uint32_t>* groupA,
781 std::vector<uint32_t>* groupB,
int max_nodes_in_group = 5);
std::shared_ptr< CObservation > Ptr
const mrpt::img::TColor balloon_curr_color
double m_offset_y_curr_node_covariance
~CLoopCloserERD() override
void setLastLaserScan2D(mrpt::obs::CObservation2DRangeScan::Ptr scan)
Assign the last recorded 2D Laser scan.
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.
void setWindowManagerPtr(mrpt::graphslam::CWindowManager *win_manager) override
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.
void updateMapPartitionsVisualization()
Update the map partitions visualization.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
int text_index_map_partitions
int full_partition_per_nodes
Full partition of map only afer X new nodes have been registered.
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.
void printParams() const override
std::string keystroke_map_partitions
double m_consec_icp_constraint_factor
Factor used for accepting an ICP Constraint as valid.
Struct for passing additional parameters to the generateHypotsPool call.
std::map< mrpt::graphs::TNodeID, path_t * > m_node_optimal_paths
Map that stores the lowest uncertainty path towards a node.
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
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.
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
Keep the last laser scan for visualization purposes.
double offset_y_map_partitions
virtual void addScanMatchingEdges(const mrpt::graphs::TNodeID &curr_nodeID)
Addd ICP constraints from X previous nodeIDs up to the given nodeID.
typename GRAPH_T::edges_map_t::iterator edges_iterator
void getDescriptiveReport(std::string *report_str) const override
const mrpt::img::TColor laser_scans_color
see Constructor for initialization
double LC_eigenvalues_ratio_thresh
Eigenvalues ratio for accepting/rejecting a hypothesis set.
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
void initMapPartitionsVisualization()
Initialize the visualization of the map partition objects.
Struct for storing together the parameters needed for ICP matching, laser scans visualization etc.
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.
void updateCurrCovarianceVisualization()
An edge hypothesis between two nodeIDs.
int prev_nodes_for_ICP
How many nodes back to check ICP against?
TLaserParams m_laser_params
const double balloon_elevation
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
mrpt::vision::TStereoCalibResults out
void updateLaserScansVisualization()
void getAsString(std::string *str) const
std::vector< path_t > paths_t
bool m_is_first_time_node_reg
Track the first node registration occurance.
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.
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.
Class acts as a container for storing pointers to mrpt::gui::CDisplayWindow3D, mrpt::graphslam::CWind...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion) override
This method load the options from a ".ini"-like file or memory-stored string list.
TSlidingWindow goodness_threshold_win
Keep track of ICP Goodness values for ICP between nearby nodes and adapt the Goodness threshold based...
void registerHypothesis(const hypot_t &h)
Wrapper around the registerNewEdge method which accepts a THypothesis object instead.
TSlidingWindow mahal_distance_ICP_odom_win
Keep track of the mahalanobis distance between the initial pose difference and the suggested new edge...
bool visualize_map_partitions
std::map< std::string, int > m_edge_types_to_nums
Keep track of the registered edge types.
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 no...
The ICP algorithm return information.
std::shared_ptr< mrpt::obs ::CObservation2DRangeScan > Ptr
node_props_t from_params
Ad.
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.
bool m_visualize_curr_node_covariance
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
std::map< std::pair< hypot_t *, hypot_t * >, double > hypotsp_to_consist_t
double m_lc_icp_constraint_factor
Factor used for accepting an ICP Constraint in the loop closure proc.
const mrpt::img::TColor m_curr_node_covariance_color
size_t m_dijkstra_node_count_thresh
Node Count lower bound before executing dijkstra.
std::map< mrpt::graphs::TNodeID, node_props_t > group_t
void getEdgesStats(std::map< std::string, int > *edge_types_to_num) const override
bool use_scan_matching
Indicate whethet to use scan-matching at all during graphSLAM [on by default].
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.
pose_t init_estim
Initial ICP estimation.
void checkPartitionsForLC(partitions_t *partitions_for_LC)
Check the registered so far partitions for potential loop closures.
int m_text_index_curr_node_covariance
const mrpt::img::TColor balloon_std_color
typename mrpt::graphs::detail::THypothesis< GRAPH_T > hypot_t
This class allows loading and storing values and vectors of different types from a configuration text...
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
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.
partitions_t m_last_partitions
Previous partitions vector.
void setDijkstraExecutionThresh(size_t new_thresh)
const double balloon_radius
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
std::vector< hypot_t * > hypotsp_t
bool m_partitions_full_update
Indicate whether the partitions have been updated recently.
void getAsString(std::string *str) const
void initLaserScansVisualization()
typename GRAPH_T::constraint_t constraint_t
mrpt::graphslam::deciders::CRangeScanOps< GRAPH_T > range_ops_t
Typedef for accessing methods of the RangeScanRegistrationDecider_t parent class.
const partitions_t & getCurrentPartitions() const
~TLoopClosureParams() override
Struct for storing together the loop-closing related parameters.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
size_t LC_min_nodeid_diff
nodeID difference for detecting potential loop closure in a partition.
mrpt::vision::TStereoCalibParams params
int LC_min_remote_nodes
how many remote nodes (large nodID difference should there be before I consider the potential loop cl...
std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > nodes_to_scans2D_t
std::vector< std::vector< uint32_t > > partitions_t
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.
typename GRAPH_T::global_pose_t global_pose_t
typename parent_t::nodes_to_scans2D_t nodes_to_scans2D_t
mrpt::slam::CIncrementalMapPartitioner m_partitioner
Instance responsible for partitioning the map.
void updateMapPartitions(bool full_update=false, bool is_first_time_node_reg=false)
Split the currently registered graph nodes into partitions.
void evaluatePartitionsForLC(const partitions_t &partitions)
Evaluate the given partitions for loop closures.
void toggleMapPartitionsVisualization()
Toggle the map partitions visualization objects.
bool LC_check_curr_partition_only
flag indicating whether to check only the partition of the last registered node for potential loop cl...
size_t getDijkstraExecutionThresh() const
Return the minimum number of nodes that should exist in the graph prior to running Dijkstra.
void updateVisuals() override
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
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.
Finds partitions in metric maps based on N-cut graph partition theory.
std::vector< hypot_t > hypots_t
Edge Registration Decider Interface from which RangeScanner-based ERDs can inherit from.
Class to monitor the evolution of a statistical quantity.
bool computeDominantEigenVector(const mrpt::math::CMatrixDouble &consist_matrix, mrpt::math::CVectorDouble *eigvec, bool use_power_method=false)
void registerNewEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge) override
const mrpt::img::TColor connecting_lines_color
void loadParams(const std::string &source_fname) 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 e...
void toggleLaserScansVisualization()
togle the LaserScans visualization on and off
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.
std::string getAsString() const
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.
Edge Registration Decider scheme specialized in Loop Closing.
void initializeVisuals() override
typename parent_t::range_ops_t range_ops_t
std::string keystroke_laser_scans
void initCurrCovarianceVisualization()
partitions_t m_curr_partitions
Current partitions vector.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
node_props_t to_params
Ad.
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.
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred) override
bool visualize_laser_scans
TLoopClosureParams m_lc_params
friend std::ostream & operator<<(std::ostream &o, const self_t ¶ms)
Holds the data of an information path.
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 ...
typename GRAPH_T::constraint_t::type_value pose_t
std::string std::string format(std::string_view fmt, ARGS &&... args)
Struct for passing additional parameters to the getICPEdge call.
typename GRAPH_T::edges_map_t::const_iterator edges_citerator
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.
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.
mrpt::graphslam::TUncertaintyPath< GRAPH_T > * queryOptimalPath(const mrpt::graphs::TNodeID node) const
Query for the optimal path of a nodeID.
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020 | |