MRPT  2.0.2
CICPCriteriaNRD_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
13 {
14 template <class GRAPH_T>
16  : params(*this) // pass reference to self when initializing the parameters
17 // m_mahal_distance_ICP_odom("Mahalanobis dist (ICP - odom)")
18 {
19  using namespace mrpt::math;
20  this->initializeLoggers("CICPCriteriaNRD");
21 
22  m_is_using_3DScan = false;
23 
27 
28  // m_mahal_distance_ICP_odom.resizeWindow(1000); // use the last X
29  // mahalanobis distance values
30 
31  m_times_used_ICP = 0;
33 
34  this->logFmt(mrpt::system::LVL_DEBUG, "Initialized class object");
35 }
36 
37 template <class GRAPH_T>
39  [[maybe_unused]] mrpt::obs::CActionCollection::Ptr action,
40  mrpt::obs::CSensoryFrame::Ptr observations,
41  mrpt::obs::CObservation::Ptr observation)
42 {
44  this->m_time_logger.enter("updateState");
45 
46  using namespace mrpt::obs;
47  using namespace mrpt::poses;
48 
49  bool registered_new_node = false;
50 
51  if (observation)
52  { // Observation-Only Rawlog
53  // delegate the action to the method responsible
54  if (IS_CLASS(*observation, CObservation2DRangeScan))
55  { // 2D
57  std::dynamic_pointer_cast<CObservation2DRangeScan>(observation);
58  registered_new_node = updateState2D(curr_laser_scan);
59  }
60  else if (IS_CLASS(*observation, CObservation3DRangeScan))
61  { // 3D
62  CObservation3DRangeScan::Ptr curr_laser_scan =
63  std::dynamic_pointer_cast<CObservation3DRangeScan>(observation);
64  registered_new_node = updateState3D(curr_laser_scan);
65  }
66  else if (IS_CLASS(*observation, CObservationOdometry))
67  { // odometry
68  // if it exists use the odometry information to reject wrong ICP
69  // matches
70  CObservationOdometry::Ptr obs_odometry =
71  std::dynamic_pointer_cast<CObservationOdometry>(observation);
72 
73  // not incremental - gives the absolute odometry reading - no InfMat
74  // either
75  m_curr_odometry_only_pose = obs_odometry->odometry;
78  }
79  }
80  else
81  { // action-observations rawlog
82  // Action part
83  if (action->getBestMovementEstimation())
84  {
85  // if it exists use the odometry information to reject wrong ICP
86  // matches
87  CActionRobotMovement2D::Ptr robot_move =
88  action->getBestMovementEstimation();
89  CPosePDF::Ptr increment = robot_move->poseChange.get_ptr();
90  CPosePDFGaussianInf increment_gaussian;
91  increment_gaussian.copyFrom(*increment);
92  m_latest_odometry_PDF += increment_gaussian;
93  }
94 
95  // observations part
96  if (observations->getObservationByClass<CObservation2DRangeScan>())
97  { // 2D
98  CObservation2DRangeScan::Ptr curr_laser_scan =
99  observations->getObservationByClass<CObservation2DRangeScan>();
100  registered_new_node = updateState2D(curr_laser_scan);
101  }
102  else if (observations->getObservationByClass<CObservation3DRangeScan>())
103  { // 3D - EXPERIMENTAL, has not been tested
104  CObservation3DRangeScan::Ptr curr_laser_scan =
105  observations->getObservationByClass<CObservation3DRangeScan>();
106  registered_new_node = updateState3D(curr_laser_scan);
107  }
108  }
109 
110  this->m_time_logger.leave("updateState");
111  return registered_new_node;
112 
113  MRPT_END
114 } // end of updateState
115 
116 template <class GRAPH_T>
119 {
120  MRPT_START
121  bool registered_new_node = false;
122 
123  m_curr_laser_scan2D = scan2d;
124  if (!m_last_laser_scan2D)
125  {
126  // initialize the last_laser_scan here - afterwards updated inside the
127  // checkRegistrationCondition*D method
129  }
130  else
131  {
132  registered_new_node = checkRegistrationCondition2D();
133  }
134 
135  return registered_new_node;
136  MRPT_END
137 } // end of updateState2D
138 
139 template <class GRAPH_T>
141 {
142  MRPT_START
143 
144  using namespace mrpt::math;
145 
146  bool registered_new_node = false;
147 
148  // Constraint that *may* update incrementally the m_since_prev_node_PDF.
149  constraint_t rel_edge;
151 
152  this->getICPEdge(
153  *m_last_laser_scan2D, *m_curr_laser_scan2D, &rel_edge, nullptr,
154  &icp_info);
155 
156  // Debugging directives
157  MRPT_LOG_DEBUG(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
159  "ICP Alignment operation:\tnIterations: %d\tgoodness: %.f\n",
160  icp_info.nIterations, icp_info.goodness);
162  "Current ICP constraint: \n\tEdge: %s\n\tNorm: %f",
163  rel_edge.getMeanVal().asString().c_str(), rel_edge.getMeanVal().norm());
165  "Corresponding Odometry constraint: \n\tEdge: %s\n\tNorm: %f",
166  m_latest_odometry_PDF.getMeanVal().asString().c_str(),
167  m_latest_odometry_PDF.getMeanVal().norm());
168 
169  // evaluate the mahalanobis distance of the above..
170  // If over an (adaptive) threshold, trust the odometry
171  double mahal_distance =
172  rel_edge.mahalanobisDistanceTo(m_latest_odometry_PDF);
173  // m_mahal_distance_ICP_odom.addNewMeasurement(mahal_distance);
174 
175  // TODO - Find out a proper criterion
176  // How do I filter out the "bad" 2DRangeScans?
177  // double mahal_distance_lim = m_mahal_distance_ICP_odom.getMedian();
178  // double mahal_distance_lim = m_mahal_distance_ICP_odom.getMean();
179  // double mahal_distance_lim =
180  // m_mahal_distance_ICP_odom.getMean() +
181  // m_mahal_distance_ICP_odom.getStdDev();
182  double mahal_distance_lim = 0.18; // visual introspection
183 
184  //
185  // check whether to use ICP or odometry Edge.
186  //
187  // if the norm of the odometry edge is 0, no odometry edge available
188  // => use ICP
189  if (mahal_distance < mahal_distance_lim ||
190  m_latest_odometry_PDF.getMeanVal().norm() == 0)
191  {
192  MRPT_LOG_DEBUG("Using ICP Edge");
194  }
195  else
196  {
197  MRPT_LOG_DEBUG("Using Odometry Edge");
198  rel_edge.copyFrom(m_latest_odometry_PDF);
200  }
201  MRPT_LOG_DEBUG_FMT("\tMahalanobis Distance = %f", mahal_distance);
203  "Times that the ICP Edge was used: %d/%d", m_times_used_ICP,
205 
206  // update the PDF until last registered node
207  this->m_since_prev_node_PDF += rel_edge;
209  registered_new_node = this->checkRegistrationCondition();
210 
211  // reset the odometry tracking as well.
214 
215  MRPT_LOG_DEBUG("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
216  return registered_new_node;
217  MRPT_END
218 } // end of checkRegistrationCondition2D
219 
220 template <class GRAPH_T>
223 {
224  THROW_EXCEPTION("Not yet implemented.");
225  return false;
226 } // end of updateState3D
227 
228 template <class GRAPH_T>
230 {
231  THROW_EXCEPTION("Not yet implemented.");
232  return false;
233 } // end of checkRegistrationCondition3D
234 
235 template <class GRAPH_T>
237 {
238  MRPT_START
239  MRPT_LOG_DEBUG("In checkRegistrationCondition");
240  using namespace mrpt::math;
241 
242  // Criterions for adding a new node
243  // - Covered distance since last node > registration_max_distance
244  // - Angle difference since last node > registration_max_angle
245 
246  bool angle_crit = false;
248  {
249  angle_crit =
250  fabs(wrapToPi(this->m_since_prev_node_PDF.getMeanVal().phi())) >
252  }
253  bool distance_crit = false;
255  {
256  distance_crit = this->m_since_prev_node_PDF.getMeanVal().norm() >
258  }
259 
260  // actual check
261  bool registered = false;
262  if (distance_crit || angle_crit)
263  {
264  registered = this->registerNewNodeAtEnd();
265  }
266 
267  return registered;
268  MRPT_END
269 } // end of checkRegistrationCondition
270 
271 template <class GRAPH_T>
272 void CICPCriteriaNRD<GRAPH_T>::loadParams(const std::string& source_fname)
273 {
274  MRPT_START
275  parent_t::loadParams(source_fname);
276 
278  source_fname, "NodeRegistrationDeciderParameters");
279  // m_mahal_distance_ICP_odom.loadFromConfigFileName(source_fname,
280  //"NodeRegistrationDeciderParameters");
281 
282  // set the logging level if given by the user
283  mrpt::config::CConfigFile source(source_fname);
284  // Minimum verbosity level of the logger
285  int min_verbosity_level = source.read_int(
286  "NodeRegistrationDeciderParameters", "class_verbosity", 1, false);
287  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
288  MRPT_LOG_DEBUG("Successfully loaded parameters.");
289  MRPT_END
290 } // end of loadParams
291 
292 template <class GRAPH_T>
294 {
297 } // end of printParams
298 
299 template <class GRAPH_T>
301  std::string* report_str) const
302 {
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 template <class GRAPH_T>
338 {
339 }
340 
341 template <class GRAPH_T>
343  std::ostream& out) const
344 {
345  MRPT_START
346 
347  using namespace mrpt::math;
348 
349  out << "------------------[ ICP Fixed Intervals Node Registration "
350  "]------------------\n";
351  out << mrpt::format(
352  "Max distance for registration = %.2f m\n", registration_max_distance);
353  out << mrpt::format(
354  "Max Angle for registration = %.2f deg\n",
355  RAD2DEG(registration_max_angle));
356 
357  decider.range_ops_t::params.dumpToTextStream(out);
358 
359  MRPT_END
360 }
361 template <class GRAPH_T>
363  const mrpt::config::CConfigFileBase& source, const std::string& section)
364 {
365  MRPT_START
366 
367  using namespace mrpt::math;
368 
369  registration_max_distance = source.read_double(
370  section, "registration_max_distance", 0.5 /* meter */, false);
371  registration_max_angle = source.read_double(
372  section, "registration_max_angle", 10 /* degrees */, false);
373  registration_max_angle = DEG2RAD(registration_max_angle);
374 
375  // load the icp parameters - from "ICP" section explicitly
376  decider.range_ops_t::params.loadFromConfigFile(source, "ICP");
377 
378  MRPT_END
379 }
380 } // namespace mrpt::graphslam::deciders
int m_times_used_ICP
How many times we used the ICP Edge instead of Odometry edge.
#define MRPT_START
Definition: exceptions.h:241
#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.
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:67
bool updateState2D(mrpt::obs::CObservation2DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 2DRangeScan information.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
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.
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
ICP-based Fixed Intervals Node Registration.
mrpt::vision::TStereoCalibParams params
unsigned int nIterations
The number of executed iterations until convergence.
Definition: CICP.h:196
STL namespace.
bool checkRegistrationCondition() override
Check whether a new node should be registered in the graph.
double registration_max_distance
Maximum distance for new node registration.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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...
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);
constexpr double DEG2RAD(const double x)
Degrees to radians.
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
double registration_max_angle
Maximum angle difference for new node registration.
This namespace contains representation of robot actions and observations.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:200
constraint_t m_latest_odometry_PDF
Odometry rigid-body transformation since the last accepted LaserScan.
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
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...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
void loadFromConfigFileName(const std::string &config_file, const std::string &section)
Behaves like loadFromConfigFile, but you can pass directly a file name and a temporary CConfigFile ob...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation) override
Update the decider state using the latest dataset measurements.
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...
void enter(const std::string_view &func_name) noexcept
Start of a named section.
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::vision::TStereoCalibResults out
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_END
Definition: exceptions.h:245
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.
The ICP algorithm return information.
Definition: CICP.h:190
mrpt::system::CTimeLogger m_time_logger
Time logger instance.
double leave(const std::string_view &func_name) noexcept
End of a named section.
bool checkRegistrationCondition3D()
Specialized checkRegistrationCondition method used solely when dealing with 3DRangeScan information...
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 printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
An observation of the current (cumulative) odometry for a wheeled robot.
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
int m_times_used_odom
How many times we used the Odometry Edge instead of the ICP edge.
bool updateState3D(mrpt::obs::CObservation3DRangeScan::Ptr observation)
Specialized updateState method used solely when dealing with 3DRangeScan information.
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 2.0.2 Git: 9efc2a654 Mon Apr 6 11:24:47 2020 +0200 at lun abr 6 11:30:12 CEST 2020