Main MRPT website > C++ reference for MRPT 1.5.7
CFixedIntervalsNRD_impl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #ifndef CFIXEDINTERVALSNRD_IMPL_H
11 #define CFIXEDINTERVALSNRD_IMPL_H
12 
13 namespace mrpt { namespace graphslam { namespace deciders {
14 
15 // Ctors, Dtors
16 //////////////////////////////////////////////////////////////
17 
18 template<class GRAPH_T>
20  using namespace mrpt::utils;
21  this->initializeLoggers("CFixedIntervalsNRD");
22 
23  this->logFmt(LVL_DEBUG, "IntervalsNRD: Initialized class object");
24 }
25 template<class GRAPH_T>
27 
28 // Member function implementations
29 //////////////////////////////////////////////////////////////
30 
31 template<class GRAPH_T>
33  mrpt::obs::CActionCollectionPtr action,
34  mrpt::obs::CSensoryFramePtr observations,
35  mrpt::obs::CObservationPtr observation ) {
36  MRPT_START;
37  using namespace mrpt::obs;
38  using namespace mrpt::math;
39  using namespace mrpt::utils;
40  using namespace mrpt::poses;
41 
42  // don't use the measurements in this implementation
43  MRPT_UNUSED_PARAM(observations);
44 
45  if (observation.present()) { // FORMAT #2 - observation-only format
46  m_observation_only_rawlog = true;
47 
48  if (IS_CLASS(observation, CObservationOdometry)) {
49 
50  CObservationOdometryPtr obs_odometry =
51  static_cast<CObservationOdometryPtr>(observation);
52  // not incremental - gives the absolute odometry reading
53  m_curr_odometry_only_pose = pose_t(obs_odometry->odometry);
54  this->logFmt(LVL_DEBUG, "Current odometry-only pose: %s",
55  m_curr_odometry_only_pose.asString().c_str());
56 
57  // I don't have any information about the covariane of the move in
58  // observation-only format
59  this->m_since_prev_node_PDF.mean =
60  m_curr_odometry_only_pose - m_last_odometry_only_pose;
61  }
62  }
63  else { // FORMAT #1 - action-observation format
64  m_observation_only_rawlog = false;
65 
67  bool found = action->getFirstMovementEstimation(move_pdf);
68  if (found) {
69  // update the relative PDF of the path since the LAST node was 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  if (m_observation_only_rawlog) {
80  // keep track of the odometry-only pose_t at the last inserted graph node
81  m_last_odometry_only_pose = m_curr_odometry_only_pose;
82  }
83  }
84 
85  return registered;
86 
87  MRPT_END;
88 } // end of updateState
89 
90 template<class GRAPH_T>
92  MRPT_START;
93 
94  // check that a node has already been registered - if not, default to (0,0,0)
95  pose_t last_pose_inserted = this->m_prev_registered_nodeID != INVALID_NODEID?
96  this->m_graph->nodes.at(this->m_prev_registered_nodeID): pose_t();
97 
98  // odometry criterion
99  bool registered = false;
100 
101  if (this->checkRegistrationCondition(
102  last_pose_inserted,
103  this->getCurrentRobotPosEstimation())) {
104  registered = this->registerNewNodeAtEnd();
105  }
106 
107  return registered;
108  MRPT_END;
109 } // end of checkRegistrationCondition
110 
111 template<class GRAPH_T>
113  const mrpt::poses::CPose2D& p1,
114  const mrpt::poses::CPose2D& p2) const {
115  using namespace mrpt::math;
116 
117  bool res = false;
118  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
119  (fabs(wrapToPi(p1.phi() - p2.phi())) > params.registration_max_angle)) {
120  res = true;
121  }
122 
123  return res;
124 } // end of checkRegistrationCondition2D
125 
126 template<class GRAPH_T>
128  const mrpt::poses::CPose3D& p1,
129  const mrpt::poses::CPose3D& p2) const {
130  using namespace mrpt::math;
131 
132  bool res = false;
133  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
134  (fabs(wrapToPi(p1.roll() - p2.roll())) > params.registration_max_angle) ||
135  (fabs(wrapToPi(p1.pitch() - p2.pitch())) > params.registration_max_angle) ||
136  (fabs(wrapToPi(p1.yaw() - p2.yaw())) > params.registration_max_angle)) {
137  res = true;
138  }
139 
140  return res;
141 } // end of checkRegistrationCondition3D
142 
143 template<class GRAPH_T>
145  MRPT_START;
146  using namespace mrpt::utils;
147  parent_t::loadParams(source_fname);
148 
149  params.loadFromConfigFileName(source_fname,
150  "NodeRegistrationDeciderParameters");
151 
152  // set the logging level if given by the user
153  CConfigFile source(source_fname);
154  int min_verbosity_level = source.read_int(
155  "NodeRegistrationDeciderParameters",
156  "class_verbosity",
157  1, false);
158  this->setMinLoggingLevel(VerbosityLevel(min_verbosity_level));
159 
160 
161  this->logFmt(LVL_DEBUG, "Successfully loaded parameters.");
162  MRPT_END;
163 }
164 
165 template<class GRAPH_T>
167  MRPT_START;
168  parent_t::printParams();
169  params.dumpToConsole();
170 
171  MRPT_END;
172 }
173 
174 template<class GRAPH_T>
176  MRPT_START;
177  using namespace std;
178 
179  const std::string report_sep(2, '\n');
180  const std::string header_sep(80, '#');
181 
182  // Report on graph
183  stringstream class_props_ss;
184  class_props_ss << "Strategy: " <<
185  "Fixed Odometry-based Intervals" << std::endl;
186  class_props_ss << header_sep << std::endl;
187 
188  // time and output logging
189  const std::string time_res = this->m_time_logger.getStatsAsText();
190  const std::string output_res = this->getLogAsString();
191 
192  // merge the individual reports
193  report_str->clear();
194  parent_t::getDescriptiveReport(report_str);
195 
196  *report_str += class_props_ss.str();
197  *report_str += report_sep;
198 
199  // configuration parameters
200  *report_str += params.getAsString();
201  *report_str += report_sep;
202 
203  // loggers results
204  *report_str += time_res;
205  *report_str += report_sep;
206 
207  *report_str += output_res;
208  *report_str += report_sep;
209 
210  MRPT_END;
211 }
212 
213 // TParams
214 //////////////////////////////////////////////////////////////
215 template<class GRAPH_T>
217 }
218 template<class GRAPH_T>
220 }
221 template<class GRAPH_T>
223  mrpt::utils::CStream &out) const {
224  MRPT_START;
225  out.printf("%s", this->getAsString().c_str());
226  MRPT_END;
227 }
228 template<class GRAPH_T>
231  const std::string &section) {
232  MRPT_START;
233  using namespace mrpt::math;
234  using namespace mrpt::utils;
235 
236  registration_max_distance = source.read_double( section,
237  "registration_max_distance",
238  0.5 /* meter */, false);
239  registration_max_angle = source.read_double( section,
240  "registration_max_angle",
241  60 /* degrees */, false);
242  registration_max_angle = DEG2RAD(registration_max_angle);
243 
244  MRPT_END;
245 }
246 
247 template<class GRAPH_T>
249  MRPT_START;
250  using namespace mrpt::math;
251  using namespace mrpt::utils;
252 
253  double max_angle_deg = RAD2DEG(registration_max_angle);
254  params_out->clear();
255 
256  *params_out +=
257  "------------------[ Fixed Intervals Node Registration ]------------------\n";
258  *params_out += mrpt::format(
259  "Max distance for registration = %.2f m\n", registration_max_distance);
260  *params_out += mrpt::format(
261  "Max angle for registration = %.2f deg\n", max_angle_deg);
262 
263  MRPT_END;
264 }
265 template<class GRAPH_T>
267  MRPT_START;
268 
269  std::string str;
270  this->getAsString(&str);
271  return str;
272 
273  MRPT_END;
274 }
275 
276 } } } // end of namespaces
277 
278 #endif /* end of include guard: CFIXEDINTERVALSNRD_IMPL_H */
GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
#define INVALID_NODEID
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
STL namespace.
This class allows loading and storing values and vectors of different types from a configuration text...
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
GRAPH_T::constraint_t constraint_t
type of graph constraints
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
bool updateState(mrpt::obs::CActionCollectionPtr action, mrpt::obs::CSensoryFramePtr observations, mrpt::obs::CObservationPtr observation)
Method makes use of the CActionCollection/CObservation to update the odometry estimation from the las...
This namespace contains representation of robot actions and observations.
bool checkRegistrationCondition()
If estimated position surpasses the registration max values since the previous registered node...
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
GLsizei const GLchar ** string
Definition: glext.h:3919
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
#define MRPT_START
#define RAD2DEG
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:103
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
GLuint res
Definition: glext.h:6298
An observation of the current (cumulative) odometry for a wheeled robot.
GLenum const GLfloat * params
Definition: glext.h:3514
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019