MRPT  1.9.9
TUncertaintyPath_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 // Implementattion file for TUncertaintyPath struct
15 
16 namespace mrpt::graphslam
17 {
18 template <class GRAPH_T>
20 {
21  this->clear();
22 }
23 template <class GRAPH_T>
25  const mrpt::graphs::TNodeID& starting_node)
26 {
27  this->clear();
28  nodes_traversed.push_back(starting_node);
29 }
30 template <class GRAPH_T>
32  const mrpt::graphs::TNodeID& starting_node,
33  const mrpt::graphs::TNodeID& ending_node, const constraint_t& edge)
34 {
35  this->clear();
36  nodes_traversed.push_back(starting_node);
37  this->addToPath(ending_node, edge);
38 }
39 
40 template <class GRAPH_T>
42 {
43  using namespace mrpt;
44  using namespace mrpt::math;
45  using namespace mrpt::poses;
46  using namespace std;
47 
48  // clear the vector of traversed nodes
49  nodes_traversed.clear();
50 
51  // clear the relative edge
52  curr_pose_pdf.mean = pose_t();
53  // by default the information matrix is set to the unit matrix
54  auto init_path_mat = CMatrixDouble33::Identity();
55 
56  // put a really large number - we are certain of this position
57  init_path_mat *= 10000; // TODO - justify this..
58  curr_pose_pdf.cov_inv = init_path_mat;
59 
60  determinant_is_updated = false;
61  determinant_cached = 0;
62 
63 } // end of clear
64 
65 template <class GRAPH_T>
67 {
68  return *this == self_t();
69 }
70 
71 template <class GRAPH_T>
73  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to) const
74 {
76  this->getSource() == from,
77  format(
78  "\nnodeID %lu is not the source of the path\n%s\n\n",
79  static_cast<unsigned long>(from), this->getAsString().c_str()));
81  this->getDestination() == to,
82  format(
83  "\nnodeID %lu is not the destination of the path\n%s\n\n",
84  static_cast<unsigned long>(to), this->getAsString().c_str()));
85 }
86 
87 template <class GRAPH_T>
89  const self_t& other)
90 {
91  using namespace mrpt;
92  using namespace mrpt::math;
93  using namespace mrpt::poses;
94  using namespace std;
95 
96  // other path should start where this ends
98  other.nodes_traversed.begin()[0] == this->nodes_traversed.rbegin()[0],
99  "\"other\" instance must start from the nodeID that this "
100  "TUncertaintyPath has ended.");
102  other.nodes_traversed.size(),
103  "\"other\" instance doesn't have an initialized list of traversed "
104  "nodes");
106  this->nodes_traversed.size(),
107  "\"this\" instance doesn't have an initialized list of traversed "
108  "nodes");
109 
110  //////// TODO Remove these - >>>>>>>>>>>>>>>>>>>>>
111  // cout << string(20, '-') << "Aggregating 2 paths.."
112  //<< string(20, '-') << endl;
113  // this->dumpToConsole(); other.dumpToConsole();
114  ////// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<
115 
116  // aggregate the two gaussian - mean & information matrix
117  this->curr_pose_pdf += other.curr_pose_pdf;
118 
119  // add the traversed nodes
120  this->nodes_traversed.insert(
121  this->nodes_traversed.end(), other.nodes_traversed.begin() + 1,
122  other.nodes_traversed.end());
123 
124  ////// TODO Remove these - >>>>>>>>>>>>>>>>>>>>>
125  // cout << std::string(10, '%') << endl << "AFTER Aggregation..." << endl;
126  // this->dumpToConsole();
127  // cout << string(50, '-') << endl;
128  // mrpt::system::pause();
129  ////// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<
130 
131  determinant_is_updated = false;
132  return *this;
133 }
134 template <class GRAPH_T>
136 {
137  using namespace mrpt;
138  using namespace mrpt::math;
139  using namespace mrpt::poses;
140  using namespace std;
141 
142  // check if the traversed nodes are the same as well as the
143  // CPoseGaussianInfs are the same..
144  return (
145  this->nodes_traversed == other.nodes_traversed &&
146  this->curr_pose_pdf == other.curr_pose_pdf);
147 }
148 template <class GRAPH_T>
150 {
151  return !(*this == other);
152 }
153 
154 template <class GRAPH_T>
156  const mrpt::graphs::TNodeID& node, const constraint_t& edge)
157 {
158  // update the path
159  curr_pose_pdf += edge;
160 
161  // update the traversed nodes
162  nodes_traversed.push_back(node);
163 
164  determinant_is_updated = false;
165 }
166 
167 template <class GRAPH_T>
169  const mrpt::config::CConfigFileBase& source, const std::string& section)
170 {
171 }
172 
173 template <class GRAPH_T>
175 {
176  out << mrpt::format("%s\n", this->getAsString().c_str());
177 }
178 
179 template <class GRAPH_T>
180 void TUncertaintyPath<GRAPH_T>::getAsString(std::string* str) const
181 {
182  using namespace mrpt;
183  using namespace mrpt::poses;
184  using namespace std;
185  using namespace mrpt::math;
186  using namespace mrpt::containers;
187 
188  stringstream ss;
189  string header_sep(30, '=');
190 
191  ss << "Path properties: " << endl;
192  ss << header_sep << endl << endl;
193 
194  ss << "- CPosePDFGaussianInf: "
195  << (curr_pose_pdf.isInfType() ? "TRUE" : "FALSE") << endl;
196  ss << "- Nodes list: \n\t< " << getSTLContainerAsString(nodes_traversed)
197  << "\b\b>" << endl;
198 
199  ss << endl;
200  ss << curr_pose_pdf << endl;
201  ss << endl;
202 
203  CMatrixDouble33 mat;
204  if (curr_pose_pdf.isInfType())
205  {
206  curr_pose_pdf.getInformationMatrix(mat);
207  }
208  else
209  {
210  curr_pose_pdf.getCovariance(mat);
211  }
212  ss << "Determinant: " << mat.det();
213 
214  *str = ss.str();
215 }
216 template <class GRAPH_T>
218 {
219  std::string s;
220  this->getAsString(&s);
221  return s;
222 }
223 
224 template <class GRAPH_T>
226 {
227  return nodes_traversed.at(0);
228 }
229 template <class GRAPH_T>
231 {
232  return nodes_traversed.back();
233 }
234 
235 template <class GRAPH_T>
237 {
238  using namespace mrpt;
239  using namespace mrpt::math;
240  using namespace mrpt::poses;
241  using namespace std;
242 
243  // if determinant is up-to-date then return the cached version...
244  if (determinant_is_updated) return determinant_cached;
245 
246  // update the cached version and return it.
247  CMatrixDouble33 mat;
248  if (curr_pose_pdf.isInfType())
249  {
250  curr_pose_pdf.getInformationMatrix(mat);
251  }
252  else
253  {
254  curr_pose_pdf.getCovariance(mat);
255  }
256  double determinant = mat.det();
257 
258  determinant_cached = determinant;
259  determinant_is_updated = true;
260 
261  return determinant;
262 }
263 
264 template <class GRAPH_T>
266  const self_t& other) const
267 {
268  using namespace mrpt;
269  using namespace mrpt::math;
270  using namespace mrpt::poses;
271  using namespace std;
272 
274  (curr_pose_pdf.isInfType() && other.curr_pose_pdf.isInfType()) ||
275  (!curr_pose_pdf.isInfType() && !other.curr_pose_pdf.isInfType()),
276  mrpt::format("Constraints of given paths don't have the same "
277  "representation of uncertainty"));
278 
279  // If we are talking about information form matrices, the *higher* the
280  // determinant the more confident we are.
281  // if we are talking about covariances then the *lower*.
282  bool has_lower = false;
283  if (curr_pose_pdf.isInfType())
284  {
285  has_lower = this->getDeterminant() > other.getDeterminant();
286  }
287  else
288  {
289  has_lower = this->getDeterminant() < other.getDeterminant();
290  }
291 
292  return has_lower;
293 }
294 } // namespace mrpt::graphslam
const mrpt::graphs::TNodeID & getSource() const
Return the source node of this path.
Holds the data of an information path.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
typename constraint_t::type_value pose_t
STL namespace.
const mrpt::graphs::TNodeID & getDestination() const
Return the Destination node of this path.
SLAM methods related to graphs of pose constraints.
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.
constraint_t curr_pose_pdf
Current path position + corresponding covariance.
Scalar det() const
Determinant of matrix.
bool hasLowerUncertaintyThan(const self_t &other) const
Test if the current path has a lower uncertainty than the other path.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void addToPath(const mrpt::graphs::TNodeID &node, const constraint_t &edge)
add a new link in the current path.
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:191
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool operator==(const self_t &other) const
mrpt::vision::TStereoCalibResults out
bool operator!=(const self_t &other) const
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
self_t & operator+=(const self_t &other)
std::vector< mrpt::graphs::TNodeID > nodes_traversed
Nodes that the Path comprises of.
void assertIsBetweenNodeIDs(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to) const
Assert that the current path is between the given nodeIDs.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
typename GRAPH_T::constraint_t constraint_t
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
std::string getSTLContainerAsString(const T &t)
Return a STL container in std::string form.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020