Main MRPT website > C++ reference for MRPT 1.9.9
CIncrementalNodeRegistrationDecider_impl.h
Go to the documentation of this file.
1 #ifndef CINCREMENTALNODEREGISTRATIONDECIDER_IMPL_H_UQKZGLEM
2 #define CINCREMENTALNODEREGISTRATIONDECIDER_IMPL_H_UQKZGLEM
3 
4 namespace mrpt
5 {
6 namespace graphslam
7 {
8 namespace deciders
9 {
10 template <class GRAPH_T>
11 CIncrementalNodeRegistrationDecider<
13 {
14 }
15 
16 template <class GRAPH_T>
18  GRAPH_T>::~CIncrementalNodeRegistrationDecider()
19 {
20 }
21 
22 template <class GRAPH_T>
24 {
25  MRPT_START;
26 
27  // check that a node has already been registered - if not, default to
28  // (0,0,0)
29  pose_t last_pose_inserted =
30  this->m_prev_registered_nodeID != INVALID_NODEID
31  ? this->m_graph->nodes.at(this->m_prev_registered_nodeID)
32  : pose_t();
33 
34  // odometry criterion
35  bool registered = false;
36 
37  if (this->checkRegistrationConditionPose(
38  last_pose_inserted, this->getCurrentRobotPosEstimation()))
39  {
40  registered = this->registerNewNodeAtEnd();
41  }
42 
43  return registered;
44  MRPT_END;
45 } // end of checkRegistrationCondition
46 
47 template <class GRAPH_T>
50  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2) const
51 {
52  using namespace mrpt::math;
53 
54  bool res = false;
55  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
56  (fabs(wrapToPi(p1.phi() - p2.phi())) > params.registration_max_angle))
57  {
58  res = true;
59  }
60 
61  return res;
62 } // end of checkRegistrationConditionPose
63 
64 template <class GRAPH_T>
67  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2) const
68 {
69  using namespace mrpt::math;
70 
71  std::cout << "In checkRegistrationConditionPose:\np1: " << p1.asString()
72  << "\np2: " << p1.asString() << std::endl;
73 
74  bool res = false;
75  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
76  (fabs(wrapToPi(p1.roll() - p2.roll())) >
77  params.registration_max_angle) ||
78  (fabs(wrapToPi(p1.pitch() - p2.pitch())) >
79  params.registration_max_angle) ||
80  (fabs(wrapToPi(p1.yaw() - p2.yaw())) > params.registration_max_angle))
81  {
82  res = true;
83  }
84 
85  return res;
86 } // end of checkRegistrationConditionPose
87 
88 template <class GRAPH_T>
90  const std::string& source_fname)
91 {
92  MRPT_START;
93  parent_t::loadParams(source_fname);
94 
95  params.loadFromConfigFileName(
96  source_fname, "NodeRegistrationDeciderParameters");
97 
98  MRPT_END;
99 }
100 
101 template <class GRAPH_T>
103 {
104  MRPT_START;
105  parent_t::printParams();
106  params.dumpToConsole();
107 
108  MRPT_END;
109 }
110 
111 template <class GRAPH_T>
113  std::string* report_str) const
114 {
115  MRPT_START;
116  using namespace std;
117 
118  *report_str += params.getAsString();
119  *report_str += this->report_sep;
120 
121  MRPT_END;
122 }
123 
124 ///////////////////////////////////////////////////////////////////////////////
125 // TParams
126 
127 template <class GRAPH_T>
129 {
130 }
131 
132 template <class GRAPH_T>
134 {
135 }
136 
137 template <class GRAPH_T>
139  std::ostream& out) const
140 {
141  MRPT_START;
142  out << mrpt::format("%s", this->getAsString().c_str());
143  MRPT_END;
144 }
145 
146 template <class GRAPH_T>
148  const mrpt::config::CConfigFileBase& source, const std::string& section)
149 {
150  MRPT_START;
151  using namespace mrpt::math;
152 
153  registration_max_distance = source.read_double(
154  section, "registration_max_distance", 0.5 /* meter */, false);
155  registration_max_angle = source.read_double(
156  section, "registration_max_angle", 15 /* degrees */, false);
157  registration_max_angle = DEG2RAD(registration_max_angle);
158 
159  MRPT_END;
160 }
161 
162 template <class GRAPH_T>
164  std::string* params_out) const
165 {
166  MRPT_START;
167  using namespace mrpt::math;
168 
169  double max_angle_deg = RAD2DEG(registration_max_angle);
170  params_out->clear();
171 
172  *params_out +=
173  "------------------[ Fixed Intervals Incremental Node Registration "
174  "]------------------\n";
175  *params_out += mrpt::format(
176  "Max distance for registration = %.2f m\n", registration_max_distance);
177  *params_out += mrpt::format(
178  "Max angle for registration = %.2f deg\n", max_angle_deg);
179 
180  MRPT_END;
181 }
182 
183 template <class GRAPH_T>
185  const
186 {
187  MRPT_START;
188 
189  std::string str;
190  this->getAsString(&str);
191  return str;
192 
193  MRPT_END;
194 }
195 }
196 }
197 } // end of namespaces
198 
199 #endif /* end of include guard: \
200  CINCREMENTALNODEREGISTRATIONDECIDER_IMPL_H_UQKZGLEM */
#define MRPT_START
Definition: exceptions.h:262
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
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.
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:534
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:528
STL namespace.
virtual bool checkRegistrationConditionPose(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2) const
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.
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.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:540
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]"...
Definition: CPose3D.h:602
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:40
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
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 MRPT_END
Definition: exceptions.h:266
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
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.
virtual bool checkRegistrationCondition()
If estimated position surpasses the registration max values since the previous registered node...
#define INVALID_NODEID
Definition: TNodeID.h:20
GLuint res
Definition: glext.h:7268
GLenum const GLfloat * params
Definition: glext.h:3534
virtual void loadParams(const std::string &source_fname)
Load the necessary for the decider/optimizer configuration parameters.



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