MRPT  2.0.3
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
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::graphslam::deciders::CFixedIntervalsNRD::TParams::getAsString
std::string getAsString() const
Definition: CFixedIntervalsNRD_impl.h:269
mrpt::obs::CObservationOdometry::Ptr
std::shared_ptr< mrpt::obs ::CObservationOdometry > Ptr
Definition: CObservationOdometry.h:31
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:39
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< mrpt::obs ::CSensoryFrame > Ptr
Definition: CSensoryFrame.h:53
mrpt::poses::CPose2D::phi
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
mrpt::config::CConfigFileBase::read_double
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:106
mrpt::graphslam::deciders::CFixedIntervalsNRD::getDescriptiveReport
void getDescriptiveReport(std::string *report_str) const override
Fill the provided string with a detailed report of the decider/optimizer state.
Definition: CFixedIntervalsNRD_impl.h:184
IS_CLASS
#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
CConfigFile.h
mrpt::graphslam::deciders::CFixedIntervalsNRD::TParams::loadFromConfigFile
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.
Definition: CFixedIntervalsNRD_impl.h:233
mrpt::graphslam::deciders::CFixedIntervalsNRD::CFixedIntervalsNRD
CFixedIntervalsNRD()
Definition: CFixedIntervalsNRD_impl.h:19
setMinLoggingLevel
app setMinLoggingLevel(mrpt::system::LVL_ERROR)
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< mrpt::obs ::CActionCollection > Ptr
Definition: CActionCollection.h:28
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
MRPT_LOG_DEBUG_FMT
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
Definition: system/COutputLogger.h:461
mrpt::graphslam::deciders::CFixedIntervalsNRD::printParams
void printParams() const override
Definition: CFixedIntervalsNRD_impl.h:174
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::graphslam::deciders::CFixedIntervalsNRD::pose_t
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
Definition: CFixedIntervalsNRD.h:83
mrpt::obs::CObservationOdometry
An observation of the current (cumulative) odometry for a wheeled robot.
Definition: CObservationOdometry.h:29
mrpt::config::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:130
mrpt::graphslam::deciders::CFixedIntervalsNRD::TParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CFixedIntervalsNRD_impl.h:225
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
mrpt::system::VerbosityLevel
VerbosityLevel
Enumeration of available verbosity levels.
Definition: system/COutputLogger.h:28
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::RAD2DEG
constexpr double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:56
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::graphslam::deciders::CFixedIntervalsNRD::constraint_t
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
Definition: CFixedIntervalsNRD.h:81
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::graphslam::deciders::CFixedIntervalsNRD::loadParams
void loadParams(const std::string &source_fname) override
Load the necessary for the decider/optimizer configuration parameters.
Definition: CFixedIntervalsNRD_impl.h:155
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::config::CConfigFile
This class allows loading and storing values and vectors of different types from "....
Definition: config/CConfigFile.h:31
mrpt::graphslam::deciders::CFixedIntervalsNRD::checkRegistrationCondition
bool checkRegistrationCondition() override
If estimated position surpasses the registration max values since the previous registered node,...
Definition: CFixedIntervalsNRD_impl.h:94
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
INVALID_NODEID
#define INVALID_NODEID
Definition: TNodeID.h:19
mrpt::poses::CPose3DPDFGaussian::copyFrom
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPose3DPDFGaussian.cpp:245
mrpt::graphslam::deciders
Definition: CEmptyERD.h:16
mrpt::graphslam::deciders::CFixedIntervalsNRD::updateState
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...
Definition: CFixedIntervalsNRD_impl.h:28
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020