MRPT  2.0.4
CIncrementalNodeRegistrationDecider_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
13 {
14 template <class GRAPH_T>
16 {
18 
19  // check that a node has already been registered - if not, default to
20  // (0,0,0)
21  pose_t last_pose_inserted =
22  this->m_prev_registered_nodeID != INVALID_NODEID
23  ? this->m_graph->nodes.at(this->m_prev_registered_nodeID)
24  : pose_t();
25 
26  // odometry criterion
27  bool registered = false;
28 
29  if (this->checkRegistrationConditionPose(
30  last_pose_inserted, this->getCurrentRobotPosEstimation()))
31  {
32  registered = this->registerNewNodeAtEnd();
33  }
34 
35  return registered;
36  MRPT_END
37 } // end of checkRegistrationCondition
38 
39 template <class GRAPH_T>
42  const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2) const
43 {
44  using namespace mrpt::math;
45 
46  bool res = false;
47  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
48  (fabs(wrapToPi(p1.phi() - p2.phi())) > params.registration_max_angle))
49  {
50  res = true;
51  }
52 
53  return res;
54 } // end of checkRegistrationConditionPose
55 
56 template <class GRAPH_T>
59  const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2) const
60 {
61  using namespace mrpt::math;
62 
63  std::cout << "In checkRegistrationConditionPose:\np1: " << p1.asString()
64  << "\np2: " << p1.asString() << std::endl;
65 
66  bool res = false;
67  if ((p1.distanceTo(p2) > params.registration_max_distance) ||
68  (fabs(wrapToPi(p1.roll() - p2.roll())) >
69  params.registration_max_angle) ||
70  (fabs(wrapToPi(p1.pitch() - p2.pitch())) >
71  params.registration_max_angle) ||
72  (fabs(wrapToPi(p1.yaw() - p2.yaw())) > params.registration_max_angle))
73  {
74  res = true;
75  }
76 
77  return res;
78 } // end of checkRegistrationConditionPose
79 
80 template <class GRAPH_T>
82  const std::string& source_fname)
83 {
85  parent_t::loadParams(source_fname);
86 
87  params.loadFromConfigFileName(
88  source_fname, "NodeRegistrationDeciderParameters");
89 
90  MRPT_END
91 }
92 
93 template <class GRAPH_T>
95 {
97  parent_t::printParams();
98  params.dumpToConsole();
99 
100  MRPT_END
101 }
102 
103 template <class GRAPH_T>
105  std::string* report_str) const
106 {
107  MRPT_START
108  using namespace std;
109 
110  *report_str += params.getAsString();
111  *report_str += this->report_sep;
112 
113  MRPT_END
114 }
115 
116 template <class GRAPH_T>
118  std::ostream& out) const
119 {
120  MRPT_START
121  out << mrpt::format("%s", this->getAsString().c_str());
122  MRPT_END
123 }
124 
125 template <class GRAPH_T>
127  const mrpt::config::CConfigFileBase& source, const std::string& section)
128 {
129  MRPT_START
130  using namespace mrpt::math;
131 
132  registration_max_distance = source.read_double(
133  section, "registration_max_distance", 0.5 /* meter */, false);
134  registration_max_angle = source.read_double(
135  section, "registration_max_angle", 15 /* degrees */, false);
136  registration_max_angle = DEG2RAD(registration_max_angle);
137 
138  MRPT_END
139 }
140 
141 template <class GRAPH_T>
143  std::string* params_out) const
144 {
145  MRPT_START
146  using namespace mrpt::math;
147 
148  double max_angle_deg = RAD2DEG(registration_max_angle);
149  params_out->clear();
150 
151  *params_out +=
152  "------------------[ Fixed Intervals Incremental Node Registration "
153  "]------------------\n";
154  *params_out += mrpt::format(
155  "Max distance for registration = %.2f m\n", registration_max_distance);
156  *params_out += mrpt::format(
157  "Max angle for registration = %.2f deg\n", max_angle_deg);
158 
159  MRPT_END
160 }
161 
162 template <class GRAPH_T>
164  const
165 {
166  MRPT_START
167 
168  std::string str;
169  this->getAsString(&str);
170  return str;
171 
172  MRPT_END
173 }
174 } // namespace mrpt::graphslam::deciders
#define MRPT_START
Definition: exceptions.h:241
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
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:546
mrpt::vision::TStereoCalibParams params
STL namespace.
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
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.
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
constexpr double DEG2RAD(const double x)
Degrees to radians.
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.
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
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:618
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
constexpr double RAD2DEG(const double x)
Radians to degrees.
#define MRPT_END
Definition: exceptions.h:245
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:19
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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020