MRPT  1.9.9
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-2019, 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  }
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(
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
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(
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:29
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
GLuint src
Definition: glext.h:7397
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...
GLuint dst
Definition: glext.h:7249
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.
GLsizei const GLchar ** string
Definition: glext.h:4116
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...
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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:20
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.
GLenum const GLfloat * params
Definition: glext.h:3538
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 1.9.9 Git: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019