Main MRPT website > C++ reference for MRPT 1.5.9
CICPCriteriaNRD_impl.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 CICPCRITERIANRD_IMPL_H
11 #define CICPCRITERIANRD_IMPL_H
12 
13 namespace mrpt { namespace graphslam { namespace deciders {
14 
15 // Ctors, Dtors
16 //////////////////////////////////////////////////////////////
17 
18 template<class GRAPH_T>
20  params(*this) // pass reference to self when initializing the parameters
21  //m_mahal_distance_ICP_odom("Mahalanobis dist (ICP - odom)")
22 {
23  using namespace mrpt::utils;
24  using namespace mrpt::math;
25  this->initializeLoggers("CICPCriteriaNRD");
26 
27  m_is_using_3DScan = false;
28 
32 
33  //m_mahal_distance_ICP_odom.resizeWindow(1000); // use the last X mahalanobis distance values
34 
35  m_times_used_ICP = 0;
37 
38  this->logFmt(LVL_DEBUG, "Initialized class object");
39 }
40 template<class GRAPH_T>
42 }
43 
44 template<class GRAPH_T>
46  mrpt::obs::CActionCollectionPtr action,
47  mrpt::obs::CSensoryFramePtr observations,
48  mrpt::obs::CObservationPtr observation ) {
49  MRPT_START;
50  MRPT_UNUSED_PARAM(action);
51  this->m_time_logger.enter("updateState");
52 
53  using namespace mrpt::obs;
54  using namespace mrpt::poses;
55 
56  bool registered_new_node = false;
57 
58  if (observation.present()) { // Observation-Only Rawlog
59  // delegate the action to the method responsible
60  if (IS_CLASS(observation, CObservation2DRangeScan) ) { // 2D
61  mrpt::obs::CObservation2DRangeScanPtr curr_laser_scan =
62  static_cast<CObservation2DRangeScanPtr>(observation);
63  registered_new_node = updateState2D(curr_laser_scan);
64 
65  }
66  else if (IS_CLASS(observation, CObservation3DRangeScan) ) { // 3D
67  CObservation3DRangeScanPtr curr_laser_scan =
68  static_cast<CObservation3DRangeScanPtr>(observation);
69  registered_new_node = updateState3D(curr_laser_scan);
70  }
71  else if (IS_CLASS(observation, CObservationOdometry) ) { // odometry
72  // if it exists use the odometry information to reject wrong ICP
73  // matches
74  CObservationOdometryPtr obs_odometry =
75  static_cast<CObservationOdometryPtr>(observation);
76 
77  // not incremental - gives the absolute odometry reading - no InfMat
78  // either
79  m_curr_odometry_only_pose = obs_odometry->odometry;
82 
83  }
84 
85  }
86  else { // action-observations rawlog
87  // Action part
88  if (action->getBestMovementEstimation()) {
89  // if it exists use the odometry information to reject wrong ICP matches
90  CActionRobotMovement2DPtr robot_move =
91  action->getBestMovementEstimation();
92  CPosePDFPtr increment = robot_move->poseChange.get_ptr();
93  CPosePDFGaussianInf increment_gaussian;
94  increment_gaussian.copyFrom(*increment);
95  m_latest_odometry_PDF += increment_gaussian;
96  }
97 
98  // observations part
99  if (observations->getObservationByClass<CObservation2DRangeScan>()) { // 2D
100  CObservation2DRangeScanPtr curr_laser_scan =
101  observations->getObservationByClass<CObservation2DRangeScan>();
102  registered_new_node = updateState2D(curr_laser_scan);
103  }
104  else if (observations->getObservationByClass<CObservation3DRangeScan>()){ // 3D - EXPERIMENTAL, has not been tested
105  CObservation3DRangeScanPtr curr_laser_scan =
106  observations->getObservationByClass<CObservation3DRangeScan>();
107  registered_new_node = updateState3D(curr_laser_scan);
108  }
109 
110  }
111 
112  this->m_time_logger.leave("updateState");
113  return registered_new_node;
114 
115  MRPT_END;
116 } // end of updateState
117 
118 template<class GRAPH_T>
120  mrpt::obs::CObservation2DRangeScanPtr scan2d) {
121  MRPT_START;
122  bool registered_new_node = false;
123 
124  m_curr_laser_scan2D = scan2d;
125  if (!m_last_laser_scan2D) {
126  // initialize the last_laser_scan here - afterwards updated inside the
127  // checkRegistrationCondition*D method
129  }
130  else {
131  registered_new_node = checkRegistrationCondition2D();
132  }
133 
134  return registered_new_node;
135  MRPT_END;
136 } // end of updateState2D
137 
138 template<class GRAPH_T>
140  MRPT_START;
141 
142  using namespace mrpt::math;
143  using namespace mrpt::utils;
144 
145  bool registered_new_node = false;
146 
147  // Constraint that *may* update incrementally the m_since_prev_node_PDF.
148  constraint_t rel_edge;
150 
151  this->getICPEdge(
154  &rel_edge,
155  NULL,
156  &icp_info);
157 
158  // Debugging directives
159  this->logFmt(LVL_DEBUG,
160  ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
161  this->logFmt(LVL_DEBUG,
162  "ICP Alignment operation:\tnIterations: %d\tgoodness: %.f\n",
163  icp_info.nIterations, icp_info.goodness);
164 
165  this->logFmt(LVL_DEBUG, "Current ICP constraint: \n\tEdge: %s\n\tNorm: %f",
166  rel_edge.getMeanVal().asString().c_str(),
167  rel_edge.getMeanVal().norm());
168  this->logFmt(LVL_DEBUG, "Corresponding Odometry constraint: \n\tEdge: %s\n\tNorm: %f",
169  m_latest_odometry_PDF.getMeanVal().asString().c_str(),
170  m_latest_odometry_PDF.getMeanVal().norm());
171 
172  // evaluate the mahalanobis distance of the above..
173  // If over an (adaptive) threshold, trust the odometry
174  double mahal_distance = rel_edge.mahalanobisDistanceTo(m_latest_odometry_PDF);
175  //m_mahal_distance_ICP_odom.addNewMeasurement(mahal_distance);
176 
177  // TODO - Find out a proper criterion
178  // How do I filter out the "bad" 2DRangeScans?
179  //double mahal_distance_lim = m_mahal_distance_ICP_odom.getMedian();
180  //double mahal_distance_lim = m_mahal_distance_ICP_odom.getMean();
181  //double mahal_distance_lim =
182  //m_mahal_distance_ICP_odom.getMean() + m_mahal_distance_ICP_odom.getStdDev();
183  double mahal_distance_lim = 0.18; // visual introspection
184 
185  //
186  // check whether to use ICP or odometry Edge.
187  //
188  // if the norm of the odometry edge is 0, no odometry edge available
189  // => use ICP
190  if (mahal_distance < mahal_distance_lim ||
191  m_latest_odometry_PDF.getMeanVal().norm() == 0) {
192  this->logFmt(LVL_DEBUG, "Using ICP Edge");
194  }
195  else {
196  this->logFmt(LVL_DEBUG, "Using Odometry Edge");
197  rel_edge.copyFrom(m_latest_odometry_PDF);
199  }
200  this->logFmt(LVL_DEBUG, "\tMahalanobis Distance = %f", mahal_distance);
201  this->logFmt(LVL_DEBUG, "Times that the ICP Edge was used: %d/%d",
203 
204  // update the PDF until last registered node
205  this->m_since_prev_node_PDF += rel_edge;
207  registered_new_node = this->checkRegistrationCondition();
208 
209  // reset the odometry tracking as well.
212 
213  this->logFmt(LVL_DEBUG,
214  "<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
215  return registered_new_node;
216  MRPT_END;
217 } // end of checkRegistrationCondition2D
218 
219 template<class GRAPH_T>
221  mrpt::obs::CObservation3DRangeScanPtr scan3d) {
222  THROW_EXCEPTION("Not yet implemented.");
223  return false;
224 } // end of updateState3D
225 
226 template<class GRAPH_T>
228  THROW_EXCEPTION("Not yet implemented.");
229  return false;
230 } // end of checkRegistrationCondition3D
231 
232 
233 template<class GRAPH_T>
235  MRPT_START;
236  using namespace mrpt::utils;
237  this->logFmt(LVL_DEBUG, "In checkRegistrationCondition");
238  using namespace mrpt::math;
239 
240  // Criterions for adding a new node
241  // - Covered distance since last node > registration_max_distance
242  // - Angle difference since last node > registration_max_angle
243 
244  bool angle_crit = false;
246  angle_crit = fabs(wrapToPi(this->m_since_prev_node_PDF.getMeanVal().phi())) >
247  params.registration_max_angle;
248  }
249  bool distance_crit = false;
251  distance_crit =
252  this->m_since_prev_node_PDF.getMeanVal().norm() >
253  params.registration_max_distance; }
254 
255  // actual check
256  bool registered = false;
257  if (distance_crit || angle_crit) {
258  registered = this->registerNewNodeAtEnd();
259  }
260 
261  return registered;
262  MRPT_END;
263 } // end of checkRegistrationCondition
264 
265 template<class GRAPH_T>
267  MRPT_START;
268  parent_t::loadParams(source_fname);
269 
270  using namespace mrpt::utils;
271 
272  params.loadFromConfigFileName(source_fname,
273  "NodeRegistrationDeciderParameters");
274  //m_mahal_distance_ICP_odom.loadFromConfigFileName(source_fname,
275  //"NodeRegistrationDeciderParameters");
276 
277  // set the logging level if given by the user
278  CConfigFile source(source_fname);
279  // Minimum verbosity level of the logger
280  int min_verbosity_level = source.read_int(
281  "NodeRegistrationDeciderParameters",
282  "class_verbosity",
283  1, false);
284  this->setMinLoggingLevel(VerbosityLevel(min_verbosity_level));
285  this->logFmt(LVL_DEBUG, "Successfully loaded parameters.");
286  MRPT_END;
287 } // end of loadParams
288 
289 template<class GRAPH_T>
291  MRPT_START;
293 
294  params.dumpToConsole();
295  //m_mahal_distance_ICP_odom.dumpToConsole();
296 
297  MRPT_END;
298 } // end of printParams
299 
300 template<class GRAPH_T>
302  std::string* report_str) const {
303  MRPT_START;
304  using namespace std;
305 
306  const std::string report_sep(2, '\n');
307  const std::string header_sep(80, '#');
308 
309  // Report on graph
310  stringstream class_props_ss;
311  class_props_ss << "ICP Goodness-based Registration Procedure Summary: "
312  << std::endl;
313  class_props_ss << header_sep << std::endl;
314 
315  // time and output logging
316  const std::string time_res = this->m_time_logger.getStatsAsText();
317  const std::string output_res = this->getLogAsString();
318 
319  // merge the individual reports
320  report_str->clear();
321  parent_t::getDescriptiveReport(report_str);
322 
323  *report_str += class_props_ss.str();
324  *report_str += report_sep;
325 
326  // loggers results
327  *report_str += time_res;
328  *report_str += report_sep;
329 
330  *report_str += output_res;
331  *report_str += report_sep;
332 
333  MRPT_END;
334 } // end of getDescriptiveReport
335 
336 
337 // TParams
338 //////////////////////////////////////////////////////////////
339 template<class GRAPH_T>
341  decider(d)
342 { }
343 template<class GRAPH_T>
345 template<class GRAPH_T>
347  mrpt::utils::CStream &out) const {
348  MRPT_START;
349 
350  using namespace mrpt::utils;
351  using namespace mrpt::math;
352 
353  out.printf(
354  "------------------[ ICP Fixed Intervals Node Registration ]------------------\n");
355  out.printf("Max distance for registration = %.2f m\n",
356  registration_max_distance);
357  out.printf("Max Angle for registration = %.2f deg\n",
358  RAD2DEG(registration_max_angle));
359 
360  decider.range_ops_t::params.dumpToTextStream(out);
361 
362  MRPT_END;
363 }
364 template<class GRAPH_T>
367  const std::string &section) {
368  MRPT_START;
369 
370  using namespace mrpt::utils;
371  using namespace mrpt::math;
372 
373  registration_max_distance = source.read_double( section,
374  "registration_max_distance",
375  0.5 /* meter */, false);
376  registration_max_angle = source.read_double( section,
377  "registration_max_angle",
378  10 /* degrees */, false);
379  registration_max_angle = DEG2RAD(registration_max_angle);
380 
381  // load the icp parameters - from "ICP" section explicitly
382  decider.range_ops_t::params.loadFromConfigFile(source, "ICP");
383 
384  MRPT_END;
385 }
386 
387 } } } // end of namespace
388 
389 
390 #endif /* end of include guard: CICPCRITERIANRD_IMPL_H */
bool updateState2D(mrpt::obs::CObservation2DRangeScanPtr observation)
Specialized updateState method used solely when dealing with 2DRangeScan information.
int m_times_used_ICP
How many times we used the ICP Edge instead of Odometry edge.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
bool updateState(mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation)
Update the decider state using the latest dataset measurements.
virtual void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
#define THROW_EXCEPTION(msg)
pose_t m_curr_odometry_only_pose
pose_t estimation using only odometry information.
pose_t m_last_odometry_only_pose
pose_t estimation using only odometry information.
static const std::string header_sep
Separator string to be used in debugging messages.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
ICP-based Fixed Intervals Node Registration.
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
STL namespace.
This class allows loading and storing values and vectors of different types from a configuration text...
bool registerNewNodeAtEnd()
Same goal as the previous method - uses the m_since_prev_node_PDF as the constraint at the end...
bool checkRegistrationCondition()
Check whether a new node should be registered in the graph.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:137
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
GRAPH_T::constraint_t constraint_t
type of graph constraints
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
This namespace contains representation of robot actions and observations.
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
constraint_t m_latest_odometry_PDF
Odometry rigid-body transformation since the last accepted LaserScan.
void resetPDF(constraint_t *c)
Reset the given PDF method and assign a fixed high-certainty Covariance/Information matrix...
virtual void initializeLoggers(const std::string &name)
Initialize the COutputLogger, CTimeLogger instances given the name of the decider/optimizer at hand...
#define DEG2RAD
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:136
GLsizei const GLchar ** string
Definition: glext.h:3919
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
bool updateState3D(mrpt::obs::CObservation3DRangeScanPtr observation)
Specialized updateState method used solely when dealing with 3DRangeScan information.
void getICPEdge(const mrpt::obs::CObservation2DRangeScan &from, const mrpt::obs::CObservation2DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=NULL, mrpt::slam::CICP::TReturnInfo *icp_info=NULL)
Align the 2D range scans provided and fill the potential edge that can transform the one into the oth...
bool checkRegistrationCondition2D()
Specialized checkRegistrationCondtion method used solely when dealing with 2DRangeScan information...
#define MRPT_START
#define RAD2DEG
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
mrpt::utils::CTimeLogger m_time_logger
Time logger instance.
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:93
The ICP algorithm return information.
Definition: CICP.h:128
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
bool checkRegistrationCondition3D()
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information...
mrpt::obs::CObservation2DRangeScanPtr m_last_laser_scan2D
handy laser scans to use in the class methods
An observation of the current (cumulative) odometry for a wheeled robot.
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
GLenum const GLfloat * params
Definition: glext.h:3514
int m_times_used_odom
How many times we used the Odometry Edge instead of the ICP edge.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
mrpt::obs::CObservation2DRangeScanPtr m_curr_laser_scan2D
Current LaserScan.
constraint_t m_since_prev_node_PDF
Tracking the PDF of the current position of the robot with regards to the <b previous registered node...



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