MRPT  2.0.4
CFixedIntervalsNRD_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
12 
14 {
15 // Ctors, Dtors
16 //////////////////////////////////////////////////////////////
17 
18 template <class GRAPH_T>
20 {
21  this->initializeLoggers("CFixedIntervalsNRD");
22 }
23 
24 // Member function implementations
25 //////////////////////////////////////////////////////////////
26 
27 template <class GRAPH_T>
30  [[maybe_unused]] mrpt::obs::CSensoryFrame::Ptr observations,
31  mrpt::obs::CObservation::Ptr observation)
32 {
33  // don't use the measurements in this implementation
34 
36  using namespace mrpt::obs;
37  using namespace mrpt::math;
38  using namespace mrpt::poses;
39 
40  if (observation)
41  { // FORMAT #2 - observation-only format
42  m_observation_only_rawlog = true;
43 
44  if (IS_CLASS(*observation, CObservationOdometry))
45  {
46  CObservationOdometry::Ptr obs_odometry =
47  std::dynamic_pointer_cast<CObservationOdometry>(observation);
48  // not incremental - gives the absolute odometry reading
49  m_curr_odometry_only_pose = pose_t(obs_odometry->odometry);
51  "Current odometry-only pose: %s",
52  m_curr_odometry_only_pose.asString().c_str());
53 
54  // I don't have any information about the covariane of the move in
55  // observation-only format
56  this->m_since_prev_node_PDF.mean =
57  m_curr_odometry_only_pose - m_last_odometry_only_pose;
58  }
59  }
60  else
61  { // FORMAT #1 - action-observation format
62  m_observation_only_rawlog = false;
63 
65  bool found = action->getFirstMovementEstimation(move_pdf);
66  if (found)
67  {
68  // update the relative PDF of the path since the LAST node was
69  // inserted
70  constraint_t incr_constraint;
71  incr_constraint.copyFrom(move_pdf);
72  this->m_since_prev_node_PDF += incr_constraint;
73  }
74  } // ELSE - FORMAT #1
75 
76  bool registered = this->checkRegistrationCondition();
77 
78  if (registered)
79  {
80  if (m_observation_only_rawlog)
81  {
82  // keep track of the odometry-only pose_t at the last inserted graph
83  // node
84  m_last_odometry_only_pose = m_curr_odometry_only_pose;
85  }
86  }
87 
88  return registered;
89 
90  MRPT_END
91 } // end of updateState
92 
93 template <class GRAPH_T>
95 {
97 
98  // check that a node has already been registered - if not, default to
99  // (0,0,0)
100  pose_t last_pose_inserted =
101  this->m_prev_registered_nodeID != INVALID_NODEID
102  ? this->m_graph->nodes.at(this->m_prev_registered_nodeID)
103  : pose_t();
104 
105  // odometry criterion
106  bool registered = false;
107 
108  if (this->checkRegistrationCondition(
109  last_pose_inserted, this->getCurrentRobotPosEstimation()))
110  {
111  registered = this->registerNewNodeAtEnd();
112  }
113 
114  return registered;
115  MRPT_END
116 } // end of checkRegistrationCondition
117 
118 template <class GRAPH_T>
120  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2) const
121 {
122  using namespace mrpt::math;
123 
124  bool res = false;
125  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
126  (fabs(wrapToPi(p1.phi() - p2.phi())) > params.registration_max_angle))
127  {
128  res = true;
129  }
130 
131  return res;
132 } // end of checkRegistrationCondition2D
133 
134 template <class GRAPH_T>
136  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2) const
137 {
138  using namespace mrpt::math;
139 
140  bool res = false;
141  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
142  (fabs(wrapToPi(p1.roll() - p2.roll())) >
143  params.registration_max_angle) ||
144  (fabs(wrapToPi(p1.pitch() - p2.pitch())) >
145  params.registration_max_angle) ||
146  (fabs(wrapToPi(p1.yaw() - p2.yaw())) > params.registration_max_angle))
147  {
148  res = true;
149  }
150 
151  return res;
152 } // end of checkRegistrationCondition3D
153 
154 template <class GRAPH_T>
155 void CFixedIntervalsNRD<GRAPH_T>::loadParams(const std::string& source_fname)
156 {
157  MRPT_START
158  parent_t::loadParams(source_fname);
159 
160  params.loadFromConfigFileName(
161  source_fname, "NodeRegistrationDeciderParameters");
162 
163  // set the logging level if given by the user
164  mrpt::config::CConfigFile source(source_fname);
165  int min_verbosity_level = source.read_int(
166  "NodeRegistrationDeciderParameters", "class_verbosity", 1, false);
167  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
168 
169  MRPT_LOG_DEBUG("Successfully loaded parameters.");
170  MRPT_END
171 }
172 
173 template <class GRAPH_T>
175 {
176  MRPT_START
177  parent_t::printParams();
178  params.dumpToConsole();
179 
180  MRPT_END
181 }
182 
183 template <class GRAPH_T>
185  std::string* report_str) const
186 {
187  MRPT_START
188  using namespace std;
189 
190  const std::string report_sep(2, '\n');
191  const std::string header_sep(80, '#');
192 
193  // Report on graph
194  stringstream class_props_ss;
195  class_props_ss << "Strategy: "
196  << "Fixed Odometry-based Intervals" << std::endl;
197  class_props_ss << header_sep << std::endl;
198 
199  // time and output logging
200  const std::string time_res = this->m_time_logger.getStatsAsText();
201  const std::string output_res = this->getLogAsString();
202 
203  // merge the individual reports
204  report_str->clear();
205  parent_t::getDescriptiveReport(report_str);
206 
207  *report_str += class_props_ss.str();
208  *report_str += report_sep;
209 
210  // configuration parameters
211  *report_str += params.getAsString();
212  *report_str += report_sep;
213 
214  // loggers results
215  *report_str += time_res;
216  *report_str += report_sep;
217 
218  *report_str += output_res;
219  *report_str += report_sep;
220 
221  MRPT_END
222 }
223 
224 template <class GRAPH_T>
226  std::ostream& out) const
227 {
228  MRPT_START
229  out << mrpt::format("%s", this->getAsString().c_str());
230  MRPT_END
231 }
232 template <class GRAPH_T>
234  const mrpt::config::CConfigFileBase& source, const std::string& section)
235 {
236  MRPT_START
237  using namespace mrpt::math;
238 
239  registration_max_distance = source.read_double(
240  section, "registration_max_distance", 0.5 /* meter */, false);
241  registration_max_angle = source.read_double(
242  section, "registration_max_angle", 60 /* degrees */, false);
243  registration_max_angle = DEG2RAD(registration_max_angle);
244 
245  MRPT_END
246 }
247 
248 template <class GRAPH_T>
250  std::string* params_out) const
251 {
252  MRPT_START
253  using namespace mrpt::math;
254 
255  double max_angle_deg = RAD2DEG(registration_max_angle);
256  params_out->clear();
257 
258  *params_out +=
259  "------------------[ Fixed Intervals Node Registration "
260  "]------------------\n";
261  *params_out += mrpt::format(
262  "Max distance for registration = %.2f m\n", registration_max_distance);
263  *params_out += mrpt::format(
264  "Max angle for registration = %.2f deg\n", max_angle_deg);
265 
266  MRPT_END
267 }
268 template <class GRAPH_T>
270 {
271  MRPT_START
272 
273  std::string str;
274  this->getAsString(&str);
275  return str;
276 
277  MRPT_END
278 }
279 } // namespace mrpt::graphslam::deciders
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
VerbosityLevel
Enumeration of available verbosity levels.
void printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
app setMinLoggingLevel(mrpt::system::LVL_ERROR)
This class allows loading and storing values and vectors of different types from ".ini" files easily.
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
mrpt::vision::TStereoCalibParams params
STL namespace.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
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.
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
constexpr double DEG2RAD(const double x)
Degrees to radians.
bool checkRegistrationCondition() override
If estimated position surpasses the registration max values since the previous registered node...
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 read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
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...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
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.
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_END
Definition: exceptions.h:245
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
#define INVALID_NODEID
Definition: TNodeID.h:19
An observation of the current (cumulative) odometry for a wheeled robot.
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.
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) override
Method makes use of the CActionCollection/CObservation to update the odometry estimation from the las...



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 7b5ddf9de Fri May 29 14:02:56 2020 +0200 at vie may 29 14:15:09 CEST 2020