Main MRPT website > C++ reference for MRPT 1.5.6
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 */
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:393
GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
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.
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
#define INVALID_NODEID
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
This class allows loading and storing values and vectors of different types from a configuration text...
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
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
#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...
bool checkRegistrationCondition()
If estimated position surpasses the registration max values since the previous registered node...
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:150
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.
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
#define DEG2RAD
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
GLuint res
Definition: glew.h:7143
#define MRPT_START
#define RAD2DEG
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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
#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
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:392
GLfloat * params
Definition: glew.h:1436
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
An observation of the current (cumulative) odometry for a wheeled robot.
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018