Gazebo Math

API Reference

7.5.1
gz/math/graph/Graph.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef GZ_MATH_GRAPH_GRAPH_HH_
18 #define GZ_MATH_GRAPH_GRAPH_HH_
19 
20 #include <cassert>
21 #include <iostream>
22 #include <map>
23 #include <set>
24 #include <string>
25 #include <utility>
26 #include <vector>
27 
28 #include <gz/math/config.hh>
29 #include "gz/math/graph/Edge.hh"
30 #include "gz/math/graph/Vertex.hh"
31 
32 namespace gz::math
33 {
34 // Inline bracket to help doxygen filtering.
35 inline namespace GZ_MATH_VERSION_NAMESPACE {
36 namespace graph
37 {
46  //
99  template<typename V, typename E, typename EdgeType>
100  class Graph
101  {
103  public: Graph() = default;
104 
108  public: Graph(const std::vector<Vertex<V>> &_vertices,
109  const std::vector<EdgeInitializer<E>> &_edges)
110  {
111  // Add all vertices.
112  for (auto const &v : _vertices)
113  {
114  if (!this->AddVertex(v.Name(), v.Data(), v.Id()).Valid())
115  {
116  std::cerr << "Invalid vertex with Id [" << v.Id() << "]. Ignoring."
117  << std::endl;
118  }
119  }
120 
121  // Add all edges.
122  for (auto const &e : _edges)
123  {
124  if (!this->AddEdge(e.vertices, e.data, e.weight).Valid())
125  std::cerr << "Ignoring edge" << std::endl;
126  }
127  }
128 
135  public: Vertex<V> &AddVertex(const std::string &_name,
136  const V &_data,
137  const VertexId &_id = kNullId)
138  {
139  auto id = _id;
140 
141  // The user didn't provide an Id, we generate it.
142  if (id == kNullId)
143  {
144  id = this->NextVertexId();
145 
146  // No space for new Ids.
147  if (id == kNullId)
148  {
149  std::cerr << "[Graph::AddVertex()] The limit of vertices has been "
150  << "reached. Ignoring vertex." << std::endl;
151  return Vertex<V>::NullVertex;
152  }
153  }
154 
155  // Create the vertex.
156  auto ret = this->vertices.insert(
157  std::make_pair(id, Vertex<V>(_name, _data, id)));
158 
159  // The Id already exists.
160  if (!ret.second)
161  {
162  std::cerr << "[Graph::AddVertex()] Repeated vertex [" << id << "]"
163  << std::endl;
164  return Vertex<V>::NullVertex;
165  }
166 
167  // Link the vertex with an empty list of edges.
168  this->adjList[id] = EdgeId_S();
169 
170  // Update the map of names.
171  this->names.insert(std::make_pair(_name, id));
172 
173  return ret.first->second;
174  }
175 
179  public: const VertexRef_M<V> Vertices() const
180  {
181  VertexRef_M<V> res;
182  for (auto const &v : this->vertices)
183  res.emplace(std::make_pair(v.first, std::cref(v.second)));
184 
185  return res;
186  }
187 
191  public: const VertexRef_M<V> Vertices(const std::string &_name) const
192  {
193  VertexRef_M<V> res;
194  for (auto const &vertex : this->vertices)
195  {
196  if (vertex.second.Name() == _name)
197  res.emplace(std::make_pair(vertex.first, std::cref(vertex.second)));
198  }
199 
200  return res;
201  }
202 
209  public: EdgeType &AddEdge(const VertexId_P &_vertices,
210  const E &_data,
211  const double _weight = 1.0)
212  {
213  auto id = this->NextEdgeId();
214 
215  // No space for new Ids.
216  if (id == kNullId)
217  {
218  std::cerr << "[Graph::AddEdge()] The limit of edges has been reached. "
219  << "Ignoring edge." << std::endl;
220  return EdgeType::NullEdge;
221  }
222 
223  EdgeType newEdge(_vertices, _data, _weight, id);
224  return this->LinkEdge(std::move(newEdge));
225  }
226 
233  public: EdgeType &LinkEdge(const EdgeType &_edge)
234  {
235  auto edgeVertices = _edge.Vertices();
236 
237  // Sanity check: Both vertices should exist.
238  for (auto const &v : {edgeVertices.first, edgeVertices.second})
239  {
240  if (this->vertices.find(v) == this->vertices.end())
241  return EdgeType::NullEdge;
242  }
243 
244  // Link the new edge.
245  for (auto const &v : {edgeVertices.first, edgeVertices.second})
246  {
247  if (v != kNullId)
248  {
249  auto vertexIt = this->adjList.find(v);
250  assert(vertexIt != this->adjList.end());
251  vertexIt->second.insert(_edge.Id());
252  }
253  }
254 
255  auto ret = this->edges.insert(std::make_pair(_edge.Id(), _edge));
256 
257  // Return the new edge.
258  return ret.first->second;
259  }
260 
264  public: const EdgeRef_M<EdgeType> Edges() const
265  {
267  for (auto const &edge : this->edges)
268  {
269  res.emplace(std::make_pair(edge.first, std::cref(edge.second)));
270  }
271 
272  return res;
273  }
274 
290  public: VertexRef_M<V> AdjacentsFrom(const VertexId &_vertex) const
291  {
292  VertexRef_M<V> res;
293 
294  // Make sure the vertex exists
295  auto vertexIt = this->adjList.find(_vertex);
296  if (vertexIt == this->adjList.end())
297  return res;
298 
299  for (auto const &edgeId : vertexIt->second)
300  {
301  const auto &edge = this->EdgeFromId(edgeId);
302  auto neighborVertexId = edge.From(_vertex);
303  if (neighborVertexId != kNullId)
304  {
305  const auto &neighborVertex = this->VertexFromId(neighborVertexId);
306  res.emplace(
307  std::make_pair(neighborVertexId, std::cref(neighborVertex)));
308  }
309  }
310 
311  return res;
312  }
313 
329  public: VertexRef_M<V> AdjacentsFrom(const Vertex<V> &_vertex) const
330  {
331  return this->AdjacentsFrom(_vertex.Id());
332  }
333 
349  public: VertexRef_M<V> AdjacentsTo(const VertexId &_vertex) const
350  {
351  auto incidentEdges = this->IncidentsTo(_vertex);
352 
353  VertexRef_M<V> res;
354  for (auto const &incidentEdgeRef : incidentEdges)
355  {
356  const auto &incidentEdgeId = incidentEdgeRef.first;
357  const auto &incidentEdge = this->EdgeFromId(incidentEdgeId);
358  const auto &neighborVertexId = incidentEdge.To(_vertex);
359  const auto &neighborVertex = this->VertexFromId(neighborVertexId);
360  res.emplace(
361  std::make_pair(neighborVertexId, std::cref(neighborVertex)));
362  }
363 
364  return res;
365  }
366 
382  public: VertexRef_M<V> AdjacentsTo(const Vertex<V> &_vertex) const
383  {
384  return this->AdjacentsTo(_vertex.Id());
385  }
386 
390  public: size_t InDegree(const VertexId &_vertex) const
391  {
392  return this->IncidentsTo(_vertex).size();
393  }
394 
398  public: size_t InDegree(const Vertex<V> &_vertex) const
399  {
400  return this->IncidentsTo(this->VertexFromId(_vertex.Id())).size();
401  }
402 
406  public: size_t OutDegree(const VertexId &_vertex) const
407  {
408  return this->IncidentsFrom(_vertex).size();
409  }
410 
414  public: size_t OutDegree(const Vertex<V> &_vertex) const
415  {
416  return this->IncidentsFrom(this->VertexFromId(_vertex.Id())).size();
417  }
418 
424  public: const EdgeRef_M<EdgeType> IncidentsFrom(const VertexId &_vertex)
425  const
426  {
428 
429  const auto &adjIt = this->adjList.find(_vertex);
430  if (adjIt == this->adjList.end())
431  return res;
432 
433  const auto &edgeIds = adjIt->second;
434  for (auto const &edgeId : edgeIds)
435  {
436  const auto &edge = this->EdgeFromId(edgeId);
437  if (edge.From(_vertex) != kNullId)
438  res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
439  }
440 
441  return res;
442  }
443 
450  const Vertex<V> &_vertex) const
451  {
452  return this->IncidentsFrom(_vertex.Id());
453  }
454 
461  const VertexId &_vertex) const
462  {
464 
465  const auto &adjIt = this->adjList.find(_vertex);
466  if (adjIt == this->adjList.end())
467  return res;
468 
469  const auto &edgeIds = adjIt->second;
470  for (auto const &edgeId : edgeIds)
471  {
472  const auto &edge = this->EdgeFromId(edgeId);
473  if (edge.To(_vertex) != kNullId)
474  res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
475  }
476 
477  return res;
478  }
479 
485  public: const EdgeRef_M<EdgeType> IncidentsTo(const Vertex<V> &_vertex)
486  const
487  {
488  return this->IncidentsTo(_vertex.Id());
489  }
490 
494  public: bool Empty() const
495  {
496  return this->vertices.empty();
497  }
498 
502  public: bool RemoveVertex(const VertexId &_vertex)
503  {
504  auto vIt = this->vertices.find(_vertex);
505  if (vIt == this->vertices.end())
506  return false;
507 
508  std::string name = vIt->second.Name();
509 
510  // Remove incident edges.
511  auto incidents = this->IncidentsTo(_vertex);
512  for (auto edgePair : incidents)
513  this->RemoveEdge(edgePair.first);
514 
515  // Remove all outgoing edges.
516  incidents = this->IncidentsFrom(_vertex);
517  for (auto edgePair : incidents)
518  this->RemoveEdge(edgePair.first);
519 
520  // Remove the vertex (key) from the adjacency list.
521  this->adjList.erase(_vertex);
522 
523  // Remove the vertex.
524  this->vertices.erase(_vertex);
525 
526  // Get an iterator to all vertices sharing name.
527  auto iterPair = this->names.equal_range(name);
528  for (auto it = iterPair.first; it != iterPair.second; ++it)
529  {
530  if (it->second == _vertex)
531  {
532  this->names.erase(it);
533  break;
534  }
535  }
536 
537  return true;
538  }
539 
543  public: bool RemoveVertex(Vertex<V> &_vertex)
544  {
545  return this->RemoveVertex(_vertex.Id());
546  }
547 
551  public: size_t RemoveVertices(const std::string &_name)
552  {
553  size_t numVertices = this->names.count(_name);
554  size_t result = 0;
555  for (size_t i = 0; i < numVertices; ++i)
556  {
557  auto iter = this->names.find(_name);
558  if (this->RemoveVertex(iter->second))
559  ++result;
560  }
561 
562  return result;
563  }
564 
570  public: bool RemoveEdge(const EdgeId &_edge)
571  {
572  auto edgeIt = this->edges.find(_edge);
573  if (edgeIt == this->edges.end())
574  return false;
575 
576  auto edgeVertices = edgeIt->second.Vertices();
577 
578  // Unlink the edge.
579  for (auto const &v : {edgeVertices.first, edgeVertices.second})
580  {
581  if (edgeIt->second.From(v) != kNullId)
582  {
583  auto vertex = this->adjList.find(v);
584  assert(vertex != this->adjList.end());
585  vertex->second.erase(_edge);
586  }
587  }
588 
589  this->edges.erase(_edge);
590 
591  return true;
592  }
593 
599  public: bool RemoveEdge(EdgeType &_edge)
600  {
601  return this->RemoveEdge(_edge.Id());
602  }
603 
608  public: const Vertex<V> &VertexFromId(const VertexId &_id) const
609  {
610  auto iter = this->vertices.find(_id);
611  if (iter == this->vertices.end())
612  return Vertex<V>::NullVertex;
613 
614  return iter->second;
615  }
616 
621  public: Vertex<V> &VertexFromId(const VertexId &_id)
622  {
623  auto iter = this->vertices.find(_id);
624  if (iter == this->vertices.end())
625  return Vertex<V>::NullVertex;
626 
627  return iter->second;
628  }
629 
638  public: const EdgeType &EdgeFromVertices(
639  const VertexId _sourceId, const VertexId _destId) const
640  {
641  // Get the adjacency iterator for the source vertex.
642  const typename std::map<VertexId, EdgeId_S>::const_iterator &adjIt =
643  this->adjList.find(_sourceId);
644 
645  // Quit early if there is no adjacency entry
646  if (adjIt == this->adjList.end())
647  return EdgeType::NullEdge;
648 
649  // Loop over the edges in the source vertex's adjacency list
650  for (std::set<EdgeId>::const_iterator edgIt = adjIt->second.begin();
651  edgIt != adjIt->second.end(); ++edgIt)
652  {
653  // Get an iterator to the actual edge
654  const typename std::map<EdgeId, EdgeType>::const_iterator edgeIter =
655  this->edges.find(*edgIt);
656 
657  // Check if the edge has the correct source and destination.
658  if (edgeIter != this->edges.end() &&
659  edgeIter->second.From(_sourceId) == _destId)
660  {
661  assert(edgeIter->second.To(_destId) == _sourceId);
662  return edgeIter->second;
663  }
664  }
665 
666  return EdgeType::NullEdge;
667  }
668 
673  public: const EdgeType &EdgeFromId(const EdgeId &_id) const
674  {
675  auto iter = this->edges.find(_id);
676  if (iter == this->edges.end())
677  return EdgeType::NullEdge;
678 
679  return iter->second;
680  }
681 
686  public: EdgeType &EdgeFromId(const EdgeId &_id)
687  {
688  auto iter = this->edges.find(_id);
689  if (iter == this->edges.end())
690  return EdgeType::NullEdge;
691 
692  return iter->second;
693  }
694 
700  public: template<typename VV, typename EE, typename EEdgeType>
702  const Graph<VV, EE, EEdgeType> &_g);
703 
706  private: VertexId &NextVertexId()
707  {
708  while (this->vertices.find(this->nextVertexId) != this->vertices.end()
709  && this->nextVertexId < MAX_UI64)
710  {
711  ++this->nextVertexId;
712  }
713 
714  return this->nextVertexId;
715  }
716 
719  private: VertexId &NextEdgeId()
720  {
721  while (this->edges.find(this->nextEdgeId) != this->edges.end() &&
722  this->nextEdgeId < MAX_UI64)
723  {
724  ++this->nextEdgeId;
725  }
726 
727  return this->nextEdgeId;
728  }
729 
731  protected: VertexId nextVertexId = 0u;
732 
734  protected: VertexId nextEdgeId = 0u;
735 
737  private: std::map<VertexId, Vertex<V>> vertices;
738 
740  private: std::map<EdgeId, EdgeType> edges;
741 
747  private: std::map<VertexId, EdgeId_S> adjList;
748 
751  };
752 
755  template<typename VV, typename EE>
757  const Graph<VV, EE, UndirectedEdge<EE>> &_g)
758  {
759  _out << "graph {" << std::endl;
760 
761  // All vertices with the name and Id as a "label" attribute.
762  for (auto const &vertexMap : _g.Vertices())
763  {
764  auto vertex = vertexMap.second.get();
765  _out << vertex;
766  }
767 
768  // All edges.
769  for (auto const &edgeMap : _g.Edges())
770  {
771  auto edge = edgeMap.second.get();
772  _out << edge;
773  }
774 
775  _out << "}" << std::endl;
776 
777  return _out;
778  }
779 
782  template<typename VV, typename EE>
784  const Graph<VV, EE, DirectedEdge<EE>> &_g)
785  {
786  _out << "digraph {" << std::endl;
787 
788  // All vertices with the name and Id as a "label" attribute.
789  for (auto const &vertexMap : _g.Vertices())
790  {
791  auto vertex = vertexMap.second.get();
792  _out << vertex;
793  }
794 
795  // All edges.
796  for (auto const &edgeMap : _g.Edges())
797  {
798  auto edge = edgeMap.second.get();
799  _out << edge;
800  }
801 
802  _out << "}" << std::endl;
803 
804  return _out;
805  }
806 
809  template<typename V, typename E>
811 
814  template<typename V, typename E>
816 } // namespace graph
817 } // namespace GZ_MATH_VERSION_NAMESPACE
818 } // namespace gz::math::graph
819 #endif // GZ_MATH_GRAPH_GRAPH_HH_
an class="comment">// namespace GZ_MATH_VERSION_NAMESPACE
818 } // namespace gz::math::graph
819 #endif // GZ_MATH_GRAPH_GRAPH_HH_