18 template <
class GRAPH_T>
23 template <
class GRAPH_T>
28 nodes_traversed.push_back(starting_node);
30 template <
class GRAPH_T>
36 nodes_traversed.push_back(starting_node);
37 this->addToPath(ending_node, edge);
40 template <
class GRAPH_T>
49 nodes_traversed.clear();
52 curr_pose_pdf.mean =
pose_t();
54 auto init_path_mat = CMatrixDouble33::Identity();
57 init_path_mat *= 10000;
58 curr_pose_pdf.cov_inv = init_path_mat;
60 determinant_is_updated =
false;
61 determinant_cached = 0;
65 template <
class GRAPH_T>
71 template <
class GRAPH_T>
76 this->getSource() == from,
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,
83 "\nnodeID %lu is not the destination of the path\n%s\n\n",
84 static_cast<unsigned long>(to), this->getAsString().c_str()));
87 template <
class GRAPH_T>
99 "\"other\" instance must start from the nodeID that this "
100 "TUncertaintyPath has ended.");
103 "\"other\" instance doesn't have an initialized list of traversed "
106 this->nodes_traversed.size(),
107 "\"this\" instance doesn't have an initialized list of traversed "
120 this->nodes_traversed.insert(
131 determinant_is_updated =
false;
134 template <
class GRAPH_T>
137 using namespace mrpt;
148 template <
class GRAPH_T>
151 return !(*
this == other);
154 template <
class GRAPH_T>
159 curr_pose_pdf += edge;
162 nodes_traversed.push_back(node);
164 determinant_is_updated =
false;
167 template <
class GRAPH_T>
173 template <
class GRAPH_T>
179 template <
class GRAPH_T>
182 using namespace mrpt;
189 string header_sep(30,
'=');
191 ss <<
"Path properties: " << endl;
192 ss << header_sep << endl << endl;
194 ss <<
"- CPosePDFGaussianInf: "
195 << (curr_pose_pdf.isInfType() ?
"TRUE" :
"FALSE") << endl;
200 ss << curr_pose_pdf << endl;
204 if (curr_pose_pdf.isInfType())
206 curr_pose_pdf.getInformationMatrix(mat);
210 curr_pose_pdf.getCovariance(mat);
212 ss <<
"Determinant: " << mat.
det();
216 template <
class GRAPH_T>
220 this->getAsString(&s);
224 template <
class GRAPH_T>
227 return nodes_traversed.at(0);
229 template <
class GRAPH_T>
232 return nodes_traversed.back();
235 template <
class GRAPH_T>
238 using namespace mrpt;
244 if (determinant_is_updated)
return determinant_cached;
248 if (curr_pose_pdf.isInfType())
250 curr_pose_pdf.getInformationMatrix(mat);
254 curr_pose_pdf.getCovariance(mat);
256 double determinant = mat.
det();
258 determinant_cached = determinant;
259 determinant_is_updated =
true;
264 template <
class GRAPH_T>
266 const self_t& other)
const
268 using namespace mrpt;
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"));
282 bool has_lower =
false;
283 if (curr_pose_pdf.isInfType())