Main MRPT website > C++ reference for MRPT 1.5.7
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-2016, 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 
13 // Implementattion file for TUncertaintyPath struct
14 
15 namespace mrpt { namespace graphslam {
16 
17 template<class GRAPH_T>
19  this->clear();
20 }
21 template<class GRAPH_T>
23  const mrpt::utils::TNodeID& starting_node) {
24  this->clear();
25  nodes_traversed.push_back(starting_node);
26 }
27 template<class GRAPH_T>
29  const mrpt::utils::TNodeID& starting_node,
30  const mrpt::utils::TNodeID& ending_node,
31  const constraint_t& edge) {
32  this->clear();
33  nodes_traversed.push_back(starting_node);
34  this->addToPath(ending_node, edge);
35 }
36 
37 template<class GRAPH_T>
39 
40 template<class GRAPH_T>
42  using namespace mrpt;
43  using namespace mrpt::math;
44  using namespace mrpt::poses;
45  using namespace std;
46 
47  // clear the vector of traversed nodes
48  nodes_traversed.clear();
49 
50  // clear the relative edge
51  curr_pose_pdf.mean = pose_t();
52  // by default the information matrix is set to the unit matrix
53  CMatrixDouble33 init_path_mat; init_path_mat.unit();
54  // put a really large number - we are certain of this position
55  init_path_mat *= 10000; //TODO - justify this..
56  curr_pose_pdf.cov_inv = init_path_mat;
57 
58  determinant_is_updated = false;
59  determinant_cached = 0;
60 
61 } // end of clear
62 
63 template<class GRAPH_T>
65  return *this == self_t();
66 }
67 
68 template<class GRAPH_T>
70  const mrpt::utils::TNodeID& from,
71  const mrpt::utils::TNodeID& to) const {
72  ASSERTMSG_(this->getSource() == from,
73  format("\nnodeID %lu is not the source of the path\n%s\n\n",
74  static_cast<unsigned long>(from),
75  this->getAsString().c_str()));
76  ASSERTMSG_(this->getDestination() == to,
77  format("\nnodeID %lu is not the destination of the path\n%s\n\n",
78  static_cast<unsigned long>(to),
79  this->getAsString().c_str()));
80 }
81 
82 template<class GRAPH_T>
85  const self_t& other) {
86  using namespace mrpt;
87  using namespace mrpt::math;
88  using namespace mrpt::poses;
89  using namespace std;
90 
91  // other path should start where this ends
92  ASSERTMSG_(other.nodes_traversed.begin()[0] ==
93  this->nodes_traversed.rbegin()[0],
94  "\"other\" instance must start from the nodeID that this "
95  "TUncertaintyPath has ended.");
96  ASSERTMSG_(other.nodes_traversed.size(),
97  "\"other\" instance doesn't have an initialized list of traversed nodes");
98  ASSERTMSG_(this->nodes_traversed.size(),
99  "\"this\" instance doesn't have an initialized list of traversed nodes");
100 
101  //////// TODO Remove these - >>>>>>>>>>>>>>>>>>>>>
102  //cout << string(20, '-') << "Aggregating 2 paths.."
103  //<< string(20, '-') << endl;
104  //this->dumpToConsole(); other.dumpToConsole();
105  ////// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<
106 
107  // aggregate the two gaussian - mean & information matrix
108  this->curr_pose_pdf += other.curr_pose_pdf;
109 
110  // add the traversed nodes
111  this->nodes_traversed.insert(
112  this->nodes_traversed.end(),
113  other.nodes_traversed.begin()+1,
114  other.nodes_traversed.end());
115 
116  ////// TODO Remove these - >>>>>>>>>>>>>>>>>>>>>
117  //cout << std::string(10, '%') << endl << "AFTER Aggregation..." << endl;
118  //this->dumpToConsole();
119  //cout << string(50, '-') << endl;
120  //mrpt::system::pause();
121  ////// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<
122 
123  determinant_is_updated = false;
124  return *this;
125 
126 }
127 template<class GRAPH_T>
129  const self_t& other) const {
130  using namespace mrpt;
131  using namespace mrpt::math;
132  using namespace mrpt::poses;
133  using namespace std;
134 
135  // check if the traversed nodes are the same as well as the
136  // CPoseGaussianInfs are the same..
137  return (
138  this->nodes_traversed == other.nodes_traversed &&
139  this->curr_pose_pdf == other.curr_pose_pdf);
140 
141 }
142 template<class GRAPH_T>
144  const self_t& other) const {
145 
146  return !(*this == other);
147 }
148 
149 template<class GRAPH_T>
151  const mrpt::utils::TNodeID& node,
152  const constraint_t& edge) {
153 
154  // update the path
155  curr_pose_pdf += edge;
156 
157  // update the traversed nodes
158  nodes_traversed.push_back(node);
159 
160  determinant_is_updated = false;
161 }
162 
163 template<class GRAPH_T>
166  const std::string &section) {}
167 
168 template<class GRAPH_T>
170  mrpt::utils::CStream &out) const {
171 
172  out.printf("%s\n", this->getAsString().c_str());
173 
174 }
175 
176 template<class GRAPH_T>
178  using namespace mrpt;
179  using namespace mrpt::poses;
180  using namespace std;
181  using namespace mrpt::utils;
182  using namespace mrpt::math;
183 
184  stringstream ss;
185  string header_sep(30, '=');
186 
187  ss << "Path properties: " << endl;
188  ss << header_sep << endl << endl;
189 
190  ss << "- CPosePDFGaussianInf: "
191  << (curr_pose_pdf.isInfType()? "TRUE" : "FALSE") << endl;
192  ss << "- Nodes list: \n\t< " <<
193  getSTLContainerAsString(nodes_traversed)
194  << "\b\b>" << endl;
195 
196  ss << endl;
197  ss << curr_pose_pdf << endl;
198  ss << endl;
199 
200  CMatrixDouble33 mat;
201  if (curr_pose_pdf.isInfType()) {
202  curr_pose_pdf.getInformationMatrix(mat);
203  }
204  else {
205  curr_pose_pdf.getCovariance(mat);
206  }
207  ss << "Determinant: " << mat.det();
208 
209  *str = ss.str();
210 }
211 template<class GRAPH_T>
213  std::string s;
214  this->getAsString(&s);
215  return s;
216 }
217 
218 template<class GRAPH_T>
221  return nodes_traversed.at(0);
222 }
223 template<class GRAPH_T>
226  return nodes_traversed.back();
227 }
228 
229 template<class GRAPH_T>
231  using namespace mrpt;
232  using namespace mrpt::math;
233  using namespace mrpt::poses;
234  using namespace std;
235 
236  // if determinant is up-to-date then return the cached version...
237  if (determinant_is_updated) return determinant_cached;
238 
239  // update the cached version and return it.
240  CMatrixDouble33 mat;
241  if (curr_pose_pdf.isInfType()) {
242  curr_pose_pdf.getInformationMatrix(mat);
243  }
244  else {
245  curr_pose_pdf.getCovariance(mat);
246  }
247  double determinant = mat.det();
248 
249  determinant_cached = determinant;
250  determinant_is_updated = true;
251 
252 
253  return determinant;
254 
255 }
256 
257 template<class GRAPH_T>
259  const self_t& other) const {
260  using namespace mrpt;
261  using namespace mrpt::math;
262  using namespace mrpt::poses;
263  using namespace std;
264 
265  ASSERTMSG_(
266  (curr_pose_pdf.isInfType() && other.curr_pose_pdf.isInfType()) ||
267  (!curr_pose_pdf.isInfType() && !other.curr_pose_pdf.isInfType()),
268  mrpt::format(
269  "Constraints of given paths don't have the same representation of uncertainty"));
270 
271  // If we are talking about information form matrices, the *higher* the
272  // determinant the more confident we are.
273  // if we are talking about covariances then the *lower*.
274  bool has_lower = false;
275  if (curr_pose_pdf.isInfType()) {
276  has_lower = this->getDeterminant() > other.getDeterminant();
277  }
278  else {
279  has_lower = this->getDeterminant() < other.getDeterminant();
280  }
281 
282  return has_lower;
283 }
284 
285 } } // end of namespaces
286 
287 #endif /* end of include guard: TUNCERTAINTYPATH_IMPL_H */
std::vector< mrpt::utils::TNodeID > nodes_traversed
Nodes that the Path comprises of.
Holds the data of an information path.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
void addToPath(const mrpt::utils::TNodeID &node, const constraint_t &edge)
add a new link in the current path.
std::string getSTLContainerAsString(const T &t)
Return a STL container in std::string form.
STL namespace.
GLdouble s
Definition: glext.h:3602
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
This class allows loading and storing values and vectors of different types from a configuration text...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
uint64_t TNodeID
The type for node IDs in graphs of different types.
constraint_t curr_pose_pdf
Current path position + corresponding covariance.
constraint_t::type_value pose_t
type of underlying poses (2D/3D).
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei const GLchar ** string
Definition: glext.h:3919
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...
Definition: CPoint.h:17
void assertIsBetweenNodeIDs(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to) const
Assert that the current path is between the given nodeIDs.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void dumpToTextStream(mrpt::utils::CStream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
bool operator==(const self_t &other) const
bool operator!=(const self_t &other) const
const mrpt::utils::TNodeID & getSource() const
Return the source node of this path.
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
self_t & operator+=(const self_t &other)
GRAPH_T::constraint_t constraint_t
Handy typedefs.
#define ASSERTMSG_(f, __ERROR_MSG)
const mrpt::utils::TNodeID & getDestination() const
Return the Destination node of this path.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019