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);