00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef MRPT_GRAPHS_H
00029 #define MRPT_GRAPHS_H
00030
00031 #include <mrpt/math/utils.h>
00032 #include <set>
00033
00034
00035
00036
00037 namespace mrpt
00038 {
00039 namespace math
00040 {
00041 using mrpt::utils::TNodeID;
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 template<class TYPE_EDGES>
00054 class CDirectedGraph
00055 {
00056 public:
00057 typedef TYPE_EDGES edge_t;
00058 typedef typename mrpt::aligned_containers<TPairNodeIDs,edge_t>::multimap_t edges_map_t;
00059 typedef typename edges_map_t::iterator iterator;
00060 typedef typename edges_map_t::const_iterator const_iterator;
00061
00062
00063 edges_map_t edges;
00064
00065
00066 inline CDirectedGraph(const edges_map_t &obj) : edges(obj) { }
00067 inline CDirectedGraph() : edges() {}
00068
00069 inline iterator begin() { return edges.begin(); }
00070 inline iterator end() { return edges.end(); }
00071 inline const_iterator begin() const { return edges.begin(); }
00072 inline const_iterator end() const { return edges.end(); }
00073
00074
00075
00076
00077
00078 inline size_t edgeCount() const { return edges.size(); }
00079 inline void clearEdges() { edges.clear(); }
00080
00081
00082
00083 inline void insertEdge(TNodeID from_nodeID, TNodeID to_nodeID,const edge_t &edge_value )
00084 {
00085 EIGEN_ALIGN16 typename edges_map_t::value_type entry(
00086 std::make_pair(from_nodeID,to_nodeID),
00087 edge_value);
00088 edges.insert(entry);
00089 }
00090
00091
00092 inline void insertEdgeAtEnd(TNodeID from_nodeID, TNodeID to_nodeID,const edge_t &edge_value )
00093 {
00094 EIGEN_ALIGN16 typename edges_map_t::value_type entry(
00095 std::make_pair(from_nodeID,to_nodeID),
00096 edge_value);
00097 edges.insert(edges.end(), entry);
00098 }
00099
00100
00101 inline bool edgeExists(TNodeID from_nodeID, TNodeID to_nodeID) const
00102 { return edges.find(std::make_pair(from_nodeID,to_nodeID))!=edges.end(); }
00103
00104
00105
00106
00107
00108
00109 edge_t & getEdge(TNodeID from_nodeID, TNodeID to_nodeID)
00110 {
00111 iterator it = edges.find(std::make_pair(from_nodeID,to_nodeID));
00112 if (it==edges.end())
00113 THROW_EXCEPTION( format("Edge %u->%u does not exist",(unsigned)from_nodeID,(unsigned)to_nodeID) )
00114 else return it->second;
00115 }
00116
00117
00118
00119
00120
00121
00122 const edge_t & getEdge(TNodeID from_nodeID, TNodeID to_nodeID) const
00123 {
00124 const_iterator it = edges.find(std::make_pair(from_nodeID,to_nodeID));
00125 if (it==edges.end())
00126 THROW_EXCEPTION( format("Edge %u->%u does not exist",(unsigned)from_nodeID,(unsigned)to_nodeID) )
00127 else return it->second;
00128 }
00129
00130
00131 std::pair<iterator,iterator> getEdges(TNodeID from_nodeID, TNodeID to_nodeID) {
00132 return edges.equal_range( std::make_pair(from_nodeID,to_nodeID) );
00133 }
00134
00135 std::pair<const_iterator,const_iterator> getEdges(TNodeID from_nodeID, TNodeID to_nodeID) const {
00136 return edges.equal_range( std::make_pair(from_nodeID,to_nodeID) );
00137 }
00138
00139
00140
00141 inline void eraseEdge(TNodeID from_nodeID, TNodeID to_nodeID) {
00142 edges.erase(std::make_pair(from_nodeID,to_nodeID));
00143 }
00144
00145
00146
00147 void getAllNodes( std::set<TNodeID> &lstNode_IDs) const
00148 {
00149 lstNode_IDs.clear();
00150 for (typename edges_map_t::const_iterator it=edges.begin();it!=edges.end();++it)
00151 {
00152 lstNode_IDs.insert(it->first.first);
00153 lstNode_IDs.insert(it->first.second);
00154 }
00155 }
00156
00157
00158 inline std::set<TNodeID> getAllNodes() const { std::set<TNodeID> lst; getAllNodes(lst); return lst; }
00159
00160
00161
00162 size_t countDifferentNodesInEdges() const
00163 {
00164 std::set<TNodeID> aux;
00165 for (typename edges_map_t::const_iterator it=edges.begin();it!=edges.end();++it)
00166 {
00167 aux.insert(it->first.first);
00168 aux.insert(it->first.second);
00169 }
00170 return aux.size();
00171 }
00172
00173
00174 void getNeighborsOf(const TNodeID nodeID, std::set<TNodeID> &neighborIDs) const
00175 {
00176 neighborIDs.clear();
00177 for (typename edges_map_t::const_iterator it=edges.begin();it!=edges.end();++it)
00178 {
00179 if (it->first.first==nodeID)
00180 neighborIDs.insert(it->first.second);
00181 else if (it->first.second==nodeID)
00182 neighborIDs.insert(it->first.first);
00183 }
00184 }
00185
00186
00187
00188
00189
00190
00191
00192
00193 template <class MAP_NODEID_SET_NODEIDS>
00194 void getAdjacencyMatrix( MAP_NODEID_SET_NODEIDS &outAdjacency ) const
00195 {
00196 outAdjacency.clear();
00197 for (typename edges_map_t::const_iterator it=edges.begin();it!=edges.end();++it)
00198 {
00199 outAdjacency[it->first.first].insert(it->first.second);
00200 outAdjacency[it->first.second].insert(it->first.first);
00201 }
00202 }
00203
00204
00205
00206 };
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219 template <class TYPE_EDGES = uint8_t>
00220 class CDirectedTree
00221 {
00222 public:
00223 struct TEdgeInfo
00224 {
00225 TNodeID id;
00226 bool reverse;
00227 TYPE_EDGES data;
00228 };
00229 typedef std::list<TEdgeInfo> TListEdges;
00230 typedef std::map<TNodeID,TListEdges> TMapNode2ListEdges;
00231
00232
00233
00234 TNodeID root;
00235 TMapNode2ListEdges edges_to_children;
00236
00237
00238
00239
00240
00241
00242 void clear() { edges_to_children.clear(); root = INVALID_NODEID; }
00243
00244
00245 struct Visitor
00246 {
00247 typedef CDirectedTree<TYPE_EDGES> tree_t;
00248
00249
00250
00251
00252
00253
00254
00255 virtual void OnVisitNode( const TNodeID parent, const typename tree_t::TEdgeInfo &edge_to_child, const size_t depth_level ) = 0;
00256 };
00257
00258
00259 void visitDepthFirst( const TNodeID root, Visitor & user_visitor, const size_t root_depth_level =0 ) const
00260 {
00261 const size_t next_depth_level = root_depth_level+1;
00262 typename TMapNode2ListEdges::const_iterator itChildren = edges_to_children.find(root);
00263 if (itChildren==edges_to_children.end()) return;
00264 const TListEdges &children = itChildren->second;
00265 for (typename TListEdges::const_iterator itEdge=children.begin();itEdge!=children.end();++itEdge)
00266 {
00267 user_visitor.OnVisitNode(root,*itEdge,next_depth_level);
00268 visitDepthFirst(itEdge->id,user_visitor, next_depth_level);
00269 }
00270 }
00271
00272
00273 void visitBreadthFirst( const TNodeID root, Visitor & user_visitor, const size_t root_depth_level =0 ) const
00274 {
00275 const size_t next_depth_level = root_depth_level+1;
00276 typename TMapNode2ListEdges::const_iterator itChildren = edges_to_children.find(root);
00277 if (itChildren==edges_to_children.end()) return;
00278 const TListEdges &children = itChildren->second;
00279 for (typename TListEdges::const_iterator itEdge=children.begin();itEdge!=children.end();++itEdge)
00280 user_visitor.OnVisitNode(root,*itEdge,next_depth_level);
00281 for (typename TListEdges::const_iterator itEdge=children.begin();itEdge!=children.end();++itEdge)
00282 visitDepthFirst(itEdge->id,user_visitor,next_depth_level);
00283 }
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295 std::string getAsTextDescription() const
00296 {
00297 std::ostringstream s;
00298 struct CMyVisitor : public mrpt::math::CDirectedTree<TYPE_EDGES>::Visitor
00299 {
00300 std::ostringstream &m_s;
00301 CMyVisitor(std::ostringstream &s) : m_s(s) { }
00302 virtual void OnVisitNode( const TNodeID parent, const typename mrpt::math::CDirectedTree<TYPE_EDGES>::Visitor::tree_t::TEdgeInfo &edge_to_child, const size_t depth_level ) {
00303 m_s << std::string(depth_level*5, ' ') << (edge_to_child.reverse ? "<-" : "->" )
00304 << std::setw(3) << edge_to_child.id << std::endl;
00305 }
00306 };
00307 CMyVisitor myVisitor(s);
00308 s << std::setw(3) << root << std::endl;
00309 visitDepthFirst( root, myVisitor );
00310 return s.str();
00311 }
00312
00313
00314
00315 };
00316
00317
00318 }
00319 }
00320 #endif