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");
VerbosityLevel
Enumeration of available verbosity levels.
double RAD2DEG(const double x)
Radians to degrees.
void printParams() const override
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
double DEG2RAD(const double x)
Degrees to radians.
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);
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
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
#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: ee555d257 Fri Aug 16 10:05:39 2019 +0200 at vie ago 16 10:10:14 CEST 2019