38 #include <type_traits>
47 template <
class GRAPH_T>
53 class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
class EDGE_ANNOTATIONS>
58 class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
class EDGE_ANNOTATIONS>
116 class MAPS_IMPLEMENTATION =
131 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
151 return literal(
"mrpt::graphs::CNetworkOfPoses<") +
164 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
170 return literal(
"global_pose_t<") +
180 template <
typename ARG1>
184 template <
typename ARG1,
typename ARG2>
196 static_cast<const NODE_ANNOTATIONS
>(*
this) ==
197 static_cast<const NODE_ANNOTATIONS
>(other));
201 return (!(*
this == other));
205 std::ostream& o,
const self_t& global_pose)
207 o << global_pose.asString() <<
"| "
208 << global_pose.retAnnotsAsString();
290 const std::string& fileName,
bool collapse_dup_edges =
true)
323 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
325 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
327 bool is_multirobot =
false;
328 std::unique_ptr<visualizer_t> viz;
334 viz.reset(
new visualizer_multirobot_t(*
this));
338 viz.reset(
new visualizer_t(*
this));
340 viz->getAs3DObject(
object, viz_params);
356 std::optional<std::reference_wrapper<std::map<TNodeID, size_t>>>
357 topological_distances = std::nullopt)
360 this, topological_distances ? &topological_distances.value().get()
391 this, it, ignoreCovariances);
413 const std::set<TNodeID>& node_IDs,
self_t* sub_graph,
415 bool auto_expand_set =
true)
const
418 using namespace mrpt;
427 "\nInvalid pointer to a CNetworkOfPoses instance is given. "
432 TNodeID root_node = root_node_in;
436 node_IDs.find(root_node) != node_IDs.end(),
437 "\nRoot_node does not exist in the given node_IDs set. "
443 node_IDs.size() >= 2,
445 "Very few nodes [%lu] for which to extract a subgraph. "
447 static_cast<unsigned long>(node_IDs.size())));
451 bool is_fully_connected_graph =
true;
452 std::set<TNodeID> node_IDs_real;
453 if (*node_IDs.rbegin() - *node_IDs.begin() + 1 == node_IDs.size())
455 node_IDs_real = node_IDs;
459 is_fully_connected_graph =
false;
463 for (
TNodeID curr_node_ID = *node_IDs.begin();
464 curr_node_ID != *node_IDs.rbegin(); ++curr_node_ID)
466 node_IDs_real.insert(curr_node_ID);
471 node_IDs_real = node_IDs;
476 for (
unsigned long node_IDs_it : node_IDs_real)
479 typename global_poses_t::const_iterator own_it;
480 for (own_it =
nodes.begin(); own_it !=
nodes.end(); ++own_it)
482 if (node_IDs_it == own_it->first)
488 own_it !=
nodes.end(),
490 "NodeID [%lu] can't be found in the initial graph.",
491 static_cast<unsigned long>(node_IDs_it)));
494 sub_graph->
nodes.insert(make_pair(node_IDs_it, curr_node));
503 root_node = sub_graph->
nodes.begin()->first;
505 sub_graph->
root = root_node;
512 const TNodeID& from = e.first.first;
513 const TNodeID& to = e.first.second;
517 if (sub_graph->
nodes.find(from) != sub_graph->
nodes.end() &&
518 sub_graph->
nodes.find(to) != sub_graph->
nodes.end())
524 if (!auto_expand_set && !is_fully_connected_graph)
531 std::set<TNodeID> root_neighbors;
534 if (root_neighbors.empty())
537 typename global_poses_t::iterator root_it =
540 if ((*root_it == *sub_graph->
nodes.rbegin()))
543 TNodeID next_to_root = (--root_it)->first;
545 sub_graph, next_to_root, sub_graph->
root);
550 TNodeID next_to_root = (++root_it)->first;
553 sub_graph, sub_graph->
root, next_to_root);
560 bool dijkstra_runs_successfully =
false;
564 while (!dijkstra_runs_successfully)
568 dijkstra_t dijkstra(*sub_graph, sub_graph->
root);
569 dijkstra_runs_successfully =
true;
573 dijkstra_runs_successfully =
false;
575 set<TNodeID> unconnected_nodeIDs;
588 const TNodeID& island_highest =
589 *unconnected_nodeIDs.rbegin();
590 const TNodeID& island_lowest = *unconnected_nodeIDs.begin();
598 std::set<TNodeID> mainland;
600 for (
typename global_poses_t::const_iterator n_it =
601 sub_graph->
nodes.begin();
602 n_it != sub_graph->
nodes.end(); ++n_it)
604 bool is_there =
false;
607 for (
unsigned long unconnected_nodeID :
610 if (n_it->first == unconnected_nodeID)
619 mainland.insert(n_it->first);
623 bool is_single_island =
624 (island_highest - island_lowest + 1 ==
625 unconnected_nodeIDs.size());
627 if (is_single_island)
641 const std::set<TNodeID>& island = unconnected_nodeIDs;
643 sub_graph, island, mainland);
654 std::vector<std::set<TNodeID>> vec_of_islands;
655 std::set<TNodeID> curr_island;
656 TNodeID prev_nodeID = *unconnected_nodeIDs.begin();
659 for (
auto it = ++unconnected_nodeIDs.begin();
660 *it < sub_graph->
root &&
661 it != unconnected_nodeIDs.end();
664 if (!(
absDiff(*it, prev_nodeID) == 1))
666 vec_of_islands.push_back(curr_island);
669 curr_island.insert(*it);
674 vec_of_islands.push_back(curr_island);
682 sub_graph, vec_of_islands.back(), mainland);
713 const bool hypots_from_other_to_self =
true,
714 std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings_out =
nullptr)
723 using nodes_cit_t =
typename global_poses_t::const_iterator;
725 const self_t& graph_from = (hypots_from_other_to_self ? other : *
this);
726 const self_t& graph_to = (hypots_from_other_to_self ? *this : other);
734 for (hypots_cit_t h_cit = common_hypots.begin();
735 h_cit != common_hypots.end(); ++h_cit)
738 graph_from.
nodes.find(h_cit->from) != graph_from.
nodes.end(),
739 format(
"NodeID %lu is not found in (from) graph", h_cit->from));
741 graph_to.
nodes.find(h_cit->to) != graph_to.
nodes.end(),
742 format(
"NodeID %lu is not found in (to) graph", h_cit->to));
747 for (nodes_cit_t n_cit = this->nodes.begin();
748 n_cit != this->nodes.end(); ++n_cit)
750 if (n_cit->first > max_nodeID)
752 max_nodeID = n_cit->first;
755 TNodeID renum_start = max_nodeID + 1;
756 size_t renum_counter = 0;
761 std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings;
767 std::map<TNodeID, TNodeID> mappings_tmp;
770 if (old_to_new_nodeID_mappings_out)
772 old_to_new_nodeID_mappings = old_to_new_nodeID_mappings_out;
776 old_to_new_nodeID_mappings = &mappings_tmp;
778 old_to_new_nodeID_mappings->clear();
783 for (nodes_cit_t n_cit = other.
nodes.begin();
784 n_cit != other.
nodes.end(); ++n_cit)
786 TNodeID new_nodeID = renum_start + renum_counter++;
787 old_to_new_nodeID_mappings->insert(
788 make_pair(n_cit->first, new_nodeID));
789 this->nodes.insert(make_pair(new_nodeID, n_cit->second));
797 for (hypots_cit_t h_cit = common_hypots.begin();
798 h_cit != common_hypots.end(); ++h_cit)
801 if (hypots_from_other_to_self)
803 from = old_to_new_nodeID_mappings->at(h_cit->from);
809 to = old_to_new_nodeID_mappings->at(h_cit->to);
820 g_cit != other.
end(); ++g_cit)
823 old_to_new_nodeID_mappings->at(g_cit->first.first);
825 old_to_new_nodeID_mappings->at(g_cit->first.second);
826 this->
insertEdge(new_from, new_to, g_cit->second);
848 self_t* sub_graph,
const std::set<TNodeID>& groupA,
849 const std::set<TNodeID>& groupB)
855 "\nInvalid pointer to a CNetworkOfPoses instance is given. "
857 ASSERTMSG_(!groupA.empty(),
"\ngroupA is empty.");
858 ASSERTMSG_(!groupB.empty(),
"\ngroupB is empty.");
862 *groupA.rend() < *groupB.rbegin() ||
863 *groupA.rbegin() > *groupB.rend(),
864 "Groups A, B contain overlapping nodeIDs");
868 const std::set<TNodeID>& low_nodeIDs =
869 *groupA.rbegin() < *groupB.rbegin() ? groupA : groupB;
870 const std::set<TNodeID>& high_nodeIDs =
871 *groupA.rbegin() > *groupB.rbegin() ? groupA : groupB;
874 const TNodeID& from_nodeID = *low_nodeIDs.rbegin();
875 const TNodeID& to_nodeID = *high_nodeIDs.begin();
888 bool ignoreCovariances =
true)
const
891 this, itEdge, ignoreCovariances);
904 bool ignoreCovariances =
true)
const
911 "Request for edge %u->%u that doesn't exist in graph.",
912 static_cast<unsigned int>(from_id),
913 static_cast<unsigned int>(to_id)));
944 graph,
"Invalid pointer to the graph instance was provided.");
956 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
957 class EDGE_ANNOTATIONS>
961 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>& obj)
964 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
971 class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
972 class EDGE_ANNOTATIONS>
976 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>& obj)
979 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;