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);
#define ASSERT_EQUAL_(__A, __B)
A directed graph with the argument of the template specifying the type of the annotations in the edge...
static double auxMaha2Dist(VEC &err, const CPosePDFGaussian &p)
double x() const
Common members of all points & poses classes.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
A template to obtain the type of its argument as a string at compile time.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
bool getNextLine(std::string &out_str)
Reads from the file and return the next (non-comment) line, as a std::string.
double roll
Roll coordinate (rotation angle over X coordinate).
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose2D &p)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define THROW_EXCEPTION(msg)
Abstract graph and tree data structures, plus generic graph algorithms.
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
double pitch() const
Get the PITCH angle (in radians)
double yaw() const
Get the YAW angle (in radians)
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
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)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussianInf &edge, std::ofstream &f)
const Scalar * const_iterator
double yaw
Yaw coordinate (rotation angle over Z axis).
static double auxMaha2Dist(VEC &err, const CPosePDFGaussianInf &p)
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPose3DPDFGaussian &edge, std::ofstream &f)
static double auxEuclid2Dist(const mrpt::poses::CPose2D &p1, const mrpt::poses::CPose2D &p2)
static double auxEuclid2Dist(const mrpt::poses::CPose3D &p1, const mrpt::poses::CPose3D &p2)
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
static void graph_of_poses_dijkstra_init(graph_t *g)
A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...
void rewind()
Reset the read pointer to the beginning of the file.
static void copyFrom2D(CPose2D &p, const CPosePDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussian &edge, std::ofstream &f)
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
uint64_t TNodeID
The type for node IDs in graphs of different types.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose3D &edge, std::ofstream &f)
static void copyFrom2D(POSE_PDF &p, const CPosePDFGaussianInf &pdf)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
static void copyFrom2D(CPose3D &p, const CPosePDFGaussianInf &pdf)
double z
Translation in x,y,z.
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)
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussian &p)
CPose3D mean
The mean value.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
A class for parsing text files, returning each non-empty and non-comment line, along its line number.
double qz
Unit quaternion part, qr,qx,qy,qz.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double roll() const
Get the ROLL angle (in radians)
void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations)
double pitch
Pitch coordinate (rotation angle over Y axis).
Lightweight 3D pose (three spatial coordinates, plus a quaternion ).
static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
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.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
CPose2D mean
The mean value.
A class used to store a 2D pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
This file implements matrix/vector text and binary serialization.
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)
const double & phi() const
Get the phi angle of the 2D pose (in radians)
static void copyFrom3D(CPose3D &p, const CPose3DPDFGaussianInf &pdf)
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
A partial specialization of CArrayNumeric for double numbers.
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
size_t getCurrentLineNumber() const
Return the line number of the last line returned with getNextLine.
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position,...
static double auxMaha2Dist(VEC &err, const CPose3DPDFGaussianInf &p)
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations)
static void copyFrom3D(CPose2D &p, const CPose3DPDFGaussianInf &pdf)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const mrpt::poses::CPose2D &edge, std::ofstream &f)
static void copyFrom3D(POSE_PDF &p, const CPose3DPDFGaussianInf &pdf)
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose2D &p, std::ofstream &f)
#define ASSERTMSG_(f, __ERROR_MSG)
#define THROW_EXCEPTION_CUSTOM_MSG1(msg, param1)
double phi
Orientation (rads)
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
static void write_EDGE_line(const TPairNodeIDs &edgeIDs, const CPosePDFGaussianInf &edge, std::ofstream &f)
static void write_VERTEX_line(const TNodeID id, const mrpt::poses::CPose3D &p, std::ofstream &f)
static double auxMaha2Dist(VEC &err, const mrpt::poses::CPose3D &p)