Gazebo Math

API Reference

6.15.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 ignition
33 {
34 namespace math
35 {
36 // Inline bracket to help doxygen filtering.
37 inline namespace IGNITION_MATH_VERSION_NAMESPACE {
38 namespace graph
39 {
48  //
101  template<typename V, typename E, typename EdgeType>
102  class Graph
103  {
105  public: Graph() = default;
106 
110  public: Graph(const std::vector<Vertex<V>> &_vertices,
111  const std::vector<EdgeInitializer<E>> &_edges)
112  {
113  // Add all vertices.
114  for (auto const &v : _vertices)
115  {
116  if (!this->AddVertex(v.Name(), v.Data(), v.Id()).Valid())
117  {
118  std::cerr << "Invalid vertex with Id [" << v.Id() << "]. Ignoring."
119  << std::endl;
120  }
121  }
122 
123  // Add all edges.
124  for (auto const &e : _edges)
125  {
126  if (!this->AddEdge(e.vertices, e.data, e.weight).Valid())
127  std::cerr << "Ignoring edge" << std::endl;
128  }
129  }
130 
137  public: Vertex<V> &AddVertex(const std::string &_name,
138  const V &_data,
139  const VertexId &_id = kNullId)
140  {
141  auto id = _id;
142 
143  // The user didn't provide an Id, we generate it.
144  if (id == kNullId)
145  {
146  id = this->NextVertexId();
147 
148  // No space for new Ids.
149  if (id == kNullId)
150  {
151  std::cerr << "[Graph::AddVertex()] The limit of vertices has been "
152  << "reached. Ignoring vertex." << std::endl;
153  return Vertex<V>::NullVertex;
154  }
155  }
156 
157  // Create the vertex.
158  auto ret = this->vertices.insert(
159  std::make_pair(id, Vertex<V>(_name, _data, id)));
160 
161  // The Id already exists.
162  if (!ret.second)
163  {
164  std::cerr << "[Graph::AddVertex()] Repeated vertex [" << id << "]"
165  << std::endl;
166  return Vertex<V>::NullVertex;
167  }
168 
169  // Link the vertex with an empty list of edges.
170  this->adjList[id] = EdgeId_S();
171 
172  // Update the map of names.
173  this->names.insert(std::make_pair(_name, id));
174 
175  return ret.first->second;
176  }
177 
181  public: const VertexRef_M<V> Vertices() const
182  {
183  VertexRef_M<V> res;
184  for (auto const &v : this->vertices)
185  res.emplace(std::make_pair(v.first, std::cref(v.second)));
186 
187  return res;
188  }
189 
193  public: const VertexRef_M<V> Vertices(const std::string &_name) const
194  {
195  VertexRef_M<V> res;
196  for (auto const &vertex : this->vertices)
197  {
198  if (vertex.second.Name() == _name)
199  res.emplace(std::make_pair(vertex.first, std::cref(vertex.second)));
200  }
201 
202  return res;
203  }
204 
211  public: EdgeType &AddEdge(const VertexId_P &_vertices,
212  const E &_data,
213  const double _weight = 1.0)
214  {
215  auto id = this->NextEdgeId();
216 
217  // No space for new Ids.
218  if (id == kNullId)
219  {
220  std::cerr << "[Graph::AddEdge()] The limit of edges has been reached. "
221  << "Ignoring edge." << std::endl;
222  return EdgeType::NullEdge;
223  }
224 
225  EdgeType newEdge(_vertices, _data, _weight, id);
226  return this->LinkEdge(std::move(newEdge));
227  }
228 
235  public: EdgeType &LinkEdge(const EdgeType &_edge)
236  {
237  auto edgeVertices = _edge.Vertices();
238 
239  // Sanity check: Both vertices should exist.
240  for (auto const &v : {edgeVertices.first, edgeVertices.second})
241  {
242  if (this->vertices.find(v) == this->vertices.end())
243  return EdgeType::NullEdge;
244  }
245 
246  // Link the new edge.
247  for (auto const &v : {edgeVertices.first, edgeVertices.second})
248  {
249  if (v != kNullId)
250  {
251  auto vertexIt = this->adjList.find(v);
252  assert(vertexIt != this->adjList.end());
253  vertexIt->second.insert(_edge.Id());
254  }
255  }
256 
257  auto ret = this->edges.insert(std::make_pair(_edge.Id(), _edge));
258 
259  // Return the new edge.
260  return ret.first->second;
261  }
262 
266  public: const EdgeRef_M<EdgeType> Edges() const
267  {
269  for (auto const &edge : this->edges)
270  {
271  res.emplace(std::make_pair(edge.first, std::cref(edge.second)));
272  }
273 
274  return res;
275  }
276 
292  public: VertexRef_M<V> AdjacentsFrom(const VertexId &_vertex) const
293  {
294  VertexRef_M<V> res;
295 
296  // Make sure the vertex exists
297  auto vertexIt = this->adjList.find(_vertex);
298  if (vertexIt == this->adjList.end())
299  return res;
300 
301  for (auto const &edgeId : vertexIt->second)
302  {
303  const auto &edge = this->EdgeFromId(edgeId);
304  auto neighborVertexId = edge.From(_vertex);
305  if (neighborVertexId != kNullId)
306  {
307  const auto &neighborVertex = this->VertexFromId(neighborVertexId);
308  res.emplace(
309  std::make_pair(neighborVertexId, std::cref(neighborVertex)));
310  }
311  }
312 
313  return res;
314  }
315 
331  public: VertexRef_M<V> AdjacentsFrom(const Vertex<V> &_vertex) const
332  {
333  return this->AdjacentsFrom(_vertex.Id());
334  }
335 
351  public: VertexRef_M<V> AdjacentsTo(const VertexId &_vertex) const
352  {
353  auto incidentEdges = this->IncidentsTo(_vertex);
354 
355  VertexRef_M<V> res;
356  for (auto const &incidentEdgeRef : incidentEdges)
357  {
358  const auto &incidentEdgeId = incidentEdgeRef.first;
359  const auto &incidentEdge = this->EdgeFromId(incidentEdgeId);
360  const auto &neighborVertexId = incidentEdge.To(_vertex);
361  const auto &neighborVertex = this->VertexFromId(neighborVertexId);
362  res.emplace(
363  std::make_pair(neighborVertexId, std::cref(neighborVertex)));
364  }
365 
366  return res;
367  }
368 
384  public: VertexRef_M<V> AdjacentsTo(const Vertex<V> &_vertex) const
385  {
386  return this->AdjacentsTo(_vertex.Id());
387  }
388 
392  public: size_t InDegree(const VertexId &_vertex) const
393  {
394  return this->IncidentsTo(_vertex).size();
395  }
396 
400  public: size_t InDegree(const Vertex<V> &_vertex) const
401  {
402  return this->IncidentsTo(this->VertexFromId(_vertex.Id())).size();
403  }
404 
408  public: size_t OutDegree(const VertexId &_vertex) const
409  {
410  return this->IncidentsFrom(_vertex).size();
411  }
412 
416  public: size_t OutDegree(const Vertex<V> &_vertex) const
417  {
418  return this->IncidentsFrom(this->VertexFromId(_vertex.Id())).size();
419  }
420 
426  public: const EdgeRef_M<EdgeType> IncidentsFrom(const VertexId &_vertex)
427  const
428  {
430 
431  const auto &adjIt = this->adjList.find(_vertex);
432  if (adjIt == this->adjList.end())
433  return res;
434 
435  const auto &edgeIds = adjIt->second;
436  for (auto const &edgeId : edgeIds)
437  {
438  const auto &edge = this->EdgeFromId(edgeId);
439  if (edge.From(_vertex) != kNullId)
440  res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
441  }
442 
443  return res;
444  }
445 
452  const Vertex<V> &_vertex) const
453  {
454  return this->IncidentsFrom(_vertex.Id());
455  }
456 
463  const VertexId &_vertex) const
464  {
466 
467  const auto &adjIt = this->adjList.find(_vertex);
468  if (adjIt == this->adjList.end())
469  return res;
470 
471  const auto &edgeIds = adjIt->second;
472  for (auto const &edgeId : edgeIds)
473  {
474  const auto &edge = this->EdgeFromId(edgeId);
475  if (edge.To(_vertex) != kNullId)
476  res.emplace(std::make_pair(edge.Id(), std::cref(edge)));
477  }
478 
479  return res;
480  }
481 
487  public: const EdgeRef_M<EdgeType> IncidentsTo(const Vertex<V> &_vertex)
488  const
489  {
490  return this->IncidentsTo(_vertex.Id());
491  }
492 
496  public: bool Empty() const
497  {
498  return this->vertices.empty();
499  }
500 
504  public: bool RemoveVertex(const VertexId &_vertex)
505  {
506  auto vIt = this->vertices.find(_vertex);
507  if (vIt == this->vertices.end())
508  return false;
509 
510  std::string name = vIt->second.Name();
511 
512  // Remove incident edges.
513  auto incidents = this->IncidentsTo(_vertex);
514  for (auto edgePair : incidents)
515  this->RemoveEdge(edgePair.first);
516 
517  // Remove all outgoing edges.
518  incidents = this->IncidentsFrom(_vertex);
519  for (auto edgePair : incidents)
520  this->RemoveEdge(edgePair.first);
521 
522  // Remove the vertex (key) from the adjacency list.
523  this->adjList.erase(_vertex);
524 
525  // Remove the vertex.
526  this->vertices.erase(_vertex);
527 
528  // Get an iterator to all vertices sharing name.
529  auto iterPair = this->names.equal_range(name);
530  for (auto it = iterPair.first; it != iterPair.second; ++it)
531  {
532  if (it->second == _vertex)
533  {
534  this->names.erase(it);
535  break;
536  }
537  }
538 
539  return true;
540  }
541 
545  public: bool RemoveVertex(Vertex<V> &_vertex)
546  {
547  return this->RemoveVertex(_vertex.Id());
548  }
549 
553  public: size_t RemoveVertices(const std::string &_name)
554  {
555  size_t numVertices = this->names.count(_name);
556  size_t result = 0;
557  for (size_t i = 0; i < numVertices; ++i)
558  {
559  auto iter = this->names.find(_name);
560  if (this->RemoveVertex(iter->second))
561  ++result;
562  }
563 
564  return result;
565  }
566 
572  public: bool RemoveEdge(const EdgeId &_edge)
573  {
574  auto edgeIt = this->edges.find(_edge);
575  if (edgeIt == this->edges.end())
576  return false;
577 
578  auto edgeVertices = edgeIt->second.Vertices();
579 
580  // Unlink the edge.
581  for (auto const &v : {edgeVertices.first, edgeVertices.second})
582  {
583  if (edgeIt->second.From(v) != kNullId)
584  {
585  auto vertex = this->adjList.find(v);
586  assert(vertex != this->adjList.end());
587  vertex->second.erase(_edge);
588  }
589  }
590 
591  this->edges.erase(_edge);
592 
593  return true;
594  }
595 
601  public: bool RemoveEdge(EdgeType &_edge)
602  {
603  return this->RemoveEdge(_edge.Id());
604  }
605 
610  public: const Vertex<V> &VertexFromId(const VertexId &_id) const
611  {
612  auto iter = this->vertices.find(_id);
613  if (iter == this->vertices.end())
614  return Vertex<V>::NullVertex;
615 
616  return iter->second;
617  }
618 
623  public: Vertex<V> &VertexFromId(const VertexId &_id)
624  {
625  auto iter = this->vertices.find(_id);
626  if (iter == this->vertices.end())
627  return Vertex<V>::NullVertex;
628 
629  return iter->second;
630  }
631 
640  public: const EdgeType &EdgeFromVertices(
641  const VertexId _sourceId, const VertexId _destId) const
642  {
643  // Get the adjacency iterator for the source vertex.
644  const typename std::map<VertexId, EdgeId_S>::const_iterator &adjIt =
645  this->adjList.find(_sourceId);
646 
647  // Quit early if there is no adjacency entry
648  if (adjIt == this->adjList.end())
649  return EdgeType::NullEdge;
650 
651  // Loop over the edges in the source vertex's adjacency list
652  for (std::set<EdgeId>::const_iterator edgIt = adjIt->second.begin();
653  edgIt != adjIt->second.end(); ++edgIt)
654  {
655  // Get an iterator to the actual edge
656  const typename std::map<EdgeId, EdgeType>::const_iterator edgeIter =
657  this->edges.find(*edgIt);
658 
659  // Check if the edge has the correct source and destination.
660  if (edgeIter != this->edges.end() &&
661  edgeIter->second.From(_sourceId) == _destId)
662  {
663  assert(edgeIter->second.To(_destId) == _sourceId);
664  return edgeIter->second;
665  }
666  }
667 
668  return EdgeType::NullEdge;
669  }
670 
675  public: const EdgeType &EdgeFromId(const EdgeId &_id) const
676  {
677  auto iter = this->edges.find(_id);
678  if (iter == this->edges.end())
679  return EdgeType::NullEdge;
680 
681  return iter->second;
682  }
683 
689  public: template<typename VV, typename EE, typename EEdgeType>
690  friend std::ostream &operator<<(std::ostream &_out,
691  const Graph<VV, EE, EEdgeType> &_g);
692 
695  private: VertexId &NextVertexId()
696  {
697  while (this->vertices.find(this->nextVertexId) != this->vertices.end()
698  && this->nextVertexId < MAX_UI64)
699  {
700  ++this->nextVertexId;
701  }
702 
703  return this->nextVertexId;
704  }
705 
708  private: VertexId &NextEdgeId()
709  {
710  while (this->edges.find(this->nextEdgeId) != this->edges.end() &&
711  this->nextEdgeId < MAX_UI64)
712  {
713  ++this->nextEdgeId;
714  }
715 
716  return this->nextEdgeId;
717  }
718 
720  protected: VertexId nextVertexId = 0u;
721 
723  protected: VertexId nextEdgeId = 0u;
724 
726  private: std::map<VertexId, Vertex<V>> vertices;
727 
729  private: std::map<EdgeId, EdgeType> edges;
730 
736  private: std::map<VertexId, EdgeId_S> adjList;
737 
740  };
741 
744  template<typename VV, typename EE>
746  const Graph<VV, EE, UndirectedEdge<EE>> &_g)
747  {
748  _out << "graph {" << std::endl;
749 
750  // All vertices with the name and Id as a "label" attribute.
751  for (auto const &vertexMap : _g.Vertices())
752  {
753  auto vertex = vertexMap.second.get();
754  _out << vertex;
755  }
756 
757  // All edges.
758  for (auto const &edgeMap : _g.Edges())
759  {
760  auto edge = edgeMap.second.get();
761  _out << edge;
762  }
763 
764  _out << "}" << std::endl;
765 
766  return _out;
767  }
768 
771  template<typename VV, typename EE>
773  const Graph<VV, EE, DirectedEdge<EE>> &_g)
774  {
775  _out << "digraph {" << std::endl;
776 
777  // All vertices with the name and Id as a "label" attribute.
778  for (auto const &vertexMap : _g.Vertices())
779  {
780  auto vertex = vertexMap.second.get();
781  _out << vertex;
782  }
783 
784  // All edges.
785  for (auto const &edgeMap : _g.Edges())
786  {
787  auto edge = edgeMap.second.get();
788  _out << edge;
789  }
790 
791  _out << "}" << std::endl;
792 
793  return _out;
794  }
795 
798  template<typename V, typename E>
800 
803  template<typename V, typename E>
805 }
806 }
807 }
808 }
809 #endif
VertexId Id() const
Get the vertex Id.
Definition: gz/math/graph/Vertex.hh:88
static const uint64_t MAX_UI64
64bit unsigned integer maximum value
Definition: gz/math/Helpers.hh:339
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
STL class.
A directed edge represents a connection between two vertices. The connection is unidirectional,...
Definition: gz/math/graph/Edge.hh:267
A vertex of a graph. It stores user information, an optional name, and keeps an internal unique Id....
Definition: gz/math/graph/Vertex.hh:54
friend std::ostream & operator<<(std::ostream &_out, const Graph< VV, EE, EEdgeType > &_g)
Stream insertion operator. The output uses DOT graph description language.
A generic graph class. Both vertices and edges can store user information. A vertex could be created ...
Definition: gz/math/graph/Graph.hh:102
T move(T... args)
std::ostream & operator<<(std::ostream &_out, const Graph< VV, EE, DirectedEdge< EE >> &_g)
Partial template specification for directed edges.
Definition: gz/math/graph/Graph.hh:772
const EdgeRef_M< EdgeType > IncidentsFrom(const Vertex< V > &_vertex) const
Get the set of outgoing edges from a given vertex.
Definition: gz/math/graph/Graph.hh:451
STL class.
T find(T... args)
const VertexRef_M< V > Vertices(const std::string &_name) const
The collection of all vertices in the graph with name == _name.
Definition: gz/math/graph/Graph.hh:193
Vertex< V > & VertexFromId(const VertexId &_id)
Get a mutable reference to a vertex using its Id.
Definition: gz/math/graph/Graph.hh:623
T emplace(T... args)
size_t InDegree(const VertexId &_vertex) const
Get the number of edges incident to a vertex.
Definition: gz/math/graph/Graph.hh:392
VertexId nextEdgeId
The next edge Id to be assigned to a new edge.
Definition: gz/math/graph/Graph.hh:723
VertexRef_M< V > AdjacentsFrom(const VertexId &_vertex) const
Get all vertices that are directly connected with one edge from a given vertex. In other words,...
Definition: gz/math/graph/Graph.hh:292
VertexId nextVertexId
The next vertex Id to be assigned to a new vertex.
Definition: gz/math/graph/Graph.hh:720
bool RemoveEdge(const EdgeId &_edge)
Remove an existing edge from the graph. After the removal, it won't be possible to reach any of the v...
Definition: gz/math/graph/Graph.hh:572
size_t RemoveVertices(const std::string &_name)
Remove all vertices with name == _name.
Definition: gz/math/graph/Graph.hh:553
static const VertexId kNullId
Represents an invalid Id.
Definition: gz/math/graph/Vertex.hh:48
size_t InDegree(const Vertex< V > &_vertex) const
Get the number of edges incident to a vertex.
Definition: gz/math/graph/Graph.hh:400
EdgeType & AddEdge(const VertexId_P &_vertices, const E &_data, const double _weight=1.0)
Add a new edge to the graph.
Definition: gz/math/graph/Graph.hh:211
Graph(const std::vector< Vertex< V >> &_vertices, const std::vector< EdgeInitializer< E >> &_edges)
Constructor.
Definition: gz/math/graph/Graph.hh:110
An undirected edge represents a connection between two vertices. The connection is bidirectional,...
Definition: gz/math/graph/Edge.hh:204
const EdgeRef_M< EdgeType > Edges() const
The collection of all edges in the graph.
Definition: gz/math/graph/Graph.hh:266
bool RemoveVertex(const VertexId &_vertex)
Remove an existing vertex from the graph.
Definition: gz/math/graph/Graph.hh:504
Vertex< V > & AddVertex(const std::string &_name, const V &_data, const VertexId &_id=kNullId)
Add a new vertex to the graph.
Definition: gz/math/graph/Graph.hh:137
T erase(T... args)
Used in the Graph constructors for uniform initialization.
Definition: gz/math/graph/Edge.hh:44
Graph()=default
Default constructor.
STL class.
VertexRef_M< V > AdjacentsTo(const Vertex< V > &_vertex) const
Get all vertices that are directly connected with one edge to a given vertex. In other words,...
Definition: gz/math/graph/Graph.hh:384
bool RemoveVertex(Vertex< V > &_vertex)
Remove an existing vertex from the graph.
Definition: gz/math/graph/Graph.hh:545
T equal_range(T... args)
VertexRef_M< V > AdjacentsTo(const VertexId &_vertex) const
Get all vertices that are directly connected with one edge to a given vertex. In other words,...
Definition: gz/math/graph/Graph.hh:351
size_t OutDegree(const VertexId &_vertex) const
Get the number of edges incident from a vertex.
Definition: gz/math/graph/Graph.hh:408
T endl(T... args)
T begin(T... args)
const EdgeRef_M< EdgeType > IncidentsFrom(const VertexId &_vertex) const
Get the set of outgoing edges from a given vertex.
Definition: gz/math/graph/Graph.hh:426
EdgeType & LinkEdge(const EdgeType &_edge)
Links an edge to the graph. This function verifies that the edge's two vertices exist in the graph,...
Definition: gz/math/graph/Graph.hh:235
T insert(T... args)
VertexRef_M< V > AdjacentsFrom(const Vertex< V > &_vertex) const
Get all vertices that are directly connected with one edge from a given vertex. In other words,...
Definition: gz/math/graph/Graph.hh:331
T count(T... args)
size_t OutDegree(const Vertex< V > &_vertex) const
Get the number of edges incident from a vertex.
Definition: gz/math/graph/Graph.hh:416
const Vertex< V > & VertexFromId(const VertexId &_id) const
Get a reference to a vertex using its Id.
Definition: gz/math/graph/Graph.hh:610
T make_pair(T... args)
T end(T... args)
uint64_t EdgeId
The unique Id for an edge.
Definition: gz/math/graph/Edge.hh:40
bool RemoveEdge(EdgeType &_edge)
Remove an existing edge from the graph. After the removal, it won't be possible to reach any of the v...
Definition: gz/math/graph/Graph.hh:601
const EdgeRef_M< EdgeType > IncidentsTo(const VertexId &_vertex) const
Get the set of incoming edges to a given vertex.
Definition: gz/math/graph/Graph.hh:462
bool Empty() const
Get whether the graph is empty.
Definition: gz/math/graph/Graph.hh:496
const EdgeType & EdgeFromId(const EdgeId &_id) const
Get a reference to an edge using its Id.
Definition: gz/math/graph/Graph.hh:675
const EdgeRef_M< EdgeType > IncidentsTo(const Vertex< V > &_vertex) const
Get the set of incoming edges to a given vertex.
Definition: gz/math/graph/Graph.hh:487
uint64_t VertexId
The unique Id of each vertex.
Definition: gz/math/graph/Vertex.hh:41
std::ostream & operator<<(std::ostream &_out, const Graph< VV, EE, UndirectedEdge< EE >> &_g)
Partial template specification for undirected edges.
Definition: gz/math/graph/Graph.hh:745
const EdgeType & EdgeFromVertices(const VertexId _sourceId, const VertexId _destId) const
Get a reference to an edge based on two vertices. A NullEdge object reference is returned if an edge ...
Definition: gz/math/graph/Graph.hh:640
STL class.
std::set< EdgeId > EdgeId_S
Definition: gz/math/graph/Edge.hh:192
T cref(T... args)
const VertexRef_M< V > Vertices() const
The collection of all vertices in the graph.
Definition: gz/math/graph/Graph.hh:181