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