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



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