9 #ifndef CONSTRAINED_POSE_NETWORK_H 10 #define CONSTRAINED_POSE_NETWORK_H 38 #include <type_traits> 48 template <
class GRAPH_T>
52 template <
class CPOSE,
53 class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
54 class EDGE_ANNOTATIONS>
57 template <
class CPOSE,
58 class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
59 class EDGE_ANNOTATIONS>
117 class MAPS_IMPLEMENTATION =
163 template <
typename ARG1>
167 template <
typename ARG1,
typename ARG2>
185 static_cast<const constraint_no_pdf_t>(*
this) ==
186 static_cast<const constraint_no_pdf_t>(other) &&
187 static_cast<const NODE_ANNOTATIONS>(*
this) ==
188 static_cast<const NODE_ANNOTATIONS>(other));
192 return (!(*
this == other));
196 std::ostream& o,
const self_t& global_pose)
198 o << global_pose.asString() <<
"| " 199 << global_pose.retAnnotsAsString();
207 typename MAPS_IMPLEMENTATION::template map<mrpt::utils::TNodeID, CPOSE>
280 const std::string& fileName,
bool collapse_dup_edges =
true)
302 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
304 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
306 bool is_multirobot =
false;
307 std::unique_ptr<visualizer_t> viz;
313 viz.reset(
new visualizer_multirobot_t(*
this));
317 viz.reset(
new visualizer_t(*
this));
319 viz->getAs3DObject(
object, viz_params);
366 itEdge != last_it; ++itEdge)
368 this, itEdge, ignoreCovariances);
390 const std::set<TNodeID>& node_IDs,
self_t* sub_graph,
392 const bool& auto_expand_set =
true)
const 395 using namespace mrpt;
404 "\nInvalid pointer to a CNetworkOfPoses instance is given. " 409 TNodeID root_node = root_node_in;
413 node_IDs.find(root_node) != node_IDs.end(),
414 "\nRoot_node does not exist in the given node_IDs set. " 420 node_IDs.size() >= 2,
422 "Very few nodes [%lu] for which to extract a subgraph. " 424 static_cast<unsigned long>(node_IDs.size())));
428 bool is_fully_connected_graph =
true;
429 std::set<TNodeID> node_IDs_real;
430 if (*node_IDs.rbegin() - *node_IDs.begin() + 1 == node_IDs.size())
432 node_IDs_real = node_IDs;
436 is_fully_connected_graph =
false;
440 for (
TNodeID curr_node_ID = *node_IDs.begin();
441 curr_node_ID != *node_IDs.rbegin(); ++curr_node_ID)
443 node_IDs_real.insert(curr_node_ID);
448 node_IDs_real = node_IDs;
454 node_IDs_real.begin();
455 node_IDs_it != node_IDs_real.end(); ++node_IDs_it)
459 for (own_it =
nodes.begin(); own_it !=
nodes.end(); ++own_it)
461 if (*node_IDs_it == own_it->first)
467 own_it !=
nodes.end(),
469 "NodeID [%lu] can't be found in the initial graph.",
470 static_cast<unsigned long>(*node_IDs_it)));
473 sub_graph->
nodes.insert(make_pair(*node_IDs_it, curr_node));
484 root_node = sub_graph->
nodes.begin()->first;
486 sub_graph->
root = root_node;
498 const TNodeID& from = it->first.first;
499 const TNodeID& to = it->first.second;
500 const typename BASE::edge_t& curr_edge = it->second;
503 if (sub_graph->
nodes.find(from) != sub_graph->
nodes.end() &&
504 sub_graph->
nodes.find(to) != sub_graph->
nodes.end())
510 if (!auto_expand_set && !is_fully_connected_graph)
517 std::set<TNodeID> root_neighbors;
520 if (root_neighbors.empty())
526 if ((*root_it == *sub_graph->
nodes.rbegin()))
531 sub_graph, next_to_root, sub_graph->
root);
539 sub_graph, sub_graph->
root, next_to_root);
546 bool dijkstra_runs_successfully =
false;
550 while (!dijkstra_runs_successfully)
554 dijkstra_t dijkstra(*sub_graph, sub_graph->
root);
555 dijkstra_runs_successfully =
true;
559 dijkstra_runs_successfully =
false;
561 set<TNodeID> unconnected_nodeIDs;
574 const TNodeID& island_highest =
575 *unconnected_nodeIDs.rbegin();
576 const TNodeID& island_lowest = *unconnected_nodeIDs.begin();
584 std::set<TNodeID> mainland;
587 sub_graph->
nodes.begin();
588 n_it != sub_graph->
nodes.end(); ++n_it)
590 bool is_there =
false;
594 uncon_it = unconnected_nodeIDs.begin();
595 uncon_it != unconnected_nodeIDs.end(); ++uncon_it)
597 if (n_it->first == *uncon_it)
606 mainland.insert(n_it->first);
610 bool is_single_island =
611 (island_highest - island_lowest + 1 ==
612 unconnected_nodeIDs.size());
614 if (is_single_island)
628 const std::set<TNodeID>& island = unconnected_nodeIDs;
630 sub_graph, island, mainland);
641 std::vector<std::set<TNodeID>> vec_of_islands;
642 std::set<TNodeID> curr_island;
643 TNodeID prev_nodeID = *unconnected_nodeIDs.begin();
647 ++unconnected_nodeIDs.begin();
648 *it < sub_graph->
root &&
649 it != unconnected_nodeIDs.end();
652 if (!(
absDiff(*it, prev_nodeID) == 1))
654 vec_of_islands.push_back(curr_island);
657 curr_island.insert(*it);
662 vec_of_islands.push_back(curr_island);
670 sub_graph, vec_of_islands.back(), mainland);
701 const bool hypots_from_other_to_self =
true,
702 std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings_out = NULL)
714 const self_t& graph_from = (hypots_from_other_to_self ? other : *
this);
715 const self_t& graph_to = (hypots_from_other_to_self ? *this : other);
723 for (hypots_cit_t h_cit = common_hypots.begin();
724 h_cit != common_hypots.end(); ++h_cit)
727 graph_from.
nodes.find(h_cit->from) != graph_from.
nodes.end(),
728 format(
"NodeID %lu is not found in (from) graph", h_cit->from))
730 graph_to.
nodes.find(h_cit->to) != graph_to.
nodes.end(),
731 format(
"NodeID %lu is not found in (to) graph", h_cit->to))
736 for (nodes_cit_t n_cit = this->nodes.begin();
737 n_cit != this->nodes.end(); ++n_cit)
739 if (n_cit->first > max_nodeID)
741 max_nodeID = n_cit->first;
744 TNodeID renum_start = max_nodeID + 1;
745 size_t renum_counter = 0;
750 std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings;
756 std::map<TNodeID, TNodeID> mappings_tmp;
759 if (old_to_new_nodeID_mappings_out)
761 old_to_new_nodeID_mappings = old_to_new_nodeID_mappings_out;
765 old_to_new_nodeID_mappings = &mappings_tmp;
767 old_to_new_nodeID_mappings->clear();
772 for (nodes_cit_t n_cit = other.
nodes.begin();
773 n_cit != other.
nodes.end(); ++n_cit)
775 TNodeID new_nodeID = renum_start + renum_counter++;
776 old_to_new_nodeID_mappings->insert(
777 make_pair(n_cit->first, new_nodeID));
778 this->nodes.insert(make_pair(new_nodeID, n_cit->second));
786 for (hypots_cit_t h_cit = common_hypots.begin();
787 h_cit != common_hypots.end(); ++h_cit)
790 if (hypots_from_other_to_self)
792 from = old_to_new_nodeID_mappings->at(h_cit->from);
798 to = old_to_new_nodeID_mappings->at(h_cit->to);
809 g_cit != other.
end(); ++g_cit)
812 old_to_new_nodeID_mappings->at(g_cit->first.first);
814 old_to_new_nodeID_mappings->at(g_cit->first.second);
815 this->
insertEdge(new_from, new_to, g_cit->second);
837 self_t* sub_graph,
const std::set<TNodeID>& groupA,
838 const std::set<TNodeID>& groupB)
844 "\nInvalid pointer to a CNetworkOfPoses instance is given. " 846 ASSERTMSG_(!groupA.empty(),
"\ngroupA is empty.");
847 ASSERTMSG_(!groupB.empty(),
"\ngroupB is empty.");
851 *groupA.rend() < *groupB.rbegin() ||
852 *groupA.rbegin() > *groupB.rend(),
853 "Groups A, B contain overlapping nodeIDs");
857 const std::set<TNodeID>& low_nodeIDs =
858 *groupA.rbegin() < *groupB.rbegin() ? groupA : groupB;
859 const std::set<TNodeID>& high_nodeIDs =
860 *groupA.rbegin() > *groupB.rbegin() ? groupA : groupB;
863 const TNodeID& from_nodeID = *low_nodeIDs.rbegin();
864 const TNodeID& to_nodeID = *high_nodeIDs.begin();
877 bool ignoreCovariances =
true)
const 880 this, itEdge, ignoreCovariances);
893 bool ignoreCovariances =
true)
const 900 "Request for edge %u->%u that doesn't exist in graph.",
901 static_cast<unsigned int>(from_id),
902 static_cast<unsigned int>(to_id)));
933 graph,
"Invalid pointer to the graph instance was provided.");
937 const typename BASE::edge_t& virt_edge(p_to - p_from);
944 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
945 class EDGE_ANNOTATIONS>
949 EDGE_ANNOTATIONS>&
obj)
959 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
960 class EDGE_ANNOTATIONS>
964 EDGE_ANNOTATIONS>&
obj)
1027 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
1028 class EDGE_ANNOTATIONS>
1030 CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>>
1034 return std::string(
"mrpt::graphs::CNetworkOfPoses<") +
A directed graph with the argument of the template specifying the type of the annotations in the edge...
CNetworkOfPoses< mrpt::poses::CPosePDFGaussianInf, mrpt::utils::map_traits_stdmap > CNetworkOfPoses2DInf
The specialization of CNetworkOfPoses for poses of type CPosePDFGaussianInf, also implementing serial...
double getGlobalSquareError(bool ignoreCovariances=true) const
Computes the overall square error from all the pose constraints (edges) with respect to the global po...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS >::global_pose_t self_t
void dijkstra_nodes_estimate()
Spanning tree computation of a simple estimation of the global coordinates of each node just from the...
CNetworkOfPoses< mrpt::poses::CPosePDFGaussianInf, mrpt::utils::map_traits_stdmap, mrpt::graphs::detail::TMRSlamNodeAnnotations > CNetworkOfPoses2DInf_NA
Specializations of CNetworkOfPoses for graphs whose nodes inherit from TMRSlamNodeAnnotations struct...
global_pose_t(const global_pose_t &other)
Copy constructor - delegate copying to the NODE_ANNOTATIONS struct.
NODE_ANNOTATIONS node_annotations_t
The extra annotations in nodes, apart from a constraint_no_pdf_t.
void clear()
Empty all edges, nodes and set root to ID 0.
CNetworkOfPoses< mrpt::poses::CPose2D, mrpt::utils::map_traits_stdmap > CNetworkOfPoses2D
The specialization of CNetworkOfPoses for poses of type CPose2D (not a PDF!), also implementing seria...
A template to obtain the type of its argument as a string at compile time.
Traits for using a mrpt::utils::map_as_vector<> (dense, fastest representation)
CNetworkOfPoses< mrpt::poses::CPose3DPDFGaussianInf, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3DInf
The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussianInf, also implementing seri...
size_t collapseDuplicatedEdges()
Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them...
edges_map_t edges
The public member with the directed edges in the graph.
void saveToTextFile(const std::string &fileName) const
Saves to a text file in the format used by TORO & HoG-man (more on the format <a href="http://www.mrpt.org/Robotics_file_formats" * >here) For 2D graphs only VERTEX2 & EDGE2 entries will be saved, and VERTEX3 & EDGE3 entries for 3D graphs.
friend std::ostream & operator<<(std::ostream &o, const self_t &global_pose)
Abstract graph and tree data structures, plus generic graph algorithms.
void getNeighborsOf(const TNodeID nodeID, std::set< TNodeID > &neighborIDs) const
Return the list of all neighbors of "nodeID", by creating a list of their node IDs.
void loadFromTextFile(const std::string &fileName, bool collapse_dup_edges=true)
Loads from a text file in the format used by TORO & HoG-man (more on the format here) Recognized line...
const Scalar * const_iterator
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
CNetworkOfPoses< mrpt::poses::CPose3DPDFGaussianInf, mrpt::utils::map_traits_stdmap, mrpt::graphs::detail::TMRSlamNodeAnnotations > CNetworkOfPoses3DInf_NA
GLsizei GLsizei GLuint * obj
double getEdgeSquareError(const typename BASE::edges_map_t::const_iterator &itEdge, bool ignoreCovariances=true) const
Computes the square error of one pose constraints (edge) with respect to the global poses in nodes If...
void clearEdges()
Erase all edges.
static void graph_of_poses_dijkstra_init(graph_t *g)
Base class for C*Visualizer classes.
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, global_pose_t > global_poses_t
A map from pose IDs to their global coordinate estimates, without uncertainty (the "most-likely value...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Internal functions for MRPT.
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.
CNetworkOfPoses< mrpt::poses::CPose3D, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3D
The specialization of CNetworkOfPoses for poses of type mrpt::poses::CPose3D (not a PDF!)...
CNetworkOfPoses< mrpt::poses::CPose3DPDFGaussian, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3DCov
The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian, also implementing seriali...
global_pose_t(const ARG1 &a1)
void getUnconnectedNodeIDs(std::set< mrpt::utils::TNodeID > *set_nodeIDs) const
Fill set with the nodeIDs Dijkstra algorithm could not reach starting from the root node...
The type of each global pose in nodes: an extension of the constraint_no_pdf_t pose with any optional...
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, CPOSE > global_poses_pdf_t
A map from pose IDs to their global coordinate estimates, with uncertainty.
void insertEdge(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t &edge_value)
Insert an edge (from -> to) with the given edge value.
std::shared_ptr< CSetOfObjects > Ptr
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
uint64_t TNodeID
The type for node IDs in graphs of different types.
bool operator!=(const global_pose_t &other) const
static void connectGraphPartitions(self_t *sub_graph, const std::set< TNodeID > &groupA, const std::set< TNodeID > &groupB)
Add an edge between the last node of the group with the lower nodeIDs and the first node of the highe...
CPOSE::type_value constraint_no_pdf_t
The type of edges or their means if they are PDFs (that is, a simple "edge" value) ...
mrpt::graphs::CDirectedGraph< CPOSE, EDGE_ANNOTATIONS > BASE
The base class "CDirectedGraph<CPOSE,EDGE_ANNOTATIONS>".
GLsizei const GLchar ** string
size_t nodeCount() const
Return number of nodes in the list nodes of global coordinates (may be different that all nodes appea...
void extractSubGraph(const std::set< TNodeID > &node_IDs, self_t *sub_graph, const TNodeID root_node_in=INVALID_NODEID, const bool &auto_expand_set=true) const
Find the edges between the nodes in the node_IDs set and fill given graph pointer accordingly...
global_pose_t()
Potential class constructors.
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)
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...
CPOSE constraint_t
The type of PDF poses in the contraints (edges) (=CPOSE template argument)
void mergeGraph(const self_t &other, const typename std::vector< detail::THypothesis< self_t >> &common_hypots, const bool hypots_from_other_to_self=true, std::map< TNodeID, TNodeID > *old_to_new_nodeID_mappings_out=NULL)
Integrate given graph into own graph using the list of provided common THypotheses.
CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > self_t
My own type.
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)
a helper struct with static template functions
mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > &obj)
Binary serialization (read) operator "stream >> graph".
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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...
global_pose_t(const ARG1 &a1, const ARG2 &a2)
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
mrpt::utils::TNodeID root
The ID of the node that is the origin of coordinates, used as reference by all coordinates in nodes...
T absDiff(const T &lhs, const T &rhs)
Absolute difference between two numbers.
Traits for using a std::map<> (sparse representation)
bool edges_store_inverse_poses
False (default) if an edge i->j stores the normal relative pose of j as seen from i: True if an edge...
GLsizei const GLfloat * value
bool operator==(const global_pose_t &other) const
EDGE_ANNOTATIONS edge_annotations_t
The extra annotations in edges, apart from a constraint_t.
double getEdgeSquareError(const mrpt::utils::TNodeID from_id, const mrpt::utils::TNodeID to_id, bool ignoreCovariances=true) const
Computes the square error of one pose constraints (edge) with respect to the global poses in nodes If...
An edge hypothesis between two nodeIDs.
Struct to be used as the NODE_ANNOTATIONS template argument in CNetworkOfPoses class instances for us...
mrpt::utils::CStream & operator<<(mrpt::utils::CStream &out, const CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > &obj)
Binary serialization (write) operator "stream << graph".
#define ASSERTMSG_(f, __ERROR_MSG)
Struct to be used as the NODE_ANNOTATIONS template argument in CNetworkOfPoses class instances for us...
Wrapper class that provides visualization of a network of poses that have been registered by many gra...
Custom exception class that passes information in case an unconnected graph is passed to a Dijkstra i...
MAPS_IMPLEMENTATION maps_implementation_t
The type of map's implementation (=MAPS_IMPLEMENTATION template argument)
edges_map_t::const_iterator const_iterator
CNetworkOfPoses< mrpt::poses::CPosePDFGaussian, mrpt::utils::map_traits_stdmap > CNetworkOfPoses2DCov
The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian, also implementing serializa...
#define MRPT_DECLARE_TTYPENAME(_TYPE)
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr object, const mrpt::utils::TParametersDouble &viz_params) const
Return 3D Visual Representation of the edges and nodes in the network of poses.
static void addVirtualEdge(self_t *graph, const TNodeID &from, const TNodeID &to)
Add a virtual edge between two nodes in the given graph.