9 #ifndef CONSTRAINED_POSE_NETWORK_H 10 #define CONSTRAINED_POSE_NETWORK_H 50 class MAPS_IMPLEMENTATION,
51 class NODE_ANNOTATIONS,
52 class EDGE_ANNOTATIONS
58 class MAPS_IMPLEMENTATION,
59 class NODE_ANNOTATIONS,
60 class EDGE_ANNOTATIONS
126 template <
typename ARG1>
inline global_pose_t(
const ARG1 &
a1) : constraint_no_pdf_t(a1) { }
127 template <
typename ARG1,
typename ARG2>
inline global_pose_t(
const ARG1 &
a1,
const ARG2 &
a2) : constraint_no_pdf_t(a1,a2) { }
134 constraint_no_pdf_t(other),
135 NODE_ANNOTATIONS(other)
140 static_cast<const constraint_no_pdf_t>(*
this) == static_cast<const constraint_no_pdf_t>(other) &&
141 static_cast<const NODE_ANNOTATIONS>(*
this) == static_cast<const NODE_ANNOTATIONS>(other));
144 return ( !(*
this == other) );
147 inline friend std::ostream&
operator<<(std::ostream& o,
const self_t& global_pose) {
148 o << global_pose.asString() <<
"| " << global_pose.retAnnotsAsString();
158 typedef typename MAPS_IMPLEMENTATION::template map<mrpt::utils::TNodeID,global_pose_t>
global_poses_t;
218 inline void loadFromTextFile(
const std::string &fileName,
bool collapse_dup_edges =
true) {
234 mrpt::opengl::CSetOfObjectsPtr
object,
237 visualizer->getAs3DObject(
object, viz_params);
296 const bool& auto_expand_set=
true)
const {
298 using namespace mrpt;
306 "\nInvalid pointer to a CNetworkOfPoses instance is given. Exiting..\n");
310 TNodeID root_node = root_node_in;
312 ASSERTMSG_(node_IDs.find(root_node) != node_IDs.end(),
313 "\nRoot_node does not exist in the given node_IDs set. Exiting.\n");
319 "Very few nodes [%lu] for which to extract a subgraph. Exiting\n",
320 static_cast<unsigned long>(node_IDs.size())));
325 bool is_fully_connected_graph =
true;
326 std::set<TNodeID> node_IDs_real;
327 if (*node_IDs.rbegin() - *node_IDs.begin() + 1 == node_IDs.size()) {
328 node_IDs_real = node_IDs;
331 is_fully_connected_graph =
false;
333 if (auto_expand_set) {
334 for (
TNodeID curr_node_ID = *node_IDs.begin();
335 curr_node_ID != *node_IDs.rbegin(); ++curr_node_ID) {
336 node_IDs_real.insert(curr_node_ID);
340 node_IDs_real = node_IDs;
346 node_IDs_it = node_IDs_real.begin();
347 node_IDs_it != node_IDs_real.end();
352 for (own_it = nodes.begin(); own_it != nodes.end(); ++own_it) {
353 if (*node_IDs_it == own_it->first) {
358 format(
"NodeID [%lu] can't be found in the initial graph.",
359 static_cast<unsigned long>(*node_IDs_it)));
362 sub_graph->
nodes.insert(make_pair(*node_IDs_it, curr_node));
373 root_node = sub_graph->
nodes.begin()->first;
375 sub_graph->
root = root_node;
385 it != BASE::edges.end();
388 const TNodeID& from = it->first.first;
389 const TNodeID& to = it->first.second;
390 const typename BASE::edge_t& curr_edge = it->second;
393 if (sub_graph->
nodes.find(from) != sub_graph->
nodes.end() &&
394 sub_graph->
nodes.find(to) != sub_graph->
nodes.end()) {
399 if (!auto_expand_set && !is_fully_connected_graph) {
404 std::set<TNodeID> root_neighbors;
407 if (root_neighbors.empty()) {
412 if ((*root_it == *sub_graph->
nodes.rbegin())) {
414 TNodeID next_to_root = (--root_it)->first;
415 self_t::addVirtualEdge(sub_graph, next_to_root, sub_graph->
root);
419 TNodeID next_to_root = (++root_it)->first;
421 self_t::addVirtualEdge(sub_graph, sub_graph->
root, next_to_root);
429 bool dijkstra_runs_successfully =
false;
433 while (!dijkstra_runs_successfully) {
435 dijkstra_t dijkstra(*sub_graph, sub_graph->
root);
436 dijkstra_runs_successfully =
true;
439 dijkstra_runs_successfully =
false;
441 set<TNodeID> unconnected_nodeIDs;
451 const TNodeID& island_highest = *unconnected_nodeIDs.rbegin();
452 const TNodeID& island_lowest = *unconnected_nodeIDs.begin();
459 std::set<TNodeID> mainland;
462 n_it = sub_graph->
nodes.begin();
463 n_it != sub_graph->
nodes.end();
465 bool is_there =
false;
469 uncon_it = unconnected_nodeIDs.begin();
470 uncon_it != unconnected_nodeIDs.end();
473 if (n_it->first == *uncon_it) {
480 mainland.insert(n_it->first);
484 bool is_single_island = (island_highest - island_lowest + 1 ==
485 unconnected_nodeIDs.size());
487 if (is_single_island) {
495 const std::set<TNodeID>& island = unconnected_nodeIDs;
504 std::vector<std::set<TNodeID> > vec_of_islands;
505 std::set<TNodeID> curr_island;
506 TNodeID prev_nodeID = *unconnected_nodeIDs.begin();
507 curr_island.insert(prev_nodeID);
509 it = ++unconnected_nodeIDs.begin();
510 *it < sub_graph->
root && it != unconnected_nodeIDs.end();
512 if (!(
absDiff(*it, prev_nodeID) == 1)) {
513 vec_of_islands.push_back(curr_island);
516 curr_island.insert(*it);
521 vec_of_islands.push_back(curr_island);
554 const bool hypots_from_other_to_self=
true,
555 std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings_out=NULL) {
565 const self_t& graph_from = (hypots_from_other_to_self? other : *
this);
566 const self_t& graph_to = (hypots_from_other_to_self? *this : other);
574 for (hypots_cit_t h_cit = common_hypots.begin();
575 h_cit != common_hypots.end();
579 format(
"NodeID %lu is not found in (from) graph", h_cit->from))
581 format(
"NodeID %lu is not found in (to) graph", h_cit->to))
586 for (nodes_cit_t n_cit = this->nodes.begin();
587 n_cit != this->nodes.end();
589 if (n_cit->first > max_nodeID) {
590 max_nodeID = n_cit->first;
593 TNodeID renum_start = max_nodeID + 1;
594 size_t renum_counter = 0;
599 std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings;
605 std::map<TNodeID, TNodeID> mappings_tmp;
608 if (old_to_new_nodeID_mappings_out) {
609 old_to_new_nodeID_mappings = old_to_new_nodeID_mappings_out;
612 old_to_new_nodeID_mappings = &mappings_tmp;
614 old_to_new_nodeID_mappings->clear();
619 for (nodes_cit_t n_cit = other.
nodes.begin();
620 n_cit != other.
nodes.end();
622 TNodeID new_nodeID = renum_start + renum_counter++;
623 old_to_new_nodeID_mappings->insert(make_pair(
626 this->nodes.insert(make_pair(
627 new_nodeID, n_cit->second));
635 for (hypots_cit_t h_cit = common_hypots.begin();
636 h_cit != common_hypots.end();
639 if (hypots_from_other_to_self) {
640 from = old_to_new_nodeID_mappings->at(h_cit->from);
645 to = old_to_new_nodeID_mappings->at(h_cit->to);
655 g_cit = other.
begin();
656 g_cit != other.
end();
658 TNodeID new_from = old_to_new_nodeID_mappings->at(g_cit->first.first);
659 TNodeID new_to = old_to_new_nodeID_mappings->at(g_cit->first.second);
660 this->
insertEdge(new_from, new_to, g_cit->second);
682 const std::set<TNodeID>& groupA,
683 const std::set<TNodeID>& groupB) {
687 "\nInvalid pointer to a CNetworkOfPoses instance is given. Exiting..\n");
688 ASSERTMSG_(!groupA.empty(),
"\ngroupA is empty.");
689 ASSERTMSG_(!groupB.empty(),
"\ngroupB is empty.");
693 *groupA.rend() < *groupB.rbegin() ||
694 *groupA.rbegin() > *groupB.rend(),
695 "Groups A, B contain overlapping nodeIDs");
699 const std::set<TNodeID>& low_nodeIDs =
700 *groupA.rbegin() < *groupB.rbegin() ? groupA : groupB;
701 const std::set<TNodeID>& high_nodeIDs =
702 *groupA.rbegin() > *groupB.rbegin() ? groupA : groupB;
705 const TNodeID& from_nodeID = *low_nodeIDs.rbegin();
706 const TNodeID& to_nodeID = *high_nodeIDs.begin();
707 self_t::addVirtualEdge(sub_graph, from_nodeID, to_nodeID);
720 bool ignoreCovariances =
true)
const {
739 ASSERTMSG_(itEdge!=BASE::edges.
end(),
format(
"Request for edge %u->%u that doesn't exist in graph.",static_cast<unsigned int>(from_id),static_cast<unsigned int>(to_id)));
748 edges_store_inverse_poses =
false;
756 inline size_t nodeCount()
const {
return nodes.size(); }
767 edges_store_inverse_poses(false)
780 EDGE_ANNOTATIONS>(*this);
788 EDGE_ANNOTATIONS>(*this);
809 ASSERTMSG_(graph,
"Invalid pointer to the graph instance was provided.");
813 const typename BASE::edge_t& virt_edge(p_to - p_from);
826 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
class EDGE_ANNOTATIONS>
827 mrpt::utils::CStream & operator << (mrpt::utils::CStream&out, const CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION,NODE_ANNOTATIONS,EDGE_ANNOTATIONS> &obj)
835 template <
class CPOSE,
class MAPS_IMPLEMENTATION,
class NODE_ANNOTATIONS,
class EDGE_ANNOTATIONS>
871 class MAPS_IMPLEMENTATION,
872 class NODE_ANNOTATIONS,
873 class EDGE_ANNOTATIONS
877 static std::string
get()
879 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...
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...
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.
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.
CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > self_t
My own type.
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.
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...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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
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...
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.
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.
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
uint64_t TNodeID
The type for node IDs in graphs of different types.
CNetworkOfPoses()
Default constructor (just sets root to "0" and edges_store_inverse_poses to "false") ...
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...
void insertEdge(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t &edge_value)
Insert an edge (from -> to) with the given edge value.
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...
CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS >::global_pose_t self_t
CPOSE::type_value constraint_no_pdf_t
The type of edges or their means if they are PDFs (that is, a simple "edge" value) ...
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.
CNetworkOfPoses< mrpt::poses::CPose3D, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3D
The specialization of CNetworkOfPoses for poses of type mrpt::poses::CPose3D (not a PDF!)...
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.
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr object, const mrpt::utils::TParametersDouble &viz_params) const
Return 3D Visual Representation of the edges and nodes in the network of poses.
CPOSE constraint_t
The type of PDF poses in the contraints (edges) (=CPOSE template argument)
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, CPOSE > global_poses_pdf_t
A map from pose IDs to their global coordinate estimates, with uncertainty.
mrpt::graphs::detail::CVisualizer< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > * visualizer
Pointer to the CVisualizer instance used to visualize the graph in the opengl window.
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::graphs::CDirectedGraph< CPOSE, EDGE_ANNOTATIONS > BASE
The base class "CDirectedGraph<CPOSE,EDGE_ANNOTATIONS>" */.
mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > &obj)
Binary serialization (read) operator "stream >> graph".
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...
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...
#define ASSERTMSG_(f, __ERROR_MSG)
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...
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)
static void addVirtualEdge(self_t *graph, const TNodeID &from, const TNodeID &to)
Add a virtual edge between two nodes in the given graph.