Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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
14 {
15 namespace graphslam
16 {
17 namespace deciders
18 {
19 // Ctors, Dtors
20 //////////////////////////////////////////////////////////////
21 
22 template <class GRAPH_T>
24 {
25  this->initializeLoggers("CFixedIntervalsNRD");
26  MRPT_LOG_DEBUG("IntervalsNRD: Initialized class object");
27 }
28 template <class GRAPH_T>
30 {
31 }
32 
33 // Member function implementations
34 //////////////////////////////////////////////////////////////
35 
36 template <class GRAPH_T>
39  mrpt::obs::CSensoryFrame::Ptr observations,
40  mrpt::obs::CObservation::Ptr observation)
41 {
42  MRPT_START;
43  using namespace mrpt::obs;
44  using namespace mrpt::math;
45  using namespace mrpt::poses;
46 
47  // don't use the measurements in this implementation
48  MRPT_UNUSED_PARAM(observations);
49 
50  if (observation)
51  { // FORMAT #2 - observation-only format
52  m_observation_only_rawlog = true;
53 
54  if (IS_CLASS(observation, CObservationOdometry))
55  {
56  CObservationOdometry::Ptr obs_odometry =
57  std::dynamic_pointer_cast<CObservationOdometry>(observation);
58  // not incremental - gives the absolute odometry reading
59  m_curr_odometry_only_pose = pose_t(obs_odometry->odometry);
61  "Current odometry-only pose: %s",
62  m_curr_odometry_only_pose.asString().c_str());
63 
64  // I don't have any information about the covariane of the move in
65  // observation-only format
66  this->m_since_prev_node_PDF.mean =
67  m_curr_odometry_only_pose - m_last_odometry_only_pose;
68  }
69  }
70  else
71  { // FORMAT #1 - action-observation format
72  m_observation_only_rawlog = false;
73 
75  bool found = action->getFirstMovementEstimation(move_pdf);
76  if (found)
77  {
78  // update the relative PDF of the path since the LAST node was
79  // inserted
80  constraint_t incr_constraint;
81  incr_constraint.copyFrom(move_pdf);
82  this->m_since_prev_node_PDF += incr_constraint;
83  }
84  } // ELSE - FORMAT #1
85 
86  bool registered = this->checkRegistrationCondition();
87 
88  if (registered)
89  {
90  if (m_observation_only_rawlog)
91  {
92  // keep track of the odometry-only pose_t at the last inserted graph
93  // node
94  m_last_odometry_only_pose = m_curr_odometry_only_pose;
95  }
96  }
97 
98  return registered;
99 
100  MRPT_END;
101 } // end of updateState
102 
103 template <class GRAPH_T>
105 {
106  MRPT_START;
107 
108  // check that a node has already been registered - if not, default to
109  // (0,0,0)
110  pose_t last_pose_inserted =
111  this->m_prev_registered_nodeID != INVALID_NODEID
112  ? this->m_graph->nodes.at(this->m_prev_registered_nodeID)
113  : pose_t();
114 
115  // odometry criterion
116  bool registered = false;
117 
118  if (this->checkRegistrationCondition(
119  last_pose_inserted, this->getCurrentRobotPosEstimation()))
120  {
121  registered = this->registerNewNodeAtEnd();
122  }
123 
124  return registered;
125  MRPT_END;
126 } // end of checkRegistrationCondition
127 
128 template <class GRAPH_T>
130  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2) const
131 {
132  using namespace mrpt::math;
133 
134  bool res = false;
135  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
136  (fabs(wrapToPi(p1.phi() - p2.phi())) > params.registration_max_angle))
137  {
138  res = true;
139  }
140 
141  return res;
142 } // end of checkRegistrationCondition2D
143 
144 template <class GRAPH_T>
146  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2) const
147 {
148  using namespace mrpt::math;
149 
150  bool res = false;
151  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
152  (fabs(wrapToPi(p1.roll() - p2.roll())) >
153  params.registration_max_angle) ||
154  (fabs(wrapToPi(p1.pitch() - p2.pitch())) >
155  params.registration_max_angle) ||
156  (fabs(wrapToPi(p1.yaw() - p2.yaw())) > params.registration_max_angle))
157  {
158  res = true;
159  }
160 
161  return res;
162 } // end of checkRegistrationCondition3D
163 
164 template <class GRAPH_T>
166 {
167  MRPT_START;
168  parent_t::loadParams(source_fname);
169 
170  params.loadFromConfigFileName(
171  source_fname, "NodeRegistrationDeciderParameters");
172 
173  // set the logging level if given by the user
174  mrpt::config::CConfigFile source(source_fname);
175  int min_verbosity_level = source.read_int(
176  "NodeRegistrationDeciderParameters", "class_verbosity", 1, false);
177  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
178 
179  MRPT_LOG_DEBUG("Successfully loaded parameters.");
180  MRPT_END;
181 }
182 
183 template <class GRAPH_T>
185 {
186  MRPT_START;
187  parent_t::printParams();
188  params.dumpToConsole();
189 
190  MRPT_END;
191 }
192 
193 template <class GRAPH_T>
195  std::string* report_str) const
196 {
197  MRPT_START;
198  using namespace std;
199 
200  const std::string report_sep(2, '\n');
201  const std::string header_sep(80, '#');
202 
203  // Report on graph
204  stringstream class_props_ss;
205  class_props_ss << "Strategy: "
206  << "Fixed Odometry-based Intervals" << std::endl;
207  class_props_ss << header_sep << std::endl;
208 
209  // time and output logging
210  const std::string time_res = this->m_time_logger.getStatsAsText();
211  const std::string output_res = this->getLogAsString();
212 
213  // merge the individual reports
214  report_str->clear();
215  parent_t::getDescriptiveReport(report_str);
216 
217  *report_str += class_props_ss.str();
218  *report_str += report_sep;
219 
220  // configuration parameters
221  *report_str += params.getAsString();
222  *report_str += report_sep;
223 
224  // loggers results
225  *report_str += time_res;
226  *report_str += report_sep;
227 
228  *report_str += output_res;
229  *report_str += report_sep;
230 
231  MRPT_END;
232 }
233 
234 // TParams
235 //////////////////////////////////////////////////////////////
236 template <class GRAPH_T>
238 {
239 }
240 template <class GRAPH_T>
242 {
243 }
244 template <class GRAPH_T>
246  std::ostream& out) const
247 {
248  MRPT_START;
249  out << mrpt::format("%s", this->getAsString().c_str());
250  MRPT_END;
251 }
252 template <class GRAPH_T>
254  const mrpt::config::CConfigFileBase& source, const std::string& section)
255 {
256  MRPT_START;
257  using namespace mrpt::math;
258 
259  registration_max_distance = source.read_double(
260  section, "registration_max_distance", 0.5 /* meter */, false);
261  registration_max_angle = source.read_double(
262  section, "registration_max_angle", 60 /* degrees */, false);
263  registration_max_angle = DEG2RAD(registration_max_angle);
264 
265  MRPT_END;
266 }
267 
268 template <class GRAPH_T>
270  std::string* params_out) const
271 {
272  MRPT_START;
273  using namespace mrpt::math;
274 
275  double max_angle_deg = RAD2DEG(registration_max_angle);
276  params_out->clear();
277 
278  *params_out +=
279  "------------------[ Fixed Intervals Node Registration "
280  "]------------------\n";
281  *params_out += mrpt::format(
282  "Max distance for registration = %.2f m\n", registration_max_distance);
283  *params_out += mrpt::format(
284  "Max angle for registration = %.2f deg\n", max_angle_deg);
285 
286  MRPT_END;
287 }
288 template <class GRAPH_T>
290 {
291  MRPT_START;
292 
293  std::string str;
294  this->getAsString(&str);
295  return str;
296 
297  MRPT_END;
298 }
299 }
300 }
301 } // end of namespaces
302 
303 #endif /* end of include guard: CFIXEDINTERVALSNRD_IMPL_H */
#define MRPT_START
Definition: exceptions.h:262
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
VerbosityLevel
Enumeration of available verbosity levels.
double RAD2DEG(const double x)
Radians to degrees.
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:211
double DEG2RAD(const double x)
Degrees to radians.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:534
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:528
STL namespace.
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
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);
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
bool checkRegistrationCondition()
If estimated position surpasses the registration max values since the previous registered node...
GLsizei const GLchar ** string
Definition: glext.h:4101
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
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:540
bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
Method makes use of the CActionCollection/CObservation to update the odometry estimation from the las...
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
#define MRPT_END
Definition: exceptions.h:266
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
#define INVALID_NODEID
Definition: TNodeID.h:20
GLuint res
Definition: glext.h:7268
An observation of the current (cumulative) odometry for a wheeled robot.
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
GLenum const GLfloat * params
Definition: glext.h:3534
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
#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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019