MRPT  1.9.9
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-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
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  mrpt::obs::CSensoryFrame::Ptr observations,
31  mrpt::obs::CObservation::Ptr observation)
32 {
33  MRPT_START;
34  using namespace mrpt::obs;
35  using namespace mrpt::math;
36  using namespace mrpt::poses;
37 
38  // don't use the measurements in this implementation
39  MRPT_UNUSED_PARAM(observations);
40 
41  if (observation)
42  { // FORMAT #2 - observation-only format
43  m_observation_only_rawlog = true;
44 
45  if (IS_CLASS(*observation, CObservationOdometry))
46  {
47  CObservationOdometry::Ptr obs_odometry =
48  std::dynamic_pointer_cast<CObservationOdometry>(observation);
49  // not incremental - gives the absolute odometry reading
50  m_curr_odometry_only_pose = pose_t(obs_odometry->odometry);
52  "Current odometry-only pose: %s",
53  m_curr_odometry_only_pose.asString().c_str());
54 
55  // I don't have any information about the covariane of the move in
56  // observation-only format
57  this->m_since_prev_node_PDF.mean =
58  m_curr_odometry_only_pose - m_last_odometry_only_pose;
59  }
60  }
61  else
62  { // FORMAT #1 - action-observation format
63  m_observation_only_rawlog = false;
64 
66  bool found = action->getFirstMovementEstimation(move_pdf);
67  if (found)
68  {
69  // update the relative PDF of the path since the LAST node was
70  // inserted
71  constraint_t incr_constraint;
72  incr_constraint.copyFrom(move_pdf);
73  this->m_since_prev_node_PDF += incr_constraint;
74  }
75  } // ELSE - FORMAT #1
76 
77  bool registered = this->checkRegistrationCondition();
78 
79  if (registered)
80  {
81  if (m_observation_only_rawlog)
82  {
83  // keep track of the odometry-only pose_t at the last inserted graph
84  // node
85  m_last_odometry_only_pose = m_curr_odometry_only_pose;
86  }
87  }
88 
89  return registered;
90 
91  MRPT_END;
92 } // end of updateState
93 
94 template <class GRAPH_T>
96 {
97  MRPT_START;
98 
99  // check that a node has already been registered - if not, default to
100  // (0,0,0)
101  pose_t last_pose_inserted =
102  this->m_prev_registered_nodeID != INVALID_NODEID
103  ? this->m_graph->nodes.at(this->m_prev_registered_nodeID)
104  : pose_t();
105 
106  // odometry criterion
107  bool registered = false;
108 
109  if (this->checkRegistrationCondition(
110  last_pose_inserted, this->getCurrentRobotPosEstimation()))
111  {
112  registered = this->registerNewNodeAtEnd();
113  }
114 
115  return registered;
116  MRPT_END;
117 } // end of checkRegistrationCondition
118 
119 template <class GRAPH_T>
121  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2) const
122 {
123  using namespace mrpt::math;
124 
125  bool res = false;
126  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
127  (fabs(wrapToPi(p1.phi() - p2.phi())) > params.registration_max_angle))
128  {
129  res = true;
130  }
131 
132  return res;
133 } // end of checkRegistrationCondition2D
134 
135 template <class GRAPH_T>
137  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2) const
138 {
139  using namespace mrpt::math;
140 
141  bool res = false;
142  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
143  (fabs(wrapToPi(p1.roll() - p2.roll())) >
144  params.registration_max_angle) ||
145  (fabs(wrapToPi(p1.pitch() - p2.pitch())) >
146  params.registration_max_angle) ||
147  (fabs(wrapToPi(p1.yaw() - p2.yaw())) > params.registration_max_angle))
148  {
149  res = true;
150  }
151 
152  return res;
153 } // end of checkRegistrationCondition3D
154 
155 template <class GRAPH_T>
157 {
158  MRPT_START;
159  parent_t::loadParams(source_fname);
160 
161  params.loadFromConfigFileName(
162  source_fname, "NodeRegistrationDeciderParameters");
163 
164  // set the logging level if given by the user
165  mrpt::config::CConfigFile source(source_fname);
166  int min_verbosity_level = source.read_int(
167  "NodeRegistrationDeciderParameters", "class_verbosity", 1, false);
168  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
169 
170  MRPT_LOG_DEBUG("Successfully loaded parameters.");
171  MRPT_END;
172 }
173 
174 template <class GRAPH_T>
176 {
177  MRPT_START;
178  parent_t::printParams();
179  params.dumpToConsole();
180 
181  MRPT_END;
182 }
183 
184 template <class GRAPH_T>
186  std::string* report_str) const
187 {
188  MRPT_START;
189  using namespace std;
190 
191  const std::string report_sep(2, '\n');
192  const std::string header_sep(80, '#');
193 
194  // Report on graph
195  stringstream class_props_ss;
196  class_props_ss << "Strategy: "
197  << "Fixed Odometry-based Intervals" << std::endl;
198  class_props_ss << header_sep << std::endl;
199 
200  // time and output logging
201  const std::string time_res = this->m_time_logger.getStatsAsText();
202  const std::string output_res = this->getLogAsString();
203 
204  // merge the individual reports
205  report_str->clear();
206  parent_t::getDescriptiveReport(report_str);
207 
208  *report_str += class_props_ss.str();
209  *report_str += report_sep;
210 
211  // configuration parameters
212  *report_str += params.getAsString();
213  *report_str += report_sep;
214 
215  // loggers results
216  *report_str += time_res;
217  *report_str += report_sep;
218 
219  *report_str += output_res;
220  *report_str += report_sep;
221 
222  MRPT_END;
223 }
224 
225 template <class GRAPH_T>
227  std::ostream& out) const
228 {
229  MRPT_START;
230  out << mrpt::format("%s", this->getAsString().c_str());
231  MRPT_END;
232 }
233 template <class GRAPH_T>
235  const mrpt::config::CConfigFileBase& source, const std::string& section)
236 {
237  MRPT_START;
238  using namespace mrpt::math;
239 
240  registration_max_distance = source.read_double(
241  section, "registration_max_distance", 0.5 /* meter */, false);
242  registration_max_angle = source.read_double(
243  section, "registration_max_angle", 60 /* degrees */, false);
244  registration_max_angle = DEG2RAD(registration_max_angle);
245 
246  MRPT_END;
247 }
248 
249 template <class GRAPH_T>
251  std::string* params_out) const
252 {
253  MRPT_START;
254  using namespace mrpt::math;
255 
256  double max_angle_deg = RAD2DEG(registration_max_angle);
257  params_out->clear();
258 
259  *params_out +=
260  "------------------[ Fixed Intervals Node Registration "
261  "]------------------\n";
262  *params_out += mrpt::format(
263  "Max distance for registration = %.2f m\n", registration_max_distance);
264  *params_out += mrpt::format(
265  "Max angle for registration = %.2f deg\n", max_angle_deg);
266 
267  MRPT_END;
268 }
269 template <class GRAPH_T>
271 {
272  MRPT_START;
273 
274  std::string str;
275  this->getAsString(&str);
276  return str;
277 
278  MRPT_END;
279 }
280 } // namespace mrpt::graphslam::deciders
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
app setMinLoggingLevel(mrpt::system::LVL_ERROR)
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
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:548
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
STL namespace.
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);
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:133
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...
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:554
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:84
mrpt::vision::TStereoCalibResults out
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
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.
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
#define INVALID_NODEID
Definition: TNodeID.h:19
GLuint res
Definition: glext.h:7385
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
GLenum const GLfloat * params
Definition: glext.h:3538
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...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



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