Main MRPT website > C++ reference for MRPT 1.9.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-2018, 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
14 {
15 namespace graphslam
16 {
17 namespace deciders
18 {
19 // Ctors, Dtors
20 //////////////////////////////////////////////////////////////
21 
22 template <class GRAPH_T>
24  : params(*this) // pass reference to self when initializing the parameters
25 // m_mahal_distance_ICP_odom("Mahalanobis dist (ICP - odom)")
26 {
27  using namespace mrpt::math;
28  this->initializeLoggers("CICPCriteriaNRD");
29 
30  m_is_using_3DScan = false;
31 
35 
36  // m_mahal_distance_ICP_odom.resizeWindow(1000); // use the last X
37  // mahalanobis distance values
38 
39  m_times_used_ICP = 0;
41 
42  this->logFmt(mrpt::system::LVL_DEBUG, "Initialized class object");
43 }
44 template <class GRAPH_T>
46 {
47 }
48 
49 template <class GRAPH_T>
52  mrpt::obs::CSensoryFrame::Ptr observations,
53  mrpt::obs::CObservation::Ptr observation)
54 {
55  MRPT_START;
56  MRPT_UNUSED_PARAM(action);
57  this->m_time_logger.enter("updateState");
58 
59  using namespace mrpt::obs;
60  using namespace mrpt::poses;
61 
62  bool registered_new_node = false;
63 
64  if (observation)
65  { // Observation-Only Rawlog
66  // delegate the action to the method responsible
67  if (IS_CLASS(observation, CObservation2DRangeScan))
68  { // 2D
70  std::dynamic_pointer_cast<CObservation2DRangeScan>(observation);
71  registered_new_node = updateState2D(curr_laser_scan);
72  }
73  else if (IS_CLASS(observation, CObservation3DRangeScan))
74  { // 3D
75  CObservation3DRangeScan::Ptr curr_laser_scan =
76  std::dynamic_pointer_cast<CObservation3DRangeScan>(observation);
77  registered_new_node = updateState3D(curr_laser_scan);
78  }
79  else if (IS_CLASS(observation, CObservationOdometry))
80  { // odometry
81  // if it exists use the odometry information to reject wrong ICP
82  // matches
83  CObservationOdometry::Ptr obs_odometry =
84  std::dynamic_pointer_cast<CObservationOdometry>(observation);
85 
86  // not incremental - gives the absolute odometry reading - no InfMat
87  // either
88  m_curr_odometry_only_pose = obs_odometry->odometry;
91  }
92  }
93  else
94  { // action-observations rawlog
95  // Action part
96  if (action->getBestMovementEstimation())
97  {
98  // if it exists use the odometry information to reject wrong ICP
99  // matches
100  CActionRobotMovement2D::Ptr robot_move =
101  action->getBestMovementEstimation();
102  CPosePDF::Ptr increment = robot_move->poseChange.get_ptr();
103  CPosePDFGaussianInf increment_gaussian;
104  increment_gaussian.copyFrom(*increment);
105  m_latest_odometry_PDF += increment_gaussian;
106  }
107 
108  // observations part
109  if (observations->getObservationByClass<CObservation2DRangeScan>())
110  { // 2D
111  CObservation2DRangeScan::Ptr curr_laser_scan =
112  observations->getObservationByClass<CObservation2DRangeScan>();
113  registered_new_node = updateState2D(curr_laser_scan);
114  }
115  else if (observations->getObservationByClass<CObservation3DRangeScan>())
116  { // 3D - EXPERIMENTAL, has not been tested
117  CObservation3DRangeScan::Ptr curr_laser_scan =
118  observations->getObservationByClass<CObservation3DRangeScan>();
119  registered_new_node = updateState3D(curr_laser_scan);
120  }
121  }
122 
123  this->m_time_logger.leave("updateState");
124  return registered_new_node;
125 
126  MRPT_END;
127 } // end of updateState
128 
129 template <class GRAPH_T>
132 {
133  MRPT_START;
134  bool registered_new_node = false;
135 
136  m_curr_laser_scan2D = scan2d;
137  if (!m_last_laser_scan2D)
138  {
139  // initialize the last_laser_scan here - afterwards updated inside the
140  // checkRegistrationCondition*D method
142  }
143  else
144  {
145  registered_new_node = checkRegistrationCondition2D();
146  }
147 
148  return registered_new_node;
149  MRPT_END;
150 } // end of updateState2D
151 
152 template <class GRAPH_T>
154 {
155  MRPT_START;
156 
157  using namespace mrpt::math;
158 
159  bool registered_new_node = false;
160 
161  // Constraint that *may* update incrementally the m_since_prev_node_PDF.
162  constraint_t rel_edge;
164 
165  this->getICPEdge(
166  *m_last_laser_scan2D, *m_curr_laser_scan2D, &rel_edge, nullptr,
167  &icp_info);
168 
169  // Debugging directives
170  MRPT_LOG_DEBUG(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
172  "ICP Alignment operation:\tnIterations: %d\tgoodness: %.f\n",
173  icp_info.nIterations, icp_info.goodness);
175  "Current ICP constraint: \n\tEdge: %s\n\tNorm: %f",
176  rel_edge.getMeanVal().asString().c_str(), rel_edge.getMeanVal().norm());
178  "Corresponding Odometry constraint: \n\tEdge: %s\n\tNorm: %f",
179  m_latest_odometry_PDF.getMeanVal().asString().c_str(),
180  m_latest_odometry_PDF.getMeanVal().norm());
181 
182  // evaluate the mahalanobis distance of the above..
183  // If over an (adaptive) threshold, trust the odometry
184  double mahal_distance =
185  rel_edge.mahalanobisDistanceTo(m_latest_odometry_PDF);
186  // m_mahal_distance_ICP_odom.addNewMeasurement(mahal_distance);
187 
188  // TODO - Find out a proper criterion
189  // How do I filter out the "bad" 2DRangeScans?
190  // double mahal_distance_lim = m_mahal_distance_ICP_odom.getMedian();
191  // double mahal_distance_lim = m_mahal_distance_ICP_odom.getMean();
192  // double mahal_distance_lim =
193  // m_mahal_distance_ICP_odom.getMean() +
194  // m_mahal_distance_ICP_odom.getStdDev();
195  double mahal_distance_lim = 0.18; // visual introspection
196 
197  //
198  // check whether to use ICP or odometry Edge.
199  //
200  // if the norm of the odometry edge is 0, no odometry edge available
201  // => use ICP
202  if (mahal_distance < mahal_distance_lim ||
203  m_latest_odometry_PDF.getMeanVal().norm() == 0)
204  {
205  MRPT_LOG_DEBUG("Using ICP Edge");
207  }
208  else
209  {
210  MRPT_LOG_DEBUG("Using Odometry Edge");
211  rel_edge.copyFrom(m_latest_odometry_PDF);
213  }
214  MRPT_LOG_DEBUG_FMT("\tMahalanobis Distance = %f", mahal_distance);
216  "Times that the ICP Edge was used: %d/%d", m_times_used_ICP,
218 
219  // update the PDF until last registered node
220  this->m_since_prev_node_PDF += rel_edge;
222  registered_new_node = this->checkRegistrationCondition();
223 
224  // reset the odometry tracking as well.
227 
228  MRPT_LOG_DEBUG("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
229  return registered_new_node;
230  MRPT_END;
231 } // end of checkRegistrationCondition2D
232 
233 template <class GRAPH_T>
236 {
237  THROW_EXCEPTION("Not yet implemented.");
238  return false;
239 } // end of updateState3D
240 
241 template <class GRAPH_T>
243 {
244  THROW_EXCEPTION("Not yet implemented.");
245  return false;
246 } // end of checkRegistrationCondition3D
247 
248 template <class GRAPH_T>
250 {
251  MRPT_START;
252  MRPT_LOG_DEBUG("In checkRegistrationCondition");
253  using namespace mrpt::math;
254 
255  // Criterions for adding a new node
256  // - Covered distance since last node > registration_max_distance
257  // - Angle difference since last node > registration_max_angle
258 
259  bool angle_crit = false;
261  {
262  angle_crit =
263  fabs(wrapToPi(this->m_since_prev_node_PDF.getMeanVal().phi())) >
264  params.registration_max_angle;
265  }
266  bool distance_crit = false;
268  {
269  distance_crit = this->m_since_prev_node_PDF.getMeanVal().norm() >
270  params.registration_max_distance;
271  }
272 
273  // actual check
274  bool registered = false;
275  if (distance_crit || angle_crit)
276  {
277  registered = this->registerNewNodeAtEnd();
278  }
279 
280  return registered;
281  MRPT_END;
282 } // end of checkRegistrationCondition
283 
284 template <class GRAPH_T>
286 {
287  MRPT_START;
288  parent_t::loadParams(source_fname);
289 
290  params.loadFromConfigFileName(
291  source_fname, "NodeRegistrationDeciderParameters");
292  // m_mahal_distance_ICP_odom.loadFromConfigFileName(source_fname,
293  //"NodeRegistrationDeciderParameters");
294 
295  // set the logging level if given by the user
296  mrpt::config::CConfigFile source(source_fname);
297  // Minimum verbosity level of the logger
298  int min_verbosity_level = source.read_int(
299  "NodeRegistrationDeciderParameters", "class_verbosity", 1, false);
300  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
301  MRPT_LOG_DEBUG("Successfully loaded parameters.");
302  MRPT_END;
303 } // end of loadParams
304 
305 template <class GRAPH_T>
307 {
308  MRPT_START;
310 
311  params.dumpToConsole();
312  // m_mahal_distance_ICP_odom.dumpToConsole();
313 
314  MRPT_END;
315 } // end of printParams
316 
317 template <class GRAPH_T>
319  std::string* report_str) const
320 {
321  MRPT_START;
322  using namespace std;
323 
324  const std::string report_sep(2, '\n');
325  const std::string header_sep(80, '#');
326 
327  // Report on graph
328  stringstream class_props_ss;
329  class_props_ss << "ICP Goodness-based Registration Procedure Summary: "
330  << std::endl;
331  class_props_ss << header_sep << std::endl;
332 
333  // time and output logging
334  const std::string time_res = this->m_time_logger.getStatsAsText();
335  const std::string output_res = this->getLogAsString();
336 
337  // merge the individual reports
338  report_str->clear();
339  parent_t::getDescriptiveReport(report_str);
340 
341  *report_str += class_props_ss.str();
342  *report_str += report_sep;
343 
344  // loggers results
345  *report_str += time_res;
346  *report_str += report_sep;
347 
348  *report_str += output_res;
349  *report_str += report_sep;
350 
351  MRPT_END;
352 } // end of getDescriptiveReport
353 
354 // TParams
355 //////////////////////////////////////////////////////////////
356 template <class GRAPH_T>
358 {
359 }
360 template <class GRAPH_T>
362 {
363 }
364 template <class GRAPH_T>
366  std::ostream& out) const
367 {
368  MRPT_START;
369 
370  using namespace mrpt::math;
371 
372  out << mrpt::format(
373  "------------------[ ICP Fixed Intervals Node Registration "
374  "]------------------\n");
375  out << mrpt::format(
376  "Max distance for registration = %.2f m\n", registration_max_distance);
377  out << mrpt::format(
378  "Max Angle for registration = %.2f deg\n",
379  RAD2DEG(registration_max_angle));
380 
381  decider.range_ops_t::params.dumpToTextStream(out);
382 
383  MRPT_END;
384 }
385 template <class GRAPH_T>
387  const mrpt::config::CConfigFileBase& source, const std::string& section)
388 {
389  MRPT_START;
390 
391  using namespace mrpt::math;
392 
393  registration_max_distance = source.read_double(
394  section, "registration_max_distance", 0.5 /* meter */, false);
395  registration_max_angle = source.read_double(
396  section, "registration_max_angle", 10 /* degrees */, false);
397  registration_max_angle = DEG2RAD(registration_max_angle);
398 
399  // load the icp parameters - from "ICP" section explicitly
400  decider.range_ops_t::params.loadFromConfigFile(source, "ICP");
401 
402  MRPT_END;
403 }
404 }
405 }
406 } // end of namespace
407 
408 #endif /* end of include guard: CICPCRITERIANRD_IMPL_H */
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.
#define MRPT_START
Definition: exceptions.h:262
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
mrpt::obs::CObservation2DRangeScan::Ptr m_last_laser_scan2D
handy laser scans to use in the class methods
VerbosityLevel
Enumeration of available verbosity levels.
double RAD2DEG(const double x)
Radians to degrees.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
virtual void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
bool updateState2D(mrpt::obs::CObservation2DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 2DRangeScan information.
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
double DEG2RAD(const double x)
Degrees to radians.
void getICPEdge(const mrpt::obs::CObservation2DRangeScan &from, const mrpt::obs::CObservation2DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=nullptr, mrpt::slam::CICP::TReturnInfo *icp_info=nullptr)
Align the 2D range scans provided and fill the potential edge that can transform the one into the oth...
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.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
static const std::string header_sep
Separator string to be used in debugging messages.
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
mrpt::obs::CObservation2DRangeScan::Ptr m_curr_laser_scan2D
Current LaserScan.
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.
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
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:199
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 a configuration text...
This base provides a set of functions for maths stuff.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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...
double leave(const char *func_name)
End of a named section.
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:196
GLsizei const GLchar ** string
Definition: glext.h:4101
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::string getLogAsString() const
Get the history of COutputLogger instance in a string representation.
bool checkRegistrationCondition2D()
Specialized checkRegistrationCondtion method used solely when dealing with 2DRangeScan information...
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...
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
#define MRPT_END
Definition: exceptions.h:266
The ICP algorithm return information.
Definition: CICP.h:192
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::system::CTimeLogger m_time_logger
Time logger instance.
bool checkRegistrationCondition3D()
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information...
An observation of the current (cumulative) odometry for a wheeled robot.
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
Update the decider state using the latest dataset measurements.
GLenum const GLfloat * params
Definition: glext.h:3534
int m_times_used_odom
How many times we used the Odometry Edge instead of the ICP edge.
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool updateState3D(mrpt::obs::CObservation3DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 3DRangeScan information.
void enter(const char *func_name)
Start of a named section.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
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.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019