MRPT  1.9.9
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-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
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>
40  mrpt::obs::CSensoryFrame::Ptr observations,
41  mrpt::obs::CObservation::Ptr observation)
42 {
43  MRPT_START;
44  MRPT_UNUSED_PARAM(action);
45  this->m_time_logger.enter("updateState");
46 
47  using namespace mrpt::obs;
48  using namespace mrpt::poses;
49 
50  bool registered_new_node = false;
51 
52  if (observation)
53  { // Observation-Only Rawlog
54  // delegate the action to the method responsible
55  if (IS_CLASS(*observation, CObservation2DRangeScan))
56  { // 2D
58  std::dynamic_pointer_cast<CObservation2DRangeScan>(observation);
59  registered_new_node = updateState2D(curr_laser_scan);
60  }
61  else if (IS_CLASS(*observation, CObservation3DRangeScan))
62  { // 3D
63  CObservation3DRangeScan::Ptr curr_laser_scan =
64  std::dynamic_pointer_cast<CObservation3DRangeScan>(observation);
65  registered_new_node = updateState3D(curr_laser_scan);
66  }
67  else if (IS_CLASS(*observation, CObservationOdometry))
68  { // odometry
69  // if it exists use the odometry information to reject wrong ICP
70  // matches
71  CObservationOdometry::Ptr obs_odometry =
72  std::dynamic_pointer_cast<CObservationOdometry>(observation);
73 
74  // not incremental - gives the absolute odometry reading - no InfMat
75  // either
76  m_curr_odometry_only_pose = obs_odometry->odometry;
79  }
80  }
81  else
82  { // action-observations rawlog
83  // Action part
84  if (action->getBestMovementEstimation())
85  {
86  // if it exists use the odometry information to reject wrong ICP
87  // matches
88  CActionRobotMovement2D::Ptr robot_move =
89  action->getBestMovementEstimation();
90  CPosePDF::Ptr increment = robot_move->poseChange.get_ptr();
91  CPosePDFGaussianInf increment_gaussian;
92  increment_gaussian.copyFrom(*increment);
93  m_latest_odometry_PDF += increment_gaussian;
94  }
95 
96  // observations part
97  if (observations->getObservationByClass<CObservation2DRangeScan>())
98  { // 2D
99  CObservation2DRangeScan::Ptr curr_laser_scan =
100  observations->getObservationByClass<CObservation2DRangeScan>();
101  registered_new_node = updateState2D(curr_laser_scan);
102  }
103  else if (observations->getObservationByClass<CObservation3DRangeScan>())
104  { // 3D - EXPERIMENTAL, has not been tested
105  CObservation3DRangeScan::Ptr curr_laser_scan =
106  observations->getObservationByClass<CObservation3DRangeScan>();
107  registered_new_node = updateState3D(curr_laser_scan);
108  }
109  }
110 
111  this->m_time_logger.leave("updateState");
112  return registered_new_node;
113 
114  MRPT_END;
115 } // end of updateState
116 
117 template <class GRAPH_T>
120 {
121  MRPT_START;
122  bool registered_new_node = false;
123 
124  m_curr_laser_scan2D = scan2d;
125  if (!m_last_laser_scan2D)
126  {
127  // initialize the last_laser_scan here - afterwards updated inside the
128  // checkRegistrationCondition*D method
130  }
131  else
132  {
133  registered_new_node = checkRegistrationCondition2D();
134  }
135 
136  return registered_new_node;
137  MRPT_END;
138 } // end of updateState2D
139 
140 template <class GRAPH_T>
142 {
143  MRPT_START;
144 
145  using namespace mrpt::math;
146 
147  bool registered_new_node = false;
148 
149  // Constraint that *may* update incrementally the m_since_prev_node_PDF.
150  constraint_t rel_edge;
152 
153  this->getICPEdge(
154  *m_last_laser_scan2D, *m_curr_laser_scan2D, &rel_edge, nullptr,
155  &icp_info);
156 
157  // Debugging directives
158  MRPT_LOG_DEBUG(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
160  "ICP Alignment operation:\tnIterations: %d\tgoodness: %.f\n",
161  icp_info.nIterations, icp_info.goodness);
163  "Current ICP constraint: \n\tEdge: %s\n\tNorm: %f",
164  rel_edge.getMeanVal().asString().c_str(), rel_edge.getMeanVal().norm());
166  "Corresponding Odometry constraint: \n\tEdge: %s\n\tNorm: %f",
167  m_latest_odometry_PDF.getMeanVal().asString().c_str(),
168  m_latest_odometry_PDF.getMeanVal().norm());
169 
170  // evaluate the mahalanobis distance of the above..
171  // If over an (adaptive) threshold, trust the odometry
172  double mahal_distance =
173  rel_edge.mahalanobisDistanceTo(m_latest_odometry_PDF);
174  // m_mahal_distance_ICP_odom.addNewMeasurement(mahal_distance);
175 
176  // TODO - Find out a proper criterion
177  // How do I filter out the "bad" 2DRangeScans?
178  // double mahal_distance_lim = m_mahal_distance_ICP_odom.getMedian();
179  // double mahal_distance_lim = m_mahal_distance_ICP_odom.getMean();
180  // double mahal_distance_lim =
181  // m_mahal_distance_ICP_odom.getMean() +
182  // 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  {
193  MRPT_LOG_DEBUG("Using ICP Edge");
195  }
196  else
197  {
198  MRPT_LOG_DEBUG("Using Odometry Edge");
199  rel_edge.copyFrom(m_latest_odometry_PDF);
201  }
202  MRPT_LOG_DEBUG_FMT("\tMahalanobis Distance = %f", mahal_distance);
204  "Times that the ICP Edge was used: %d/%d", m_times_used_ICP,
206 
207  // update the PDF until last registered node
208  this->m_since_prev_node_PDF += rel_edge;
210  registered_new_node = this->checkRegistrationCondition();
211 
212  // reset the odometry tracking as well.
215 
216  MRPT_LOG_DEBUG("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
217  return registered_new_node;
218  MRPT_END;
219 } // end of checkRegistrationCondition2D
220 
221 template <class GRAPH_T>
224 {
225  THROW_EXCEPTION("Not yet implemented.");
226  return false;
227 } // end of updateState3D
228 
229 template <class GRAPH_T>
231 {
232  THROW_EXCEPTION("Not yet implemented.");
233  return false;
234 } // end of checkRegistrationCondition3D
235 
236 template <class GRAPH_T>
238 {
239  MRPT_START;
240  MRPT_LOG_DEBUG("In checkRegistrationCondition");
241  using namespace mrpt::math;
242 
243  // Criterions for adding a new node
244  // - Covered distance since last node > registration_max_distance
245  // - Angle difference since last node > registration_max_angle
246 
247  bool angle_crit = false;
249  {
250  angle_crit =
251  fabs(wrapToPi(this->m_since_prev_node_PDF.getMeanVal().phi())) >
252  params.registration_max_angle;
253  }
254  bool distance_crit = false;
256  {
257  distance_crit = this->m_since_prev_node_PDF.getMeanVal().norm() >
258  params.registration_max_distance;
259  }
260 
261  // actual check
262  bool registered = false;
263  if (distance_crit || angle_crit)
264  {
265  registered = this->registerNewNodeAtEnd();
266  }
267 
268  return registered;
269  MRPT_END;
270 } // end of checkRegistrationCondition
271 
272 template <class GRAPH_T>
274 {
275  MRPT_START;
276  parent_t::loadParams(source_fname);
277 
278  params.loadFromConfigFileName(
279  source_fname, "NodeRegistrationDeciderParameters");
280  // m_mahal_distance_ICP_odom.loadFromConfigFileName(source_fname,
281  //"NodeRegistrationDeciderParameters");
282 
283  // set the logging level if given by the user
284  mrpt::config::CConfigFile source(source_fname);
285  // Minimum verbosity level of the logger
286  int min_verbosity_level = source.read_int(
287  "NodeRegistrationDeciderParameters", "class_verbosity", 1, false);
288  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
289  MRPT_LOG_DEBUG("Successfully loaded parameters.");
290  MRPT_END;
291 } // end of loadParams
292 
293 template <class GRAPH_T>
295 {
297  params.dumpToConsole();
298 } // end of printParams
299 
300 template <class GRAPH_T>
302  std::string* report_str) const
303 {
304  MRPT_START;
305  using namespace std;
306 
307  const std::string report_sep(2, '\n');
308  const std::string header_sep(80, '#');
309 
310  // Report on graph
311  stringstream class_props_ss;
312  class_props_ss << "ICP Goodness-based Registration Procedure Summary: "
313  << std::endl;
314  class_props_ss << header_sep << std::endl;
315 
316  // time and output logging
317  const std::string time_res = this->m_time_logger.getStatsAsText();
318  const std::string output_res = this->getLogAsString();
319 
320  // merge the individual reports
321  report_str->clear();
322  parent_t::getDescriptiveReport(report_str);
323 
324  *report_str += class_props_ss.str();
325  *report_str += report_sep;
326 
327  // loggers results
328  *report_str += time_res;
329  *report_str += report_sep;
330 
331  *report_str += output_res;
332  *report_str += report_sep;
333 
334  MRPT_END;
335 } // end of getDescriptiveReport
336 
337 template <class GRAPH_T>
339 {
340 }
341 
342 template <class GRAPH_T>
344  std::ostream& out) const
345 {
346  MRPT_START;
347 
348  using namespace mrpt::math;
349 
350  out << "------------------[ ICP Fixed Intervals Node Registration "
351  "]------------------\n";
352  out << mrpt::format(
353  "Max distance for registration = %.2f m\n", registration_max_distance);
354  out << mrpt::format(
355  "Max Angle for registration = %.2f deg\n",
356  RAD2DEG(registration_max_angle));
357 
358  decider.range_ops_t::params.dumpToTextStream(out);
359 
360  MRPT_END;
361 }
362 template <class GRAPH_T>
364  const mrpt::config::CConfigFileBase& source, const std::string& section)
365 {
366  MRPT_START;
367 
368  using namespace mrpt::math;
369 
370  registration_max_distance = source.read_double(
371  section, "registration_max_distance", 0.5 /* meter */, false);
372  registration_max_angle = source.read_double(
373  section, "registration_max_angle", 10 /* degrees */, false);
374  registration_max_angle = DEG2RAD(registration_max_angle);
375 
376  // load the icp parameters - from "ICP" section explicitly
377  decider.range_ops_t::params.loadFromConfigFile(source, "ICP");
378 
379  MRPT_END;
380 }
381 } // 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.
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.
STL namespace.
bool checkRegistrationCondition() override
Check whether a new node should be registered in the graph.
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...
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:197
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.
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:133
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...
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:194
GLsizei const GLchar ** string
Definition: glext.h:4116
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
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
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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.
GLenum const GLfloat * params
Definition: glext.h:3538
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.
#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: e47402b84 Wed Oct 23 01:09:07 2019 +0200 at miƩ oct 23 01:10:13 CEST 2019