9 #ifndef CONSTRAINED_POSE_NETWORK_IMPL_H
10 #define CONSTRAINED_POSE_NETWORK_IMPL_H
57 template <
class graph_t>
63 f <<
"VERTEX2 " <<
id <<
" " << p.
x() <<
" " << p.
y() <<
" " << p.
phi() << endl;
69 f <<
"VERTEX3 " <<
id <<
" " << p.
x() <<
" " << p.
y() <<
" " << p.z()<<
" " << p.
roll()<<
" " << p.
pitch()<<
" " << p.
yaw() << endl;
77 f <<
"EDGE2 " << edgeIDs.first <<
" " << edgeIDs.second <<
" " <<
87 f <<
"EDGE3 " << edgeIDs.first <<
" " << edgeIDs.second <<
" " <<
101 write_EDGE_line(edgeIDs,p,f);
107 write_EDGE_line(edgeIDs,p,f);
114 write_EDGE_line(edgeIDs,p,f);
121 write_EDGE_line(edgeIDs,p,f);
137 write_VERTEX_line(itNod->first, itNod->second, f);
141 if (it->first.first!=it->first.second)
142 write_EDGE_line( it->first, it->second, f);
156 const uint32_t version = 0;
158 out << g->nodes << g->edges << g->root;
168 std::string sStoredClassName;
169 in >> sStoredClassName;
173 uint32_t stored_version;
174 in >> stored_version;
177 switch (stored_version)
180 in >> g->nodes >> g->edges >> g->root;
195 typedef typename graph_t::constraint_t CPOSE;
197 set<string> alreadyWarnedUnknowns;
204 const bool graph_is_3D = CPOSE::is_3D();
213 map<TNodeID,TNodeID> lstEquivs;
220 const string lin = s.str();
223 if ( !(s >> key) || key.empty() )
224 THROW_EXCEPTION(
format(
"Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str() ) );
230 if (!(s>> id1 >> id2))
231 THROW_EXCEPTION(
format(
"Line %u: Can't read id1 & id2 in EQUIV line: '%s'", lineNum, lin.c_str() ) );
232 lstEquivs[std::max(id1,id2)] = std::min(id1,id2);
245 const string lin = s.str();
258 if ( !(s >> key) || key.empty() )
259 THROW_EXCEPTION(
format(
"Line %u: Can't read string for entry type in: '%s'", lineNum, lin.c_str() ) );
265 if (!(s>>
id >> p2D.
x >> p2D.
y >> p2D.
phi))
269 if (g->nodes.find(
id)!=g->nodes.end())
270 THROW_EXCEPTION(
format(
"Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum,
static_cast<unsigned int>(
id), lin.c_str() ) );
275 if (itEq!=lstEquivs.end())
id = itEq->second;
279 if (g->nodes.find(
id)==g->nodes.end())
285 else if (
strCmpI(key,
"VERTEX3") )
288 THROW_EXCEPTION(
format(
"Line %u: Try to load VERTEX3 into a 2D graph: '%s'", lineNum, lin.c_str() ) );
294 if (!(s>>
id >> p3D.
x >> p3D.
y >> p3D.
z >> p3D.
roll >> p3D.
pitch >> p3D.
yaw ))
298 if (g->nodes.find(
id)!=g->nodes.end())
299 THROW_EXCEPTION(
format(
"Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum,
static_cast<unsigned int>(
id), lin.c_str() ) );
304 if (itEq!=lstEquivs.end())
id = itEq->second;
308 if (g->nodes.find(
id)==g->nodes.end())
313 else if (
strCmpI(key,
"VERTEX_SE3:QUAT") )
316 THROW_EXCEPTION(
format(
"Line %u: Try to load VERTEX_SE3:QUAT into a 2D graph: '%s'", lineNum, lin.c_str() ) );
321 if (!(s>>
id >> p3D.
x >> p3D.
y >> p3D.
z >> p3D.
qx >> p3D.
qy >> p3D.
qz >> p3D.
qr ))
322 THROW_EXCEPTION(
format(
"Line %u: Error parsing VERTEX_SE3:QUAT line: '%s'", lineNum, lin.c_str() ) );
325 if (g->nodes.find(
id)!=g->nodes.end())
326 THROW_EXCEPTION(
format(
"Line %u: Error, duplicated verted ID %u in line: '%s'", lineNum,
static_cast<unsigned int>(
id), lin.c_str() ) );
331 if (itEq!=lstEquivs.end())
id = itEq->second;
335 if (g->nodes.find(
id)==g->nodes.end())
350 if (!(s>> from_id >> to_id ))
356 if (itEq!=lstEquivs.end()) to_id = itEq->second;
360 if (itEq!=lstEquivs.end()) from_id = itEq->second;
368 Ap_mean.
x >> Ap_mean.
y >> Ap_mean.
phi >>
369 Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(1,1) >>
370 Ap_cov_inv(2,2) >> Ap_cov_inv(0,2) >> Ap_cov_inv(1,2) ))
374 Ap_cov_inv(1,0) = Ap_cov_inv(0,1);
375 Ap_cov_inv(2,0) = Ap_cov_inv(0,2);
376 Ap_cov_inv(2,1) = Ap_cov_inv(1,2);
381 g->insertEdge(from_id, to_id, newEdge);
384 else if (
strCmpI(key,
"EDGE3") )
391 if (!(s>> from_id >> to_id ))
397 if (itEq!=lstEquivs.end()) to_id = itEq->second;
401 if (itEq!=lstEquivs.end()) from_id = itEq->second;
409 if (!(s>> Ap_mean.
x >> Ap_mean.
y >> Ap_mean.
z >> Ap_mean.
roll >> Ap_mean.
pitch >> Ap_mean.
yaw ))
414 Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(0,2) >> Ap_cov_inv(0,5) >> Ap_cov_inv(0,4) >> Ap_cov_inv(0,3) >>
415 Ap_cov_inv(1,1) >> Ap_cov_inv(1,2) >> Ap_cov_inv(1,5) >> Ap_cov_inv(1,4) >> Ap_cov_inv(1,3) >>
416 Ap_cov_inv(2,2) >> Ap_cov_inv(2,5) >> Ap_cov_inv(2,4) >> Ap_cov_inv(2,3) >>
417 Ap_cov_inv(5,5) >> Ap_cov_inv(5,4) >> Ap_cov_inv(5,3) >>
418 Ap_cov_inv(4,4) >> Ap_cov_inv(4,3) >>
422 Ap_cov_inv.unit(6,1.0);
424 if (alreadyWarnedUnknowns.find(
"MISSING_3D")==alreadyWarnedUnknowns.end())
426 alreadyWarnedUnknowns.insert(
"MISSING_3D");
427 cerr <<
"[CNetworkOfPoses::loadFromTextFile] " << fil <<
":" << lineNum <<
": Warning: Information matrix missing, assuming unity.\n";
433 for (
size_t r=1;r<6;r++)
434 for (
size_t c=0;c<r;c++)
435 Ap_cov_inv(r,c) = Ap_cov_inv(c,r);
441 g->insertEdge(from_id, to_id, newEdge);
444 else if (
strCmpI(key,
"EDGE_SE3:QUAT") )
452 if (!(s>> from_id >> to_id ))
458 if (itEq!=lstEquivs.end()) to_id = itEq->second;
462 if (itEq!=lstEquivs.end()) from_id = itEq->second;
469 if (!(s>> Ap_mean.
x >> Ap_mean.
y >> Ap_mean.
z >> Ap_mean.
qx >> Ap_mean.
qy >> Ap_mean.
qz >> Ap_mean.
qr ))
474 Ap_cov_inv(0,0) >> Ap_cov_inv(0,1) >> Ap_cov_inv(0,2) >> Ap_cov_inv(0,5) >> Ap_cov_inv(0,4) >> Ap_cov_inv(0,3) >>
475 Ap_cov_inv(1,1) >> Ap_cov_inv(1,2) >> Ap_cov_inv(1,5) >> Ap_cov_inv(1,4) >> Ap_cov_inv(1,3) >>
476 Ap_cov_inv(2,2) >> Ap_cov_inv(2,5) >> Ap_cov_inv(2,4) >> Ap_cov_inv(2,3) >>
477 Ap_cov_inv(5,5) >> Ap_cov_inv(5,4) >> Ap_cov_inv(5,3) >>
478 Ap_cov_inv(4,4) >> Ap_cov_inv(4,3) >>
482 Ap_cov_inv.unit(6,1.0);
484 if (alreadyWarnedUnknowns.find(
"MISSING_3D")==alreadyWarnedUnknowns.end())
486 alreadyWarnedUnknowns.insert(
"MISSING_3D");
487 cerr <<
"[CNetworkOfPoses::loadFromTextFile] " << fil <<
":" << lineNum <<
": Warning: Information matrix missing, assuming unity.\n";
493 for (
size_t r=1;r<6;r++)
494 for (
size_t c=0;c<r;c++)
495 Ap_cov_inv(r,c) = Ap_cov_inv(c,r);
501 g->insertEdge(from_id, to_id, newEdge);
504 else if (
strCmpI(key,
"EQUIV") )
510 if (alreadyWarnedUnknowns.find(key)==alreadyWarnedUnknowns.end())
512 alreadyWarnedUnknowns.insert(key);
513 cerr <<
"[CNetworkOfPoses::loadFromTextFile] " << fil <<
":" << lineNum <<
": Warning: unknown entry type: " << key << endl;
535 typedef map<pair<TNodeID,TNodeID>, vector<TEdgeIterator> > TListAllEdges;
536 TListAllEdges lstAllEdges;
539 for (TEdgeIterator itEd=g->edges.begin();itEd!=g->edges.end();++itEd)
542 const pair<TNodeID,TNodeID> arc_id = make_pair( std::min(itEd->first.first,itEd->first.second),std::max(itEd->first.first,itEd->first.second) );
544 vector<TEdgeIterator> &lstEdges = lstAllEdges[arc_id];
546 lstEdges.push_back(itEd);
553 const size_t N = it->second.size();
554 for (
size_t i=1;i<N;i++)
555 g->edges.erase( it->second[i] );
557 if (N>=2) nRemoved+=N-1;
576 typedef typename graph_t::constraint_t constraint_t;
578 dijkstra_t dijkstra(*g, g->root);
582 typename dijkstra_t::tree_graph_t treeView;
583 dijkstra.getTreeGraph(treeView);
586 struct VisitorComputePoses :
public dijkstra_t::tree_graph_t::Visitor
590 VisitorComputePoses(graph_t *g) : m_g(g) { }
591 virtual void OnVisitNode(
const TNodeID parent_id,
const typename dijkstra_t::tree_graph_t::Visitor::tree_t::TEdgeInfo &edge_to_child,
const size_t depth_level )
MRPT_OVERRIDE
594 const TNodeID child_id = edge_to_child.id;
598 if ( (!edge_to_child.reverse && !m_g->edges_store_inverse_poses) ||
599 ( edge_to_child.reverse && m_g->edges_store_inverse_poses)
602 m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id], edge_to_child.data->getPoseMean() );
606 m_g->nodes[child_id].composeFrom( m_g->nodes[parent_id], - edge_to_child.data->getPoseMean() );
613 g->nodes[g->root] =
typename constraint_t::type_value();
616 VisitorComputePoses myVisitor(g);
617 treeView.visitBreadthFirst(treeView.root, myVisitor);
685 bool ignoreCovariances )
690 const TNodeID from_id = itEdge->first.first;
691 const TNodeID to_id = itEdge->first.second;
696 ASSERTMSG_(itPoseFrom!=g->nodes.end(),
format(
"Node %u doesn't have a global pose in 'nodes'.",
static_cast<unsigned int>(from_id)))
697 ASSERTMSG_(itPoseTo!=g->nodes.end(),
format(
"Node %u doesn't have a global pose in 'nodes'.",
static_cast<unsigned int>(to_id)))
700 typedef typename graph_t::constraint_t constraint_t;
702 const typename constraint_t::type_value &from_mean = itPoseFrom->second;
703 const typename constraint_t::type_value &to_mean = itPoseTo->second;
706 const constraint_t &edge_delta_pose = itEdge->second;
707 const typename constraint_t::type_value &edge_delta_pose_mean = edge_delta_pose.getPoseMean();
709 if (ignoreCovariances)
713 from_plus_delta.composeFrom(from_mean, edge_delta_pose_mean);
716 return auxEuclid2Dist(from_plus_delta,to_mean);
722 constraint_t from_plus_delta = edge_delta_pose;
723 from_plus_delta.changeCoordinatesReference(from_mean);
731 err[i] = from_plus_delta.getPoseMean()[i] - to_mean[i];
734 return auxMaha2Dist(err,from_plus_delta);
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
A directed graph with the argument of the template specifying the type of the annotations in the edge...
A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position,...
A partial specialization of CArrayNumeric for double numbers.
A numeric matrix of compile-time fixed size.
A class used to store a 2D pose.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
double pitch() const
Get the PITCH angle (in radians)
double roll() const
Get the ROLL angle (in radians)
double yaw() const
Get the YAW angle (in radians)
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
CPose3D mean
The mean value.
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations)
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
double x() const
Common members of all points & poses classes.
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations)
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
CPose2D mean
The mean value.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A class for parsing text files, returning each non-empty and non-comment line, along its line number.
size_t getCurrentLineNumber() const
Return the line number of the last line returned with getNextLine.
void rewind()
Reset the read pointer to the beginning of the file.
bool getNextLine(std::string &out_str)
Reads from the file and return the next (non-comment) line, as a std::string.
const Scalar * const_iterator
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
uint64_t TNodeID
The type for node IDs in graphs of different types.
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
bool BASE_IMPEXP strCmpI(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case insensitive)
This file implements matrix/vector text and binary serialization.
#define ASSERT_EQUAL_(__A, __B)
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
#define THROW_EXCEPTION(msg)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define ASSERTMSG_(f, __ERROR_MSG)
Abstract graph and tree data structures, plus generic graph algorithms.
This base provides a set of functions for maths stuff.
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
T square(const T x)
Inline function for the square of a number.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
static void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf)
static void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf)
static void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf)
static void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf)
static void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf)
static void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf)
a helper struct with static template functions
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose2D &p, std::ofstream &f)
static void graph_of_poses_dijkstra_init(graph_t *g)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussianInf &edge, std::ofstream &f)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussianInf &edge, std::ofstream &f)
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
static double auxMaha2Dist(VEC &err, const CPosePDFGaussianInf &p)
static double auxEuclid2Dist(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose3D &edge, std::ofstream &f)
static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussian &p)
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussianInf &p)
static double auxMaha2Dist(VEC &err, const CPosePDFGaussian &p)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussian &edge, std::ofstream &f)
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
static double auxEuclid2Dist(const mrpt::poses::CPose3D &p1, const mrpt::poses::CPose3D &p2)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussian &edge, std::ofstream &f)
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose3D &p)
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose2D &p)
static double graph_edge_sqerror(const graph_t *g, const typename mrpt::graphs::CDirectedGraph< typename graph_t::constraint_t >::edges_map_t::const_iterator &itEdge, bool ignoreCovariances)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose2D &edge, std::ofstream &f)
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D &p, std::ofstream &f)
static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
double phi
Orientation (rads)
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
double roll
Roll coordinate (rotation angle over X coordinate).
double pitch
Pitch coordinate (rotation angle over Y axis).
double yaw
Yaw coordinate (rotation angle over Z axis).
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
double z
Translation in x,y,z.
double qz
Unit quaternion part, qr,qx,qy,qz.