10 #ifndef TUNCERTAINTYPATH_IMPL_H 11 #define TUNCERTAINTYPATH_IMPL_H 19 template <
class GRAPH_T>
24 template <
class GRAPH_T>
29 nodes_traversed.push_back(starting_node);
31 template <
class GRAPH_T>
37 nodes_traversed.push_back(starting_node);
38 this->addToPath(ending_node, edge);
41 template <
class GRAPH_T>
46 template <
class GRAPH_T>
55 nodes_traversed.clear();
58 curr_pose_pdf.mean =
pose_t();
63 init_path_mat *= 10000;
64 curr_pose_pdf.cov_inv = init_path_mat;
66 determinant_is_updated =
false;
67 determinant_cached = 0;
71 template <
class GRAPH_T>
77 template <
class GRAPH_T>
82 this->getSource() == from,
84 "\nnodeID %lu is not the source of the path\n%s\n\n",
85 static_cast<unsigned long>(from), this->getAsString().c_str()));
87 this->getDestination() == to,
89 "\nnodeID %lu is not the destination of the path\n%s\n\n",
90 static_cast<unsigned long>(to), this->getAsString().c_str()));
93 template <
class GRAPH_T>
105 "\"other\" instance must start from the nodeID that this " 106 "TUncertaintyPath has ended.");
109 "\"other\" instance doesn't have an initialized list of traversed " 112 this->nodes_traversed.size(),
113 "\"this\" instance doesn't have an initialized list of traversed " 126 this->nodes_traversed.insert(
137 determinant_is_updated =
false;
140 template <
class GRAPH_T>
143 using namespace mrpt;
154 template <
class GRAPH_T>
157 return !(*
this == other);
160 template <
class GRAPH_T>
165 curr_pose_pdf += edge;
168 nodes_traversed.push_back(node);
170 determinant_is_updated =
false;
173 template <
class GRAPH_T>
179 template <
class GRAPH_T>
182 out <<
mrpt::format(
"%s\n", this->getAsString().c_str());
185 template <
class GRAPH_T>
188 using namespace mrpt;
195 string header_sep(30,
'=');
197 ss <<
"Path properties: " << endl;
198 ss << header_sep << endl << endl;
200 ss <<
"- CPosePDFGaussianInf: " 201 << (curr_pose_pdf.isInfType() ?
"TRUE" :
"FALSE") << endl;
206 ss << curr_pose_pdf << endl;
210 if (curr_pose_pdf.isInfType())
212 curr_pose_pdf.getInformationMatrix(mat);
216 curr_pose_pdf.getCovariance(mat);
218 ss <<
"Determinant: " << mat.det();
222 template <
class GRAPH_T>
226 this->getAsString(&
s);
230 template <
class GRAPH_T>
233 return nodes_traversed.at(0);
235 template <
class GRAPH_T>
238 return nodes_traversed.back();
241 template <
class GRAPH_T>
244 using namespace mrpt;
250 if (determinant_is_updated)
return determinant_cached;
254 if (curr_pose_pdf.isInfType())
256 curr_pose_pdf.getInformationMatrix(mat);
260 curr_pose_pdf.getCovariance(mat);
262 double determinant = mat.det();
264 determinant_cached = determinant;
265 determinant_is_updated =
true;
270 template <
class GRAPH_T>
272 const self_t& other)
const 274 using namespace mrpt;
280 (curr_pose_pdf.isInfType() && other.
curr_pose_pdf.isInfType()) ||
281 (!curr_pose_pdf.isInfType() && !other.
curr_pose_pdf.isInfType()),
283 "Constraints of given paths don't have the same " 284 "representation of uncertainty"));
289 bool has_lower =
false;
290 if (curr_pose_pdf.isInfType())
const mrpt::graphs::TNodeID & getSource() const
Return the source node of this path.
std::string getAsString() const
Holds the data of an information path.
typename constraint_t::type_value pose_t
type of underlying poses (2D/3D).
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.
GLsizei const GLchar ** string
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)
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
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
GLsizei GLsizei GLchar * source
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ion)
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.
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.
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.