MRPT  2.0.4
CLoopCloserERD.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
14 #include <mrpt/img/TColor.h>
15 #include <mrpt/math/CMatrixF.h>
18 #include <mrpt/obs/CSensoryFrame.h>
19 #include <mrpt/slam/CICP.h>
21 
28 
29 #include <map>
30 #include <set>
31 #include <string>
32 #include <utility>
33 #include <vector>
34 
36 {
37 /**\brief Edge Registration Decider scheme specialized in Loop Closing.
38  *
39  * ## Description
40  *
41  * Current decider is implemented based on the following papers:
42  *
43  * - [1] <a
44  *
45 href="http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=1641810&url=http%3A%2F%2Fieeexplore.ieee.org%2Fxpls%2Fabs_all.jsp%3Farnumber%3D1641810">Consistent
46  * Observation Grouping for Generating Metric-Topological Maps that Improves
47  * Robot Localization</a> - J. Blanco, J. Gonzalez, J. Antonio Fernandez
48  * Madrigal, 2006
49  * - [2] <a
50 href="https://april.eecs.umich.edu/pdfs/olson2009ras.pdf">Recognizing
51  * places using spectrally clustered local matches</a> - E. Olson, 2009
52  *
53  * ### Specifications
54  *
55  * - Map type: 2D
56  * - MRPT rawlog format: #1, #2
57  * - Graph Type: CPosePDFGaussianInf
58  * - Observations: CObservation2DRangeScan
59  * - Edge Registration Strategy: ICP Scan-matching between nearby nodes
60  * - Loop Registration Strategy: Pair-wise Consistency of ICP Edges
61  *
62  * ### Loop Closing Strategy
63  *
64  * The Loop closure registration strategy is described below:
65  *
66  * - We split the graph under-construction into groups of nodes. The groups are
67  * formatted based on the observations gathered in each node position. The
68  * actual split between the groups is decided by the minimum normalized Cut
69  * (minNcut) as described in [1].
70  * - Having assembled the groups of nodes, we find the groups that might
71  * contain loop closure edges (these groups contain successive nodes with
72 large
73  * difference in their IDs). These groups are then split into two subgroups
74  * A, B, with the former containing the lower NodeIDs and the latter the
75  * higher. To minimize computational cost as well as the possibility of wrong
76  * loop closure registration, we search for loop closures only between the
77  * <a>last</a> nodes of group B and the <a>first</a> nodes of group A. Based
78 on [2] the
79  * potential loop closure edges are not evaluated individually but rather in
80  * sets. Refer to [2] for insight on the algorithm and to
81  * evaluatePartitionsForLC method for the actual implementation. See below
82  * for images of potential Loop closure edges.
83  *
84  * <div>
85  * <div>
86  * <img src="graphslam-engine_loop_closing_full.png">
87  * <caption align="bottom">
88  * Loop closing schematic. blue nodes belong to group A and have lower
89  * NodeIDs, while red nodes belong to group B and have higher NodeIDs.
90  * Nodes of both groups A and B belong to the same partition based on their
91  * corresponding 2DRangeScan observations.
92  * </caption>
93  * </div>
94  * <div>
95  * <img src="graphslam-engine_loop_closing_consistency_element.png">
96  * <caption align="bottom">
97  * Rigid body transformation of hypotheses and two corresponding
98  * dijkstra links
99  * </caption>
100  * </div>
101  * </div>
102  *
103  * \note
104  * Olson uses the following formula for evaluating the pairwise
105  * consistency between two hypotheses i,j:
106  * <br><center> \f$ A_{i,j} = e^{T \Sigma_T^{-1} T^T} \f$ </center>
107  *
108  * \note
109  * Where:
110  * - T is the rigid body transformation using the two hypotheses and the two
111  * Dijkstra Links connecting the nodes that form the hypotheses
112  * - \f$ \Sigma_T \f$ is the covariance matrix of the aforementioned rigid
113  * body transformation
114  *
115  * \note
116  * However this formula is <a>inconsistent with the rest of the paper
117  * explanations and mathematical formulas </a>:
118  * - The author states that:
119  * \verbatim
120 "This quantity is proportional to the probability density that the rigid-body
121 transformation is the identity matrix (i.e., T = [0 0 0])"
122 \endverbatim
123  * This is \a inconsistent with the given formula. Suppose that a
124  * wrong loop closure is included in the \f$ A_{i,j} \f$, therefore the
125  * pairwise-consistency element should have a low value. For this to hold
126  * true the exponent of the consistency element shoud be small and
127  * neglecting the covariance matrix of rigid-body transformation (e.g. unit
128  * covariance matrix), \f$ T T^T \f$ should be small.
129  * When a wrong loop closure is evaluated the aforementioned quantity
130  * increases since the hypotheses do not form a correct loop. Therefore the
131  * worse the rigid-body transformation the higher the exponent term,
132  * therefore the higher the consistency element
133  * - Author uses the information matrix \f$ \Sigma_T^{-1} \f$ in the
134 exponential.
135  * However in the optimal case (high certainty of two correct loop closure
136  * hypotheses) information matrix and rigid body transformation vector T
137  * have opposite effects in the exponent term:
138  * - \f$ \text{Correct loop closure} \Rightarrow T \rightarrow [0, 0, 0]
139  * \Rightarrow \text{exponent} \downarrow \f$
140  * - \f$ \text{Correct loop closure} \Rightarrow
141  * \text{diagonal\_terms}(\Sigma_T^{-1}) \uparrow \Rightarrow \text{exponent}
142  * \uparrow \f$
143  *
144  * \note
145  * Based on the previous comments the following formula is used in the decider:
146  * <br><center> \f$ A_{i,j} = e^{-T \Sigma_T T^T} \f$ </center>
147  *
148  * ### .ini Configuration Parameters
149  *
150  * \htmlinclude graphslam-engine_config_params_preamble.txt
151  *
152  * - \b class_verbosity
153  * + \a Section : EdgeRegistrationDeciderParameters
154  * + \a Default value : 1 (mrpt::system::LVL_INFO)
155  * + \a Required : FALSE
156  *
157  * - \b use_scan_matching
158  * + \a Section : EdgeRegistrationDeciderParameters
159  * + \a Default value : TRUE
160  * + \a Required : FALSE
161  * + \a Description :Indicates whether the decider uses scan matching
162  * between the current and previous laser scans to correct the robot
163  * trajectory.
164  *
165  * - \b visualize_laser_scans
166  * + \a Section : VisualizationParameters
167  * + \a Default value : 10
168  * + \a Required : FALSE
169  *
170  * - \b LC_min_nodeid_diff
171  * + \a Section : GeneralConfiguration
172  * + \a Default value : 30
173  * + \a Required : FALSE
174  * + \a Description : Minimum NodeID difference for an edge to be considered
175  * a loop closure.
176  *
177  * - \b LC_min_remote_nodes
178  * + \a Section : EdgeRegistrationDeciderParameters
179  * + \a Default value : 3
180  * + \a Required : FALSE
181  * + \a Description : Number of remote nodes that must exist in order for
182  * the Loop Closure procedure to commence
183  *
184  * - \b LC_eigenvalues_ratio_thresh
185  * + \a Section : EdgeRegistrationDeciderParameters
186  * + \a Default value : 2
187  * + \a Required : FALSE
188  * + \a Description : Minimum ratio of the two dominant eigenvalues for a
189  * loop closing hypotheses set to be considered valid
190  *
191  * - \b LC_check_curr_partition_only
192  * + \a Section : EdgeRegistrationDeciderParameters
193  * + \a Default value : TRUE
194  * + \a Required : FALSE
195  * + \a Description : Boolean flag indicating whether to check for loop
196  * closures only in the current node's partition
197  *
198  * - \b visualize_map_partitions
199  * + \a Section : VisualizationParameters
200  * + \a Default value : TRUE
201  * + \a Required : FALSE
202  *
203  * \note Class contains an instance of the
204  * mrpt::slam::CIncrementalMapPartitioner class and it parses the configuration
205  * parameters of the latter from the "EdgeRegistrationDeciderParameters"
206  * section. Refer to mrpt::slam::CIncrementalMapPartitioner documentation for
207  * its list of configuration parameters
208  *
209  * \sa mrpt::slam::CIncrementalMapPartitioner
210  * \ingroup mrpt_graphslam_grp
211  */
212 template <class GRAPH_T = typename mrpt::graphs::CNetworkOfPoses2DInf>
215 {
216  public:
217  /**\brief Edge Registration Decider */
219 
220  using constraint_t = typename GRAPH_T::constraint_t;
221  using pose_t = typename GRAPH_T::constraint_t::type_value;
222  using global_pose_t = typename GRAPH_T::global_pose_t;
223  using decider_t = CLoopCloserERD<GRAPH_T>; /**< self type */
226  using partitions_t = std::vector<std::vector<uint32_t>>;
227  using edges_citerator = typename GRAPH_T::edges_map_t::const_iterator;
228  using edges_iterator = typename GRAPH_T::edges_map_t::iterator;
230  using hypots_t = std::vector<hypot_t>;
231  using hypotsp_t = std::vector<hypot_t*>;
232  using hypotsp_to_consist_t =
233  std::map<std::pair<hypot_t*, hypot_t*>, double>;
235  using paths_t = std::vector<path_t>;
237  /**\}*/
238 
239  CLoopCloserERD();
240  ~CLoopCloserERD() override;
241 
242  bool updateState(
244  mrpt::obs::CSensoryFrame::Ptr observations,
245  mrpt::obs::CObservation::Ptr observation) override;
246 
247  void setWindowManagerPtr(
248  mrpt::graphslam::CWindowManager* win_manager) override;
250  const std::map<std::string, bool>& events_occurred) override;
251  void getEdgesStats(
252  std::map<std::string, int>* edge_types_to_num) const override;
253 
254  void initializeVisuals() override;
255  void updateVisuals() override;
256  void loadParams(const std::string& source_fname) override;
257  void printParams() const override;
258 
259  void getDescriptiveReport(std::string* report_str) const override;
260  void getCurrentPartitions(partitions_t& partitions_out) const;
261  const partitions_t& getCurrentPartitions() const;
262  /**\brief Return the minimum number of nodes that should exist in the graph
263  * prior to running Dijkstra
264  */
266  {
268  }
269  void setDijkstraExecutionThresh(size_t new_thresh)
270  {
271  m_dijkstra_node_count_thresh = new_thresh;
272  }
273 
274  /**\name Helper structs */
275  /**\{ */
276 
277  /**
278  * \brief Struct for passing additional parameters to the getICPEdge call
279  *
280  * Handy for overriding the search to the \a GRAPH_T::nodes map or the
281  * search for the node's LaserScan
282  */
284  {
286 
287  node_props_t from_params; /**< Ad. params for the from_node */
288  node_props_t to_params; /**< Ad. params for the to_node */
289  pose_t init_estim; /**< Initial ICP estimation */
290 
291  void getAsString(std::string* str) const
292  {
293  str->clear();
294  *str += mrpt::format(
295  "from_params: %s", from_params.getAsString().c_str());
296  *str +=
297  mrpt::format("to_params: %s", to_params.getAsString().c_str());
298  *str +=
299  mrpt::format("init_estim: %s\n", init_estim.asString().c_str());
300  }
301  std::string getAsString() const
302  {
303  std::string str;
304  this->getAsString(&str);
305  return str;
306  }
307  friend std::ostream& operator<<(std::ostream& o, const self_t& params)
308  {
309  o << params.getAsString() << endl;
310  return o;
311  }
312  };
313  /**\brief Struct for passing additional parameters to the
314  * generateHypotsPool call
315  */
317  {
318  using group_t = std::map<mrpt::graphs::TNodeID, node_props_t>;
319 
320  /**\brief Ad. params for groupA */
322  /**\brief Ad. params for groupB */
324  };
325  /**\} */
326 
327  /**\brief Generate the hypothesis pool for all the inter-group constraints
328  * between two groups of nodes.
329  *
330  * \param[in] groupA First group to be tested
331  * \param[in] groupB Second group to be tested
332  * \param[out] generated_hypots Pool of generated hypothesis. Hypotheses
333  * are generated in the heap, so the caller is responsible of afterwards
334  * calling \a delete.
335  */
336  void generateHypotsPool(
337  const std::vector<uint32_t>& groupA,
338  const std::vector<uint32_t>& groupB, hypotsp_t* generated_hypots,
339  const TGenerateHypotsPoolAdParams* ad_params = nullptr);
340  /**\brief Compute the pair-wise consistencies Matrix.
341  *
342  * \param[in] groupA First group to be used
343  * \param[in] groupB Second group to be used
344  * \param[in] hypots_pool Pool of hypothesis that has been generated
345  * between the two groups
346  * \pram[out] consist_matrix Pointer to Pair-wise consistencies matrix that
347  * is to be filled
348 
349  * \param[in] groupA_opt_paths Pointer to vector of optimal paths that can
350  * be used instead of making queries to the m_node_optimal_paths class
351  * vector. See corresponding argument in generatePWConsistencyElement
352  * method
353  * \param[in] groupB_opt_paths
354  *
355  * \sa generatePWConsistencyElement
356  * \sa evalPWConsistenciesMatrix
357  */
359  const std::vector<uint32_t>& groupA,
360  const std::vector<uint32_t>& groupB, const hypotsp_t& hypots_pool,
361  mrpt::math::CMatrixDouble* consist_matrix,
362  const paths_t* groupA_opt_paths = nullptr,
363  const paths_t* groupB_opt_paths = nullptr);
364  /**\brief Evalute the consistencies matrix, fill the valid hypotheses
365  *
366  * Call to this method should be made right after generating the
367  * consistencies matrix using the generatePWConsistenciesMatrix method
368  *
369  * \sa generatePWConsistenciesMatrix
370  */
372  const mrpt::math::CMatrixDouble& consist_matrix,
373  const hypotsp_t& hypots_pool, hypotsp_t* valid_hypots);
374  // Public variables
375  // ////////////////////////////
376  protected:
377  // protected functions
378  //////////////////////////////////////////////////////////////
379 
380  /**\brief Fill the TNodeProps instance using the parameters from the map
381  *
382  * \param[in] nodeID ID of node corresponding to the TNodeProps struct that
383  * is to be filled
384  * \param[in] group_params Map of TNodeID to corresponding TNodeProps
385  * instance.
386  * \param[out] node_props Pointer to the TNodeProps struct to be filled.
387  *
388  *
389  * \return True if operation was successful, false otherwise.
390  */
392  const mrpt::graphs::TNodeID& nodeID,
393  const std::map<mrpt::graphs::TNodeID, node_props_t>& group_params,
394  node_props_t* node_props);
395  /**\brief Fill the pose and LaserScan for the given nodeID.
396  * Pose and LaserScan are either fetched from the TNodeProps struct if it
397  * contains valid data, otherwise from the corresponding class vars
398  *
399  * \return True if operation was successful and pose, scan contain valid
400  * data.
401  */
402  bool getPropsOfNodeID(
403  const mrpt::graphs::TNodeID& nodeID, global_pose_t* pose,
405  const node_props_t* node_props = nullptr) const;
406 
407  /**\brief Struct for storing together the parameters needed for ICP
408  * matching, laser scans visualization etc.
409  */
411  {
412  public:
413  TLaserParams();
414  ~TLaserParams() override;
415 
416  void loadFromConfigFile(
417  const mrpt::config::CConfigFileBase& source,
418  const std::string& section) override;
419  void dumpToTextStream(std::ostream& out) const override;
421  /**\brief How many nodes back to check ICP against?
422  */
424 
425  /** see Constructor for initialization */
427  mrpt::img::TColor(0, 20, 255);
429  // keystroke to be used by the user to toggle the LaserScans from
430  // the CDisplayWindow
431  std::string keystroke_laser_scans = "l";
432 
433  /**\brief Indicate whethet to use scan-matching at all during
434  * graphSLAM [on by default].
435  *
436  * \warning It is strongly recomended that the user does not set this
437  * to false (via the .ini file). graphSLAM may diverge significantly if
438  * no scan-matching is not used.
439  */
441  bool has_read_config = false;
442  ;
443  /**\brief Keep track of the mahalanobis distance between the initial
444  * pose
445  * difference and the suggested new edge for the pairs of checked
446  * nodes.
447  */
449  /**\brief Keep track of ICP Goodness values for ICP between nearby
450  * nodes and adapt the Goodness threshold based on the median of the
451  * recorded Goodness values.
452  */
454  };
455 
456  /**\brief Struct for storing together the loop-closing related parameters.
457  */
459  {
460  public:
462  ~TLoopClosureParams() override;
463 
464  void loadFromConfigFile(
465  const mrpt::config::CConfigFileBase& source,
466  const std::string& section) override;
467  void dumpToTextStream(std::ostream& out) const override;
468  /**\brief flag indicating whether to check only the partition of the
469  * last
470  * registered node for potential loop closures
471  */
473  /**\brief nodeID difference for detecting potential loop closure in a
474  * partition.
475  *
476  * If this difference is surpassed then the partition should be
477  * investigated for loop closures using Olson's strategy.
478  */
480  /**\brief Eigenvalues ratio for accepting/rejecting a hypothesis set.
481  *
482  * By default this is set to 2.
483  */
485  /**\brief how many remote nodes (large nodID difference should there be
486  * before I consider the potential loop closure.
487  */
489  /**\brief Full partition of map only afer X new nodes have been
490  * registered
491  */
495 
498 
499  // map partitioning - visualization window parameters
500  const double balloon_elevation{3};
501  const double balloon_radius{0.5};
505 
506  bool has_read_config{false};
507  };
510 
511  /**brief Compare the suggested ICP edge against the initial node
512  * difference.
513  *
514  * If this difference is significantly larger than the rest of of the
515  * recorded mahalanobis distances, reject the suggested ICP edge.
516  *
517  * \return True if suggested ICP edge is accepted
518  * \note Method updates the Mahalanobis Distance TSlidingWindow which
519  * keep track of the recorded mahalanobis distance values.
520  * \sa getICPEdge
521  */
523  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
524  const constraint_t& rel_edge);
525  /**\brief Wrapper around the registerNewEdge method which accepts a
526  * THypothesis object instead.
527  */
528  void registerHypothesis(const hypot_t& h);
529  void registerNewEdge(
530  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
531  const constraint_t& rel_edge) override;
532  /**\brief Fetch a list of nodes with regards prior to the given nodeID for
533  * which to try and add scan matching edges
534  *
535  * \sa addScanMatchingEdges
536  */
537  virtual void fetchNodeIDsForScanMatching(
538  const mrpt::graphs::TNodeID& curr_nodeID,
539  std::set<mrpt::graphs::TNodeID>* nodes_set);
540  /**\brief Addd ICP constraints from X previous nodeIDs up to the given
541  * nodeID.
542  *
543  * X is set by the user in the .ini configuration file (see
544  * TLaserParams::prev_nodes_for_ICP)
545  *
546  * \sa fetchNodeIDsForScanMatching
547  */
548  virtual void addScanMatchingEdges(const mrpt::graphs::TNodeID& curr_nodeID);
551  /**\brief togle the LaserScans visualization on and off
552  */
555  std::string viz_flag, int sleep_time = 500 /* ms */);
556  /**\brief Split the currently registered graph nodes into partitions. */
557  void updateMapPartitions(
558  bool full_update = false, bool is_first_time_node_reg = false);
559  /**\brief Initialize the visualization of the map partition objects. */
561  /**\brief Update the map partitions visualization. */
563  /**\brief Toggle the map partitions visualization objects.
564  *
565  * To be called upon relevant keystroke press by the user (see \b
566  * TLoopClosureParams::keystroke_map_partitions)
567  */
571  /**\brief Compute the Centroid of a group of a vector of node positions.
572  *
573  * \param[in] nodes_list List of node IDs whose positions are taken into
574  * account
575  * \param[out] centroid_coords Contains the Centroid coordinates as a pair
576  * [x,y]
577  *
578  * \note Method is used during the visualization of the map partitions.
579  */
581  const std::vector<uint32_t>& nodes_list,
582  std::pair<double, double>* centroid_coords) const;
583  /**\brief Check the registered so far partitions for potential loop
584  * closures.
585  *
586  * Practically checks whether there exist nodes in a single partition whose
587  * distance surpasses the minimum loop closure nodeID distance. The latter
588  * is read from a .ini external file, thus specified by the user (see \b
589  * TLoopClosureParams.LC_min_nodeid_diff.
590  *
591  * \sa evaluatePartitionsForLC
592  */
593  void checkPartitionsForLC(partitions_t* partitions_for_LC);
594  /**\brief Evaluate the given partitions for loop closures.
595  *
596  * Call this method when you have identified potential loop closures - e.g.
597  * far away nodes in the same partitions - and you want to evaluate the
598  * potential hypotheses in the group. Comprises the main function that
599  * tests potential loop closures in <b>partitions of nodes</b>
600  *
601  * \sa checkPartitionsForLC
602  */
603  void evaluatePartitionsForLC(const partitions_t& partitions);
604 
606  const mrpt::math::CMatrixDouble& consist_matrix,
607  mrpt::math::CVectorDouble* eigvec, bool use_power_method = false);
608  /**\brief Return the pair-wise consistency between the observations of the
609  * given nodes.
610  *
611  * For the tranformation matrix of the loop use the following edges
612  * - a1=>a2 (Dijkstra Link)
613  * - a2=>b1 (hypothesis - ICP edge)
614  * - b1=>b2 (Dijkstra Link)
615  * - b2=>a1 (hypothesis - ICP edge)
616  *
617  * Given the transformation vector \f$ (x,y,\phi)\f$ of the above
618  * composition (e.g. T) the
619  * pairwise consistency element would then be:
620  * <br><center> \f$ A_{i,j} = e^{-T \Sigma_T T^T} \f$ </center>
621  *
622  * \param[in] hypots Hypothesis corresponding to the potential inter-group
623  * constraints
624  * \param[in] opt_paths Vector of optimal paths that can be used instead of
625  * making queries to the m_node_optimal_paths class vector. See
626  * corresponding argument in generatePWConsistenciesMatrix method
627  * - 1st element \rightarrow a1->a2 path
628  * - 2nd element \rightarrow b1->b2 path
629  *
630  * \return Pairwise consistency eleement of the composition of
631  * transformations
632  *
633  * \sa generatePWConsistenciesMatrix
634  */
638  const hypotsp_t& hypots, const paths_t* opt_paths = nullptr);
639  /**\brief Given a vector of THypothesis objects, find the one that
640  * has the given start and end nodes.
641  *
642  * \note If multiple hypothesis between the same start and end node exist,
643  * only the first one is returned.
644  *
645  * \param[in] vec_hypots Vector of hypothesis to check
646  * \param[in] from Starting Node for hypothesis
647  * \param[in] to Ending Node for hypothesis
648  * \param[in] throw_exc If true and hypothesis is not found, <b>throw a
649  * HypothesisNotFoundException</b>
650  *
651  * \return Pointer to the found hypothesis if that is found, otherwise
652  * nullptr.
653  *
654  */
655  static hypot_t* findHypotByEnds(
656  const hypotsp_t& vec_hypots, const mrpt::graphs::TNodeID& from,
657  const mrpt::graphs::TNodeID& to, bool throw_exc = true);
658  /**\brief Given a vector of TUncertaintyPath objects, find the one that has
659  * the given source and destination nodeIDs.
660  *
661  * \note If multiple paths between the same start and end node exist,
662  * only the first one is returned.
663  *
664  * \return nullptr if a path with the given source and destination NodeIDs
665  * is not found, otherwise a pointer to the matching TUncertaintyPath.
666  *
667  * \exception std::runtime_error if path was not found and throw_exc is set
668  * to true
669  */
670  static const path_t* findPathByEnds(
671  const paths_t& vec_paths, const mrpt::graphs::TNodeID& src,
672  const mrpt::graphs::TNodeID& dst, bool throw_exc = true);
673  /**\brief Given a vector of THypothesis objects, find the one that
674  * has the given ID.
675  *
676  * \note If multiple hypothesis with the same ID exist, only the first one
677  * is returned.
678  *
679  * \param[in] vec_hypots Vector of hypothesis to check
680  * \param[in] id, ID of the hypothesis to be returned
681  * \param[in] throw_exc If true and hypothesis is not found, <b>throw a
682  * HypothesisNotFoundException</b>
683  *
684  * \return Pointer to the hypothesis with the given ID if that is found,
685  * otherwies nullptr.
686  */
687  static hypot_t* findHypotByID(
688  const hypotsp_t& vec_hypots, size_t id, bool throw_exc = true);
689  /**\brief Get the ICP Edge between the provided nodes.
690  *
691  * Handy for not having to manually fetch the laser scans, as the method
692  * takes care of this.
693  *
694  * \param[out] icp_info Struct that will be filled with the results of the
695  * ICP operation
696  *
697  * \param[in] ad_params Pointer to additional parameters in the getICPEdge
698  * call
699  *
700  * \return True if operation was successful, false otherwise (e.g. if the
701  * either of the nodes' CObservation2DRangeScan object does not contain
702  * valid data.
703  */
704  virtual bool getICPEdge(
705  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
706  constraint_t* rel_edge,
707  mrpt::slam::CICP::TReturnInfo* icp_info = nullptr,
708  const TGetICPEdgeAdParams* ad_params = nullptr);
709  /**\brief compute the minimum uncertainty of each node position with
710  * regards to the graph root.
711  *
712  * \param[in] starting_node Node from which I start the Dijkstra projection
713  * algorithm
714  * \param[in] ending_node Specify the nodeID whose uncertainty wrt the
715  * starting_node, we are interested in computing. If given, method
716  * execution ends when this path is computed.
717  */
719  mrpt::graphs::TNodeID starting_node = 0,
720  mrpt::graphs::TNodeID ending_node = INVALID_NODEID);
721  /**\brief Given two nodeIDs compute and return the path connecting them.
722  *
723  * Method takes care of multiple edges, as well as edges with 0 covariance
724  * matrices
725  */
727  const mrpt::graphs::TNodeID from, const mrpt::graphs::TNodeID to,
728  path_t* path) const;
729  /**\brief Find the minimum uncertainty path from te given pool of
730  * TUncertaintyPath instances.
731  *
732  * Removes (and returns) the found path from the pool.
733  *
734  * \return Minimum uncertainty path from the pool provided
735  */
737  std::set<path_t*>* pool_of_paths) const;
738  /**\brief Append the paths starting from the current node.
739  *
740  * \param[in] pool_of_paths Paths that are currently registered
741  * \param[in] curr_path Path that I am currently traversing. This path is
742  * already removed from \a pool_of_paths
743  * \param[in] neighbors std::set of neighboring nodes to the last node of
744  * the current path
745  */
746  void addToPaths(
747  std::set<path_t*>* pool_of_paths, const path_t& curr_path,
748  const std::set<mrpt::graphs::TNodeID>& neibors) const;
749  /**\brief Query for the optimal path of a nodeID.
750  *
751  * Method handles calls to out-of-bounds nodes as well as nodes whose paths
752  * have not yet been computed.
753  *
754  * \param[in] node nodeID for which hte path is going to be returned
755  *
756  * \return Optimal path corresponding to the given nodeID or nullptr if the
757  * former is not found.
758  */
760  const mrpt::graphs::TNodeID node) const;
761  /**\brief Split an existing partition to Groups
762  *
763  * Have two groups A, B.
764  * - Group A consists of the lower nodeIDs. They correspond to the start
765  * of the course
766  * - Group B consists of the higher (more recent) nodeIDs. They
767  * correspond to the end of the course find where to split the current
768  * partition
769  *
770  * \note Method is used in single-robot graphSLAM for spliting a
771  * partition of nodes to lower and higher node IDs
772  *
773  * \param[in] partition Partition to be split.
774  * \param[out] groupA First group of nodes.
775  * \param[out] groupB Second group of nodes.
776  * \param[in] max_nodes_in_group Max number of nodes that are to exist in
777  * each group (Use -1 to disable this threshold).
778  */
780  std::vector<uint32_t>& partition, std::vector<uint32_t>* groupA,
781  std::vector<uint32_t>* groupB, int max_nodes_in_group = 5);
782  /**\brief Assign the last recorded 2D Laser scan
783  *
784  * \note Compact way of assigning the last recorded laser scan for both
785  * MRPT rawlog formats.
786  *
787  * Method takes into account the start of graphSLAM proc. when two nodes
788  * are added at the graph at the same time (root + node for 1st constraint)
789  */
791 
792  /**\brief Instance responsible for partitioning the map */
794 
797  mrpt::img::TColor(160, 160, 160, 255);
800 
801  /**\brief Keep track of the registered edge types.
802  *
803  * Handy for displaying them in the Visualization window.
804  */
805  std::map<std::string, int> m_edge_types_to_nums;
806  /**\brief Keep the last laser scan for visualization purposes */
808  /**\name Partition vectors */
809  /**\{ */
810  /**\brief Previous partitions vector */
812  /**\brief Current partitions vector */
814  /**\} */
815  /**\brief Indicate whether the partitions have been updated recently */
817  /**\brief Keep track of the evaluated partitions so they are not checked
818  * again if nothing changed in them.
819  */
820  std::map<int, std::vector<uint32_t>> m_partitionID_to_prev_nodes_list;
821  /**\brief Map that stores the lowest uncertainty path towards a node.
822  * Starting node depends on the starting node as used in the
823  * execDijkstraProjection method
824  */
825  typename std::map<mrpt::graphs::TNodeID, path_t*> m_node_optimal_paths;
826  /**\brief Keep track of the first recorded laser scan so that it can be
827  * assigned to the root node when the NRD adds the first *two* nodes to the
828  * graph.
829  */
831  /**\brief Track the first node registration occurance
832  *
833  * Handy so that we can assign a measurement to the root node as well.
834  */
836  /**\brief Node Count lower bound before executing dijkstra
837  */
839  /**\brief Factor used for accepting an ICP Constraint as valid.
840  */
842  /**\brief Factor used for accepting an ICP Constraint in the loop closure
843  * proc.
844  */
846 };
847 } // namespace mrpt::graphslam::deciders
848 #include "CLoopCloserERD_impl.h"
Struct for storing together the parameters needed for ICP matching, laser scans visualization etc...
void toggleMapPartitionsVisualization()
Toggle the map partitions visualization objects.
const partitions_t & getCurrentPartitions() const
partitions_t m_curr_partitions
Current partitions vector.
mrpt::slam::CIncrementalMapPartitioner m_partitioner
Instance responsible for partitioning the map.
friend std::ostream & operator<<(std::ostream &o, const self_t &params)
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 ...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
void initMapPartitionsVisualization()
Initialize the visualization of the map partition objects.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
typename GRAPH_T::constraint_t::type_value pose_t
typename GRAPH_T::global_pose_t global_pose_t
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
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...
bool m_partitions_full_update
Indicate whether the partitions have been updated recently.
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.
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.
void setDijkstraExecutionThresh(size_t new_thresh)
void getAsString(std::string *str) const
Definition: TNodeProps.h:33
mrpt::vision::TStereoCalibParams params
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 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.
bool use_scan_matching
Indicate whethet to use scan-matching at all during graphSLAM [on by default].
void getDescriptiveReport(std::string *report_str) const override
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.
TSlidingWindow mahal_distance_ICP_odom_win
Keep track of the mahalanobis distance between the initial pose difference and the suggested new edge...
Edge Registration Decider Interface from which RangeScanner-based ERDs can inherit from...
void setWindowManagerPtr(mrpt::graphslam::CWindowManager *win_manager) override
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.
Struct for passing additional parameters to the getICPEdge call.
void registerHypothesis(const hypot_t &h)
Wrapper around the registerNewEdge method which accepts a THypothesis object instead.
void evaluatePartitionsForLC(const partitions_t &partitions)
Evaluate the given partitions for loop closures.
bool computeDominantEigenVector(const mrpt::math::CMatrixDouble &consist_matrix, mrpt::math::CVectorDouble *eigvec, bool use_power_method=false)
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...
double LC_eigenvalues_ratio_thresh
Eigenvalues ratio for accepting/rejecting a hypothesis set.
This class allows loading and storing values and vectors of different types from a configuration text...
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.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
typename GRAPH_T::constraint_t constraint_t
int prev_nodes_for_ICP
How many nodes back to check ICP against?
bool m_is_first_time_node_reg
Track the first node registration occurance.
std::map< mrpt::graphs::TNodeID, path_t * > m_node_optimal_paths
Map that stores the lowest uncertainty path towards a node.
std::map< std::string, int > m_edge_types_to_nums
Keep track of the registered edge types.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
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...
int full_partition_per_nodes
Full partition of map only afer X new nodes have been registered.
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.
std::vector< std::vector< uint32_t > > partitions_t
const mrpt::img::TColor m_curr_node_covariance_color
void registerNewEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge) override
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
Keep the last laser scan for visualization purposes.
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.
bool LC_check_curr_partition_only
flag indicating whether to check only the partition of the last registered node for potential loop cl...
typename parent_t::nodes_to_scans2D_t nodes_to_scans2D_t
void setLastLaserScan2D(mrpt::obs::CObservation2DRangeScan::Ptr scan)
Assign the last recorded 2D Laser scan.
Finds partitions in metric maps based on N-cut graph partition theory.
size_t getDijkstraExecutionThresh() const
Return the minimum number of nodes that should exist in the graph prior to running Dijkstra...
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.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
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...
std::map< mrpt::graphs::TNodeID, node_props_t > group_t
mrpt::vision::TStereoCalibResults out
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.
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.
Class to monitor the evolution of a statistical quantity.
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred) override
void checkPartitionsForLC(partitions_t *partitions_for_LC)
Check the registered so far partitions for potential loop closures.
void loadParams(const std::string &source_fname) override
Struct for passing additional parameters to the generateHypotsPool call.
The ICP algorithm return information.
Definition: CICP.h:190
double m_lc_icp_constraint_factor
Factor used for accepting an ICP Constraint in the loop closure proc.
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 updateMapPartitions(bool full_update=false, bool is_first_time_node_reg=false)
Split the currently registered graph nodes into partitions.
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
int LC_min_remote_nodes
how many remote nodes (large nodID difference should there be before I consider the potential loop cl...
Edge Registration Decider scheme specialized in Loop Closing.
typename GRAPH_T::edges_map_t::iterator edges_iterator
mrpt::graphslam::deciders::CRangeScanOps< GRAPH_T > range_ops_t
Typedef for accessing methods of the RangeScanRegistrationDecider_t parent class. ...
typename parent_t::range_ops_t range_ops_t
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
#define INVALID_NODEID
Definition: TNodeID.h:19
std::map< std::pair< hypot_t *, hypot_t * >, double > hypotsp_to_consist_t
const mrpt::img::TColor laser_scans_color
see Constructor for initialization
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.
void updateMapPartitionsVisualization()
Update the map partitions visualization.
A RGB color - 8bit.
Definition: TColor.h:25
typename GRAPH_T::edges_map_t::const_iterator edges_citerator
size_t LC_min_nodeid_diff
nodeID difference for detecting potential loop closure in a partition.
An edge hypothesis between two nodeIDs.
Definition: THypothesis.h:32
double m_consec_icp_constraint_factor
Factor used for accepting an ICP Constraint as valid.
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
size_t m_dijkstra_node_count_thresh
Node Count lower bound before executing dijkstra.
mrpt::graphslam::TUncertaintyPath< GRAPH_T > * queryOptimalPath(const mrpt::graphs::TNodeID node) const
Query for the optimal path of a nodeID.
Struct for storing together the loop-closing related parameters.
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.
void getEdgesStats(std::map< std::string, int > *edge_types_to_num) const override
partitions_t m_last_partitions
Previous partitions vector.
typename mrpt::graphs::detail::THypothesis< GRAPH_T > hypot_t
Class acts as a container for storing pointers to mrpt::gui::CDisplayWindow3D, mrpt::graphslam::CWind...
virtual void addScanMatchingEdges(const mrpt::graphs::TNodeID &curr_nodeID)
Addd ICP constraints from X previous nodeIDs up to the given nodeID.
std::map< mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr > nodes_to_scans2D_t
TSlidingWindow goodness_threshold_win
Keep track of ICP Goodness values for ICP between nearby nodes and adapt the Goodness threshold based...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020