37 template <
class POSE_PDF>
75 template <
class graph_t>
82 f <<
"VERTEX_SE2 " <<
id <<
" " << p.
x() <<
" " << p.
y() <<
" "
91 f <<
"VERTEX3 " <<
id <<
" " << p.
x() <<
" " << p.
y() <<
" " << p.z()
92 <<
" " << p.
roll() <<
" " << p.
pitch() <<
" " << p.
yaw();
101 f <<
"EDGE_SE2 " << edgeIDs.first <<
" " << edgeIDs.second <<
" "
117 f <<
"EDGE3 " << edgeIDs.first <<
" " << edgeIDs.second <<
" "
138 write_EDGE_line(edgeIDs, p, f);
146 write_EDGE_line(edgeIDs, p, f);
155 write_EDGE_line(edgeIDs, p, f);
164 write_EDGE_line(edgeIDs, p, f);
168 const graph_t* g, std::ostream& f)
171 for (
const auto& n : g->nodes)
173 write_VERTEX_line(n.first, n.second, f);
175 const auto sAnnot = n.second.retAnnotsAsString();
176 if (!sAnnot.empty()) f <<
" | " << sAnnot;
179 if (n.first == g->root) f <<
"FIX " << n.first << endl;
183 for (
const auto& e : *g)
184 if (e.first.first != e.first.second)
185 write_EDGE_line(e.first, e.second, f);
193 const graph_t* g,
const std::string& fil)
199 "Error opening file '%s' for writing", fil.c_str());
200 save_graph_of_poses_to_ostream(g, f);
210 const std::string sClassName =
215 const uint32_t version = 0;
217 out << g->nodes << g->edges << g->root;
227 const std::string sClassName =
229 std::string sStoredClassName;
230 in >> sStoredClassName;
234 uint32_t stored_version;
235 in >> stored_version;
238 switch (stored_version)
241 in >> g->nodes >> g->edges >> g->root;
252 graph_t* g, std::istream& f,
253 const std::string& fil = std::string(
"(none)"))
258 using CPOSE =
typename graph_t::constraint_t;
260 set<string> alreadyWarnedUnknowns;
269 const bool graph_is_3D = CPOSE::is_3D();
278 map<TNodeID, TNodeID> lstEquivs;
287 const string lin = s.str();
290 if (!(s >> key) || key.empty())
292 "Line %u: Can't read string for entry type in: '%s'",
293 lineNum, lin.c_str()));
299 if (!(s >> id1 >> id2))
301 "Line %u: Can't read id1 & id2 in EQUIV line: '%s'",
302 lineNum, lin.c_str()));
303 lstEquivs[std::max(id1, id2)] = std::min(id1, id2);
316 const string lin = s.str();
332 if (!(s >> key) || key.empty())
334 "Line %u: Can't read string for entry type in: '%s'",
335 lineNum, lin.c_str()));
342 if (!(s >>
id >> p2D.
x >> p2D.
y >> p2D.
phi))
344 "Line %u: Error parsing VERTEX2 line: '%s'", lineNum,
348 if (g->nodes.find(
id) != g->nodes.end())
350 "Line %u: Error, duplicated verted ID %u in line: "
352 lineNum,
static_cast<unsigned int>(
id), lin.c_str()));
356 const auto itEq = lstEquivs.find(
id);
357 if (itEq != lstEquivs.end())
id = itEq->second;
361 if (g->nodes.find(
id) == g->nodes.end())
364 CPOSE>::constraint_t::type_value;
365 pose_t& newNode = g->
nodes[id];
370 else if (
strCmpI(key,
"VERTEX3"))
374 "Line %u: Try to load VERTEX3 into a 2D graph: "
376 lineNum, lin.c_str()));
383 if (!(s >>
id >> p3D.
x >> p3D.
y >> p3D.
z >> p3D.
roll >>
386 "Line %u: Error parsing VERTEX3 line: '%s'", lineNum,
390 if (g->nodes.find(
id) != g->nodes.end())
392 "Line %u: Error, duplicated verted ID %u in line: "
394 lineNum,
static_cast<unsigned int>(
id), lin.c_str()));
398 const auto itEq = lstEquivs.find(
id);
399 if (itEq != lstEquivs.end())
id = itEq->second;
403 if (g->nodes.find(
id) == g->nodes.end())
410 else if (
strCmpI(key,
"VERTEX_SE3:QUAT"))
414 "Line %u: Try to load VERTEX_SE3:QUAT into a 2D "
416 lineNum, lin.c_str()));
421 if (!(s >>
id >> p3D.
x >> p3D.
y >> p3D.
z >> p3D.
qx >> p3D.
qy >>
424 "Line %u: Error parsing VERTEX_SE3:QUAT line: '%s'",
425 lineNum, lin.c_str()));
428 if (g->nodes.find(
id) != g->nodes.end())
430 "Line %u: Error, duplicated verted ID %u in line: "
432 lineNum,
static_cast<unsigned int>(
id), lin.c_str()));
436 const auto itEq = lstEquivs.find(
id);
437 if (itEq != lstEquivs.end())
id = itEq->second;
441 if (g->nodes.find(
id) == g->nodes.end())
460 if (!(s >> from_id >> to_id))
462 "Line %u: Error parsing EDGE2 line: '%s'", lineNum,
467 const auto itEq = lstEquivs.find(to_id);
468 if (itEq != lstEquivs.end()) to_id = itEq->second;
471 const auto itEq = lstEquivs.find(from_id);
472 if (itEq != lstEquivs.end()) from_id = itEq->second;
475 if (from_id != to_id)
480 if (!(s >> Ap_mean.
x >> Ap_mean.
y >> Ap_mean.
phi >>
481 Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >>
482 Ap_cov_inv(0, 2) >> Ap_cov_inv(1, 1) >>
483 Ap_cov_inv(1, 2) >> Ap_cov_inv(2, 2)))
485 "Line %u: Error parsing EDGE2 line: '%s'", lineNum,
489 Ap_cov_inv(1, 0) = Ap_cov_inv(0, 1);
490 Ap_cov_inv(2, 0) = Ap_cov_inv(0, 2);
491 Ap_cov_inv(2, 1) = Ap_cov_inv(1, 2);
498 g->insertEdge(from_id, to_id, newEdge);
501 else if (
strCmpI(key,
"EDGE3"))
505 "Line %u: Try to load EDGE3 into a 2D graph: '%s'",
506 lineNum, lin.c_str()));
511 if (!(s >> from_id >> to_id))
513 "Line %u: Error parsing EDGE3 line: '%s'", lineNum,
518 const auto itEq = lstEquivs.find(to_id);
519 if (itEq != lstEquivs.end()) to_id = itEq->second;
522 const auto itEq = lstEquivs.find(from_id);
523 if (itEq != lstEquivs.end()) from_id = itEq->second;
526 if (from_id != to_id)
533 if (!(s >> Ap_mean.
x >> Ap_mean.
y >> Ap_mean.
z >>
536 "Line %u: Error parsing EDGE3 line: '%s'", lineNum,
541 if (!(s >> Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >>
542 Ap_cov_inv(0, 2) >> Ap_cov_inv(0, 5) >>
543 Ap_cov_inv(0, 4) >> Ap_cov_inv(0, 3) >>
544 Ap_cov_inv(1, 1) >> Ap_cov_inv(1, 2) >>
545 Ap_cov_inv(1, 5) >> Ap_cov_inv(1, 4) >>
546 Ap_cov_inv(1, 3) >> Ap_cov_inv(2, 2) >>
547 Ap_cov_inv(2, 5) >> Ap_cov_inv(2, 4) >>
548 Ap_cov_inv(2, 3) >> Ap_cov_inv(5, 5) >>
549 Ap_cov_inv(5, 4) >> Ap_cov_inv(5, 3) >>
550 Ap_cov_inv(4, 4) >> Ap_cov_inv(4, 3) >>
556 if (alreadyWarnedUnknowns.find(
"MISSING_3D") ==
557 alreadyWarnedUnknowns.end())
559 alreadyWarnedUnknowns.insert(
"MISSING_3D");
560 cerr <<
"[CNetworkOfPoses::loadFromTextFile] "
561 << fil <<
":" << lineNum
562 <<
": Warning: Information matrix missing, "
569 for (
size_t r = 1; r < 6; r++)
570 for (
size_t c = 0; c < r; c++)
571 Ap_cov_inv(r, c) = Ap_cov_inv(c, r);
579 g->insertEdge(from_id, to_id, newEdge);
582 else if (
strCmpI(key,
"EDGE_SE3:QUAT"))
586 "Line %u: Try to load EDGE3 into a 2D graph: '%s'",
587 lineNum, lin.c_str()));
594 if (!(s >> from_id >> to_id))
596 "Line %u: Error parsing EDGE_SE3:QUAT line: '%s'",
597 lineNum, lin.c_str()));
601 const auto itEq = lstEquivs.find(to_id);
602 if (itEq != lstEquivs.end()) to_id = itEq->second;
605 const auto itEq = lstEquivs.find(from_id);
606 if (itEq != lstEquivs.end()) from_id = itEq->second;
609 if (from_id != to_id)
614 if (!(s >> Ap_mean.
x >> Ap_mean.
y >> Ap_mean.
z >>
615 Ap_mean.
qx >> Ap_mean.
qy >> Ap_mean.
qz >> Ap_mean.
qr))
617 "Line %u: Error parsing EDGE_SE3:QUAT line: "
619 lineNum, lin.c_str()));
623 if (!(s >> Ap_cov_inv(0, 0) >> Ap_cov_inv(0, 1) >>
624 Ap_cov_inv(0, 2) >> Ap_cov_inv(0, 5) >>
625 Ap_cov_inv(0, 4) >> Ap_cov_inv(0, 3) >>
626 Ap_cov_inv(1, 1) >> Ap_cov_inv(1, 2) >>
627 Ap_cov_inv(1, 5) >> Ap_cov_inv(1, 4) >>
628 Ap_cov_inv(1, 3) >> Ap_cov_inv(2, 2) >>
629 Ap_cov_inv(2, 5) >> Ap_cov_inv(2, 4) >>
630 Ap_cov_inv(2, 3) >> Ap_cov_inv(5, 5) >>
631 Ap_cov_inv(5, 4) >> Ap_cov_inv(5, 3) >>
632 Ap_cov_inv(4, 4) >> Ap_cov_inv(4, 3) >>
638 if (alreadyWarnedUnknowns.find(
"MISSING_3D") ==
639 alreadyWarnedUnknowns.end())
641 alreadyWarnedUnknowns.insert(
"MISSING_3D");
642 cerr <<
"[CNetworkOfPoses::loadFromTextFile] "
643 << fil <<
":" << lineNum
644 <<
": Warning: Information matrix missing, "
651 for (
size_t r = 1; r < 6; r++)
652 for (
size_t c = 0; c < r; c++)
653 Ap_cov_inv(r, c) = Ap_cov_inv(c, r);
662 g->insertEdge(from_id, to_id, newEdge);
665 else if (
strCmpI(key,
"EQUIV"))
674 "Line %u: Can't read id in FIX line: '%s'", lineNum,
680 if (alreadyWarnedUnknowns.find(key) ==
681 alreadyWarnedUnknowns.end())
683 alreadyWarnedUnknowns.insert(key);
684 cerr <<
"[CNetworkOfPoses::loadFromTextFile] " << fil <<
":"
685 << lineNum <<
": Warning: unknown entry type: " << key
694 graph_t* g,
const std::string& fil)
696 std::ifstream f(fil);
699 "Error opening file '%s' for reading", fil.c_str());
700 load_graph_of_poses_from_text_stream(g, f, fil);
716 using TEdgeIterator =
typename graph_t::edges_map_t::iterator;
720 using TListAllEdges =
721 map<pair<TNodeID, TNodeID>,
722 vector<TEdgeIterator>>;
725 TListAllEdges lstAllEdges;
728 for (
auto itEd = g->edges.begin(); itEd != g->edges.end(); ++itEd)
731 const pair<TNodeID, TNodeID> arc_id = make_pair(
732 std::min(itEd->first.first, itEd->first.second),
733 std::max(itEd->first.first, itEd->first.second));
735 vector<TEdgeIterator>& lstEdges = lstAllEdges[arc_id];
737 lstEdges.push_back(itEd);
742 for (
auto it = lstAllEdges.begin(); it != lstAllEdges.end(); ++it)
744 const size_t N = it->second.size();
745 for (
size_t i = 1; i < N; i++)
746 g->edges.erase(it->second[i]);
748 if (N >= 2) nRemoved += N - 1;
765 graph_t* g, std::map<TNodeID, size_t>* topological_distances =
nullptr)
773 using constraint_t =
typename graph_t::constraint_t;
776 dijkstra_t dijkstra(*g, g->root);
780 typename dijkstra_t::tree_graph_t treeView;
781 dijkstra.getTreeGraph(treeView);
784 struct VisitorComputePoses :
public dijkstra_t::tree_graph_t::Visitor
787 std::map<TNodeID, size_t>* m_topo_dists{
nullptr};
789 VisitorComputePoses(graph_t* g) : m_g(g) {}
792 const typename dijkstra_t::tree_graph_t::Visitor::tree_t::
793 TEdgeInfo& edge_to_child,
794 [[maybe_unused]]
const size_t depth_level)
override
796 const TNodeID child_id = edge_to_child.id;
799 if (m_topo_dists) (*m_topo_dists)[child_id] = depth_level;
805 if ((!edge_to_child.reverse &&
806 !m_g->edges_store_inverse_poses) ||
807 (edge_to_child.reverse && m_g->edges_store_inverse_poses))
809 m_g->nodes[child_id].composeFrom(
810 m_g->nodes[parent_id],
811 edge_to_child.data->getPoseMean());
815 m_g->nodes[child_id].composeFrom(
816 m_g->nodes[parent_id],
817 -edge_to_child.data->getPoseMean());
827 bool empty_node_annots = g->nodes.begin()->second.is_node_annots_empty;
828 map<const TNodeID, TNodeAnnotations*> nodeID_to_annots;
829 if (!empty_node_annots)
831 for (
auto poses_cit = g->nodes.begin(); poses_cit != g->nodes.end();
834 nodeID_to_annots.insert(make_pair(
835 poses_cit->first, poses_cit->second.getCopyOfAnnots()));
841 typename constraint_t::type_value();
844 VisitorComputePoses myVisitor(g);
845 myVisitor.m_topo_dists = topological_distances;
846 treeView.visitBreadthFirst(treeView.root, myVisitor);
849 if (topological_distances) (*topological_distances)[g->root] = 0;
852 if (!empty_node_annots)
854 for (
auto poses_cit = g->nodes.begin(); poses_cit != g->nodes.end();
858 nodeID_to_annots.at(poses_cit->first);
859 bool res = poses_cit->second.setAnnots(*node_annots);
863 node_annots =
nullptr;
868 "Setting annotations for nodeID \"%lu\" was "
870 static_cast<unsigned long>(poses_cit->first)));
951 typename graph_t::constraint_t>::edges_map_t::const_iterator&
953 bool ignoreCovariances)
958 const TNodeID from_id = itEdge->first.first;
959 const TNodeID to_id = itEdge->first.second;
962 auto itPoseFrom = g->nodes.find(from_id);
963 auto itPoseTo = g->nodes.find(to_id);
965 itPoseFrom != g->nodes.end(),
967 "Node %u doesn't have a global pose in 'nodes'.",
968 static_cast<unsigned int>(from_id)));
970 itPoseTo != g->nodes.end(),
972 "Node %u doesn't have a global pose in 'nodes'.",
973 static_cast<unsigned int>(to_id)));
976 using constraint_t =
typename graph_t::constraint_t;
978 const typename constraint_t::type_value& from_mean = itPoseFrom->second;
979 const typename constraint_t::type_value& to_mean = itPoseTo->second;
982 const constraint_t& edge_delta_pose = itEdge->second;
983 const typename constraint_t::type_value& edge_delta_pose_mean =
984 edge_delta_pose.getPoseMean();
986 if (ignoreCovariances)
989 typename constraint_t::type_value from_plus_delta(
991 from_plus_delta.composeFrom(from_mean, edge_delta_pose_mean);
994 return auxEuclid2Dist(from_plus_delta, to_mean);
1000 constraint_t from_plus_delta = edge_delta_pose;
1001 from_plus_delta.changeCoordinatesReference(from_mean);
1009 constraint_t::type_value::static_size>
1011 for (
size_t i = 0; i < constraint_t::type_value::static_size; i++)
1012 err[i] = from_plus_delta.getPoseMean()[i] - to_mean[i];
1015 return auxMaha2Dist(err, from_plus_delta);