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 */
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::graphslam::deciders::CFixedIntervalsNRD::TParams::getAsString
std::string getAsString() const
Definition: CFixedIntervalsNRD_impl.h:289
mrpt::graphslam::deciders::CFixedIntervalsNRD::TParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form,...
Definition: CFixedIntervalsNRD_impl.h:245
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:40
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< CSensoryFrame > Ptr
Definition: CSensoryFrame.h:56
mrpt::obs::CObservationOdometry::Ptr
std::shared_ptr< CObservationOdometry > Ptr
Definition: CObservationOdometry.h:34
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:534
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
mrpt::poses::CPose2D::phi
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
mrpt::poses::CPoseOrPoint::distanceTo
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:211
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::graphslam::deciders::CFixedIntervalsNRD::CFixedIntervalsNRD
CFixedIntervalsNRD()
Class constructor.
Definition: CFixedIntervalsNRD_impl.h:23
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::RAD2DEG
double RAD2DEG(const double x)
Radians to degrees.
Definition: core/include/mrpt/core/bits_math.h:48
MRPT_LOG_DEBUG_FMT
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
Definition: system/COutputLogger.h:461
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::graphslam::deciders::CFixedIntervalsNRD::pose_t
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
Definition: CFixedIntervalsNRD.h:88
mrpt::graphslam::deciders::CFixedIntervalsNRD::updateState
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...
Definition: CFixedIntervalsNRD_impl.h:37
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::obs::CObservationOdometry
An observation of the current (cumulative) odometry for a wheeled robot.
Definition: CObservationOdometry.h:32
mrpt::graphslam::deciders::CFixedIntervalsNRD::getDescriptiveReport
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
Definition: CFixedIntervalsNRD_impl.h:194
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
mrpt::system::VerbosityLevel
VerbosityLevel
Enumeration of available verbosity levels.
Definition: system/COutputLogger.h:28
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:40
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
res
GLuint res
Definition: glext.h:7268
mrpt::graphslam::deciders::CFixedIntervalsNRD::printParams
void printParams() const
Definition: CFixedIntervalsNRD_impl.h:184
mrpt::graphslam::deciders::CFixedIntervalsNRD::constraint_t
typename GRAPH_T::constraint_t constraint_t
type of graph constraints
Definition: CFixedIntervalsNRD.h:86
IS_CLASS
#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
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:540
mrpt::graphslam::deciders::CFixedIntervalsNRD::checkRegistrationCondition
bool checkRegistrationCondition()
If estimated position surpasses the registration max values since the previous registered node,...
Definition: CFixedIntervalsNRD_impl.h:104
mrpt::graphslam::deciders::CFixedIntervalsNRD::TParams::TParams
TParams()
Definition: CFixedIntervalsNRD_impl.h:237
mrpt::graphslam::deciders::CFixedIntervalsNRD::~CFixedIntervalsNRD
~CFixedIntervalsNRD()
Class destructor.
Definition: CFixedIntervalsNRD_impl.h:29
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:528
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::config::CConfigFile
This class allows loading and storing values and vectors of different types from "....
Definition: config/CConfigFile.h:33
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::graphslam::deciders::CFixedIntervalsNRD::TParams::loadFromConfigFile
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.
Definition: CFixedIntervalsNRD_impl.h:253
INVALID_NODEID
#define INVALID_NODEID
Definition: TNodeID.h:20
mrpt::poses::CPose3DPDFGaussian::copyFrom
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPose3DPDFGaussian.cpp:251
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< CActionCollection > Ptr
Definition: CActionCollection.h:30
mrpt::graphslam::deciders::CFixedIntervalsNRD::loadParams
void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.
Definition: CFixedIntervalsNRD_impl.h:165
mrpt::graphslam::deciders::CFixedIntervalsNRD::TParams::~TParams
~TParams()
Definition: CFixedIntervalsNRD_impl.h:241
params
GLenum const GLfloat * params
Definition: glext.h:3534
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST