point_cloud_graph.h
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_GRAPH_POINT_CLOUD_GRAPH_H
39 #define PCL_GRAPH_POINT_CLOUD_GRAPH_H
40 
41 #include <boost/graph/adjacency_list.hpp>
42 #include <boost/graph/subgraph.hpp>
43 
44 #include <pcl/point_cloud.h>
45 #include <pcl/point_types.h>
46 #include <pcl/PointIndices.h>
47 
48 /** \class pcl::graph::point_cloud_graph
49  *
50  * A sibling of `boost::adjacency_list` with PCL points bundled in vertices and copy-free access to them as a PCL point
51  * cloud.
52  *
53  *
54  * # Motivation #
55  *
56  *
57  * Boost Graph Library has a concept of [bundled properties][BundledProperties], i.e. custom data types that may be
58  * stored in graph vertices and conveniently accessed with `operator[]`. In the context of PCL it makes a lot of sense
59  * to have PCL points bundled in graph vertices. The following specialization of [boost::adjacency_list][AdjacencyList]
60  * achieves that:
61  *
62  * ~~~cpp
63  * typedef
64  * boost::adjacency_list<
65  * boost::vecS // Represent edge-list with std::vector
66  * , boost::vecS // Represent vertex-list with std::vector
67  * , boost::undirectedS // Undirected graph
68  * , pcl::PointXYZ> // Bundle XYZ points in vertices
69  * Graph
70  * ~~~
71  *
72  * A graph-based algorithm can easily access points associated with graph vertices:
73  *
74  * ~~~cpp
75  * Graph graph;
76  * Graph::vertex_descriptor v1 = boost::add_vertex (graph);
77  * Graph::vertex_descriptor v2 = boost::add_vertex (graph);
78  * graph[v1] = pcl::PointXYZ (1, 2, 3);
79  * graph[v2].x = graph[v1].x;
80  * ~~~
81  *
82  * The problem though is that there is no efficient way to put the point cloud data inside the graph and retrieve it
83  * back. Suppose that in some application there is a point cloud that should first be filtered using some PCL tools
84  * (e.g. pcl::PassThrough filter), then a graph-based algorithm should be applied to it, and finally some more
85  * processing using PCL tools is required. The user will need to copy all the points of the original point cloud
86  * one-by-one to the graph, run the algorithm, and then copy it back one-by-one again.
87  *
88  *
89  * # Solution #
90  *
91  *
92  * point_cloud_graph resolves this issue by storing bundled points internally as a PCL point cloud. In fact, it stores
93  * a shared pointer to a point cloud, so input and output of points is really copy-free. One can create a new graph
94  * based on an existing point cloud:
95  *
96  * ~~~cpp
97  * // Create a point cloud with 10 points
98  * pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> (10, 1);
99  * // Create a graph based on the cloud
100  * pcl::graph::point_cloud_graph<pcl::PointXYZ> graph (cloud);
101  * // The graph will have a vertex for each point of the original cloud
102  * assert (10 == boost::num_vertices (graph));
103  * // The points may be accessed using operator[]
104  * graph[1].x = 14;
105  * // The graph shares data with the original point cloud, so modifying a bundled point
106  * // changes the corresponding point in the original cloud
107  * assert (14 == cloud->points[1].x);
108  * ~~~
109  *
110  * The internal point cloud as a whole may be accessed using `pcl::graph::point_cloud (Graph& g)`:
111  *
112  * ~~~cpp
113  * // Perform some graph-based algorithm on point cloud graph
114  * // ...
115  * // Retrieve the bundled data as a point cloud
116  * pcl::PointCloud<pcl::PointXYZ>::Ptr data = pcl::graph::point_cloud (graph);
117  * // Continue to work with the data
118  * pcl::io::savePCDFile ("output.pcd", *data);
119  * ~~~
120  *
121  * Despite the fact that the point data are stored in a PCL point cloud, all the standard BGL features associated with
122  * bundled properties are supported, including `operator[]` and `get`/`put` access.
123  *
124  * [BundledProperties]: http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/bundles.html
125  *
126  *
127  * ## Comparison with [boost::adjacency_list][AdjacencyList] ##
128  *
129  *
130  * point_cloud_graph uses the same back-bone implementation as [boost::adjacency_list][AdjacencyList] and therefore
131  * models the same concepts and may be used everywhere `boost::adjacency_list` could. In particular, the very same
132  * functions could be used to add and remove vertices, access properties, iterate over edges, etc.
133  *
134  * As `boost::adjacency_list`, point_cloud_graph allows to configure itself with a number of template parameters. They
135  * are almost the same as in `boost::adjacency_list`, though there are several differences:
136  *
137  * 1. There is a required template parameter `PointT` which allows the user to select the type of PCL points to be
138  * bundled in graph vertices;
139  *
140  * Note that the `VertexProperty` parameter is still available for the user, so it is possible to additionally
141  * attach as many [internal properties][InternalProperties] (such as `vertex_color_t`) as needed.
142  *
143  * 2. The default value for `Directed` parameter is changed to `undirectedS` since it is more relevant in the context
144  * of point cloud processing;
145  *
146  * 3. The vertex-list of the graph is constrained to be `std::vector`. The user has no control over it as there is no
147  * `VertexList` template parameter;
148  *
149  * The choice of the container for the vertex-list determines the space complexity of the graph structure, as well
150  * as the time complexity of the various graph operations, see [using boost::adjacency_list][UsingAdjacencyList] for
151  * a more detailed discussion. In the case of point_cloud_graph the vertex bundled properties (PCL points) are
152  * stored in a PCL point cloud (which has `std::vector` behind the scenes), so it is only logical to have
153  * `std::vector` as the container type for the vertex-list itself.
154  *
155  * 4. There is no `GraphProperty` template parameter.
156  *
157  * [boost::adjacency_list][AdjacencyList] provides a facility to store an arbitrary piece of data associated with
158  * the graph as a whole (graph property). While being useful in certain cases, we do not consider this as a vital
159  * feature and have sacrificed it to support [boost::subgraph][Subgraph]. For more details please refer to the
160  * section about `boost::subgraph` below.
161  *
162  * [AdjacencyList]: http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/adjacency_list.html "adjacency_list"
163  * [InternalProperties]: http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/using_adjacency_list.html#sec:adjacency-list-properties
164  * [UsingAdjacencyList]: http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/using_adjacency_list.html#sec:choosing-graph-type
165  * [Subgraph]: http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/subgraph.html "subgraph"
166  *
167  *
168  * ## Compatibility with [boost::subgraph][Subgraph] ##
169  *
170  *
171  * `boost::subgraph` provides a convenient mechanism to keep track of a graph and its induced subgraphs. Two issues
172  * have been accounted for in order to allow smooth inter-operation between point_cloud_graph and `boost::subgraph`.
173  *
174  *
175  * ### Fixed interface ###
176  *
177  *
178  * `boost::subgraph` is a template class that is *wrapped around* a graph class rather than a class that *inherits*
179  * from a graph class. Therefore if we want it to be possible to use point_cloud_graph either way (plain or wrapped by
180  * `boost::subgraph`), we have to limit ourselves to the interface exposed by `boost::subgraph`. In particular, we can
181  * not have a PCL point cloud-based constructor for point_cloud_graph because (for apparent reasons) `boost::subgraph`
182  * does not have it. However, the ability to construct a point_cloud_graph from an existing PCL point cloud is an
183  * essential feature. We noted that `boost::subgraph` has a GraphProperty-based constructor that could be re-used as a
184  * desired PCL point cloud-based constructor if we alias GraphProperty to PCL point cloud. Thus GraphProperty is used
185  * to support internal mechanics of point_cloud_graph and is no longer available to the user.
186  *
187  *
188  * ### Lack of internal storage for points ###
189  *
190  *
191  * Recall that point_cloud_graph stores the points bundled in its vertices inside a PCL point cloud. It could be
192  * conveniently retrieved using `pcl::graph::point_cloud (Graph& g)` and then used in PCL algorithms.
193  * `boost::subgraph`, however, is designed in such a way that it stores the indices of the vertices and edges of the
194  * parent (root) graph that belong to it, but not the data associated with these vertices and edges. In other words, a
195  * hierarchy of subgraphs (each of which represents a particular subset of vertices and edges of the root graph) shares
196  * common vertex/edge data, and all these data are contained inside the root graph. Consequently, there does not exist
197  * a PCL point cloud for each subgraph, but rather a single PCL point cloud for the root graph. This raises a question
198  * as of how the `pcl::graph::point_cloud (Subgraph& g)` function should be implemented.
199  *
200  * One option is to construct a new PCL point cloud and fill it with subgraph's points. This, however, would be
201  * inconsistent with the behavior of `pcl::graph::point_cloud (Graph& g)`, which simply returns a pointer (meaning it
202  * is *O(1)* operation) that could be used to read and modify the points of the graph. Luckily, working with subsets of
203  * point clouds is such a common task in the PCL world that \ref pcl::PCLBase class (which lies at the top of the
204  * hierarchy of PCL algorithms) has a means for the user to supply an input point cloud *and* a vector of indices to
205  * which the processing should be limited to. Therefore, we decided that `pcl::graph::point_cloud (Subgraph& g)` should
206  * return the PCL point cloud of its root graph (that is, the whole set of points), and there should be an auxiliary
207  * function `pcl::graph::indices (Subgraph& g)` that returns indices of the points that belong to the subgraph.
208  *
209  *
210  * - - -
211  *
212  *
213  * # Specification #
214  *
215  *
216  * As was noted, point_cloud_graph is virtually the same as [boost::adjacency_list][AdjacencyList]. Please refer to
217  * its documentation. The specifications below only highlight the differences.
218  *
219  *
220  * ## Template parameters ##
221  *
222  *
223  * Parameter | Description | Default
224  * -------------- | ------------------------------------------------------------------------------------|--------------
225  * PointT | Type of PCL points bundled in graph vertices | --
226  * OutEdgeList | Selector for the container used to represent the edge-list for each of the vertices | `vecS`
227  * Directed | Selector to choose whether the graph is directed/undirected/bidirectional | `undirectedS`
228  * VertexProperty | For specifying internal vertex property storage (apart from bundled points) | `no_property`
229  * EdgeProperty | For specifying internal edge property storage | `no_property`
230  * EdgeList | Selector for the container used to represent the edge-list for the graph | `listS`
231  *
232  *
233  * ## Model of ##
234  *
235  *
236  * [PointCloudGraph](\ref boost::concepts::PointCloudGraphConcept),
237  * [VertexAndEdgeListGraph](http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/VertexAndEdgeListGraph.html),
238  * [VertexMutablePropertyGraph](http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/MutablePropertyGraph.html),
239  * [EdgeMutablePropertyGraph](http://www.boost.org/doc/libs/1_55_0/libs/graph/doc/MutablePropertyGraph.html),
240  * [CopyConstructible](http://www.boost.org/doc/libs/1_55_0/libs/utility/CopyConstructible.html),
241  * [Assignable](http://www.boost.org/doc/libs/1_55_0/libs/utility/Assignable.html)
242  *
243  * Probably it could also support [Serializable][Serialization], however this option has not been explored.
244  *
245  * [Serialization]: http://www.boost.org/doc/libs/1_55_0/libs/serialization/doc/index.html
246  *
247  *
248  * ## Associated types ##
249  *
250  *
251  * point_cloud_graph_traits structure provides a means to access the type of points bundled in the point_cloud_graph
252  * and, for convenience, the types of PCL point cloud and shared pointer to PCL point cloud of those points.
253  *
254  *
255  * ## Non-member functions ##
256  *
257  *
258  * ~~~cpp
259  * typename pcl::PointCloud<PointT>::Ptr
260  * pcl::graph::point_cloud (point_cloud_graph<PointT>& g);
261  * ~~~
262  *
263  * Return a shared pointer to the PCL point cloud stored internally. There are both `const` and `non-const` versions.
264  *
265  * ~~~cpp
266  * typename pcl::PointCloud<PointT>::Ptr
267  * pcl::graph::point_cloud (boost::subgraph<point_cloud_graph<PointT>>& g);
268  * ~~~
269  *
270  * Return a shared pointer to the PCL point cloud stored in the *root* graph. There are both `const` and `non-const`
271  * versions.
272  *
273  * ~~~cpp
274  * pcl::PointIndices::Ptr
275  * pcl::graph::indices (point_cloud_graph<PointT>& g);
276  * ~~~
277  *
278  * Return a shared pointer to a vector of indices of the points that belong to this graph. Since point_cloud_graph is
279  * a complete graph (i.e. it is not a subgraph of some other graph), the returned indices are guaranteed to be
280  * *[0..N-1]*, where *N* is the number of vertices. There are both `const`- and `non-const`- versions.
281  *
282  * ~~~cpp
283  * pcl::PointIndices::Ptr
284  * pcl::graph::indices (boost::subgraph<point_cloud_graph<PointT>>& g);
285  * ~~~
286  *
287  * Return a shared pointer to a vector of indices of the points of the root graph that belong to this subgraph. There
288  * are both `const`- and `non-const`- versions.
289  *
290  * \author Sergey Alexandrov
291  * \ingroup graph */
292 
293 namespace pcl::graph
294 {
295 
296  template <typename PointT,
297  typename OutEdgeListS = boost::vecS,
298  typename DirectedS = boost::undirectedS,
299  typename VertexProperty = boost::no_property,
300  typename EdgeProperty = boost::no_property,
301  typename EdgeListS = boost::listS>
303  : public boost::detail::adj_list_gen <
304  point_cloud_graph <
305  PointT
306  , OutEdgeListS
307  , DirectedS
308  , VertexProperty
309  , EdgeProperty
310  , EdgeListS
311  >
312  , boost::vecS
313  , OutEdgeListS
314  , DirectedS
315  , VertexProperty
316  , EdgeProperty
317  , typename pcl::PointCloud<PointT>::Ptr
318  , EdgeListS
319  >::type
320  // Not even sure what `maybe_named_graph` is, but without it
321  // CopyConstructible concept is not satisfied because of missing
322  // `vertex_by_property` (and some others) member functions
323  , public boost::graph::maybe_named_graph <
324  point_cloud_graph <
325  PointT
326  , OutEdgeListS
327  , DirectedS
328  , VertexProperty
329  , EdgeProperty
330  , EdgeListS
331  >
332  , typename boost::adjacency_list_traits <
333  OutEdgeListS
334  , boost::vecS
335  , DirectedS
336  , EdgeListS
337  >::vertex_descriptor
338  , VertexProperty
339  >
340  {
341 
342  private:
343 
344  typedef point_cloud_graph self;
345  typedef
346  typename boost::detail::adj_list_gen <
347  self
348  , boost::vecS
349  , OutEdgeListS
350  , DirectedS
351  , VertexProperty
352  , EdgeProperty
353  , typename pcl::PointCloud<PointT>::Ptr
354  , EdgeListS
355  >::type
356  Base;
357 
358  // Ensure that the user did not provide his own Bundle for vertices
360 
361  public:
362 
363  typedef typename pcl::PointCloud<PointT>::Ptr graph_property_type;
364  typedef typename pcl::PointCloud<PointT>::Ptr graph_bundled;
365  typedef typename Base::vertex_property_type vertex_property_type;
367  typedef typename Base::edge_property_type edge_property_type;
368  typedef typename Base::edge_bundled edge_bundled;
369 
370  typedef typename Base::stored_vertex stored_vertex;
371  typedef typename Base::vertices_size_type vertices_size_type;
372  typedef typename Base::edges_size_type edges_size_type;
373  typedef typename Base::degree_size_type degree_size_type;
374  typedef typename Base::vertex_descriptor vertex_descriptor;
375  typedef typename Base::edge_descriptor edge_descriptor;
376  typedef OutEdgeListS out_edge_list_selector;
377  typedef boost::vecS vertex_list_selector;
378  typedef DirectedS directed_selector;
379  typedef EdgeListS edge_list_selector;
380 
381  /// Type of PCL points bundled in graph vertices
383  typedef pcl::PointCloud<PointT> point_cloud_type;
384  typedef typename point_cloud_type::Ptr point_cloud_ptr;
385  typedef typename point_cloud_type::ConstPtr point_cloud_const_ptr;
386 
387  /** Construct a graph based on existing point cloud.
388  *
389  * The graph will have the same amount of vertices as the input point
390  * cloud has. The shared pointer will be stored internally so that the
391  * graph retains access to the point data.
392  *
393  * If the cloud is not given, then a new empty cloud will be created. */
395  : Base(p->size())
396  , m_point_cloud(p)
397  {
398  }
399 
400  /** Construct a graph with a given number of vertices.
401  *
402  * This constructor will create a new point cloud to store point data.
403  * The second parameter is completely ignored and is provided only to
404  * have the same interface as `adjacency_list`. */
407  : Base(num_vertices)
408  , m_point_cloud(new point_cloud_type(num_vertices, 1))
409  {
410  }
411 
412  /** Copy constructor.
413  *
414  * Acts just like the standard copy constructor of `adjacency_list`,
415  * i.e. copies vertex and edge set along with associated properties.
416  * Note that a **deep copy** of the underlying point cloud is made.
417  *
418  * This constructor reuses assignment operator. An alternative approach
419  * would be to use the copy constructor of the base class and then copy
420  * over the underlying point cloud, however the problem is that the
421  * base constructor uses `boost::add_vertex` function, which will
422  * attempt to push points to the point cloud, although it will not be
423  * initialized yet at that point in time (hence segfault). */
425  {
426  *this = x;
427  }
428 
429  /** Assignment operator.
430  *
431  * Acts just like the standard assignment operator of `adjacency_list`,
432  * i.e. copies vertex and edge set along with associated properties. Note
433  * that a **deep copy** of the underlying point cloud is made. */
436  {
437  if (&x != this)
438  {
439  /* The motivation behind this `reset` invocation is as follows. The
440  * `operator=` function of the base class (`vec_adj_list_impl`) will
441  * use the standard `add_vertex` to append each vertex of the source
442  * graph one after another. Apparently, this is not the fastest way
443  * to copy the underlying point cloud. Resetting the point cloud
444  * pointer effectively makes `added_vertex` a no-op, so the time is
445  * not wasted copying points one by one. Rather in the next line we
446  * make a complete copy with one call. */
447  m_point_cloud.reset();
448  Base::operator= (x);
450  }
451  return (*this);
452  }
453 
454  /** Remove all of the edges and vertices from the graph.
455  *
456  * Note that it wipes the underlying point cloud as well. */
457  void clear()
458  {
459  this->clearing_graph();
460  m_point_cloud->clear();
461  Base::clear();
462  }
463 
464  /* A note on the implementation of vertex addition/removal.
465  *
466  * In BGL vertices are added and removed to a graph via non-member
467  * functions `add_vertex` and `remove_vertex`. The implementation that is
468  * provided for `vec_adj_list_impl` (from which this class actually
469  * derives) does everything that is needed, except to adding/removing
470  * points from internal point cloud (for obvious reasons). In order to
471  * augment the behavior we could have specialized these functions for
472  * `point_cloud_graph` class, however that would involve copy-pasting of
473  * quite some code. Luckily, these functions have some sort of hooks
474  * which are member functions of graph object and got called in the end,
475  * namely `added_vertex` and `removing_vertex`. (In fact, they are
476  * designated to support some `named_graph` extension, but who cares.)
477  * Therefore these two hooks are implemented in `point_cloud_graph` to
478  * perform the desired point cloud maintenance. */
479 
480  void
482  {
483  if (m_point_cloud)
484  {
485  m_point_cloud->push_back(point_type());
486  }
487  }
488 
489  void
491  {
492  if (m_point_cloud)
493  {
494  m_point_cloud->erase(m_point_cloud->begin() + vertex);
495  }
496  }
497 
498  /* This second version of `removing_vertex` is to account for the
499  * change introduced in Boost 1.55, which added a second parameter to
500  * this function. The type of the second parameter is unimportant and
501  * likely is not present in earlier versions of Boost, hence generic
502  * templated version. */
503 
504  template <typename T> void
506  {
507  removing_vertex(vertex);
508  }
509 
510  /** \name Access to bundled vertex/edge properties.
511  *
512  * The `operators[]`'s in this group may be used to access the data
513  * bundled in vertices and edges of the graph. Implementation was
514  * directly copied from `adjacency_list`.
515  *
516  * Note that there is no operator access to the GraphProperty as there
517  * is no such thing in point_cloud_graph (see the corresponding
518  * section in the class description for more information). */
519 
520  ///@{
521 
524  {
525  return (boost::get(boost::vertex_bundle, *this)[v]);
526  }
527 
528  const vertex_bundled&
530  {
531  return (boost::get(boost::vertex_bundle, *this)[v]);
532  }
533 
534  edge_bundled&
536  {
537  return (boost::get(boost::edge_bundle, *this)[e]);
538  }
539 
540  const edge_bundled&
542  {
543  return (boost::get(boost::edge_bundle, *this)[e]);
544  }
545 
546  ///@}
547 
548  /// Storage for the internal cloud data.
549  // This is public for the same reasons as everything in `boost::graph`
550  // is public.
552 
553  };
554 
555  /** Traits struct to access the types associated with point_cloud_graph. */
556  template <typename Graph>
558  {
559  /// The type of PCL points bundled in vertices.
560  typedef typename Graph::point_type point_type;
561  /// The type of PCL point cloud the graph can be viewed as.
562  typedef typename Graph::point_cloud_type point_cloud_type;
563  /// The type of a shared pointer to PCL point cloud the graph can be
564  /// viewed as.
565  typedef typename Graph::point_cloud_ptr point_cloud_ptr;
566  /// The type of a shared pointer to const PCL point cloud the graph can be
567  /// viewed as.
568  typedef typename Graph::point_cloud_const_ptr point_cloud_const_ptr;
569  };
570 
571  /** Specialization for point_cloud_graphs wrapped in `boost::subgraph`. */
572  template <typename Graph>
573  struct point_cloud_graph_traits<boost::subgraph<Graph> > : point_cloud_graph_traits<Graph>
574  { };
575 
576  /** This class is to expose the point cloud stored in the point_cloud_graph
577  * as a vertex bundle property map.
578  *
579  * The code is based on `boost::vector_property_map`. It might be the case
580  * that some of the member functions are not needed. */
581  template <typename Graph>
583  : public boost::put_get_helper <
584  typename std::iterator_traits <
585  typename point_cloud_graph_traits<Graph>::point_cloud_type::iterator
586  >::reference
587  , point_cloud_property_map<Graph>
588  >
589  {
590 
591  public:
592 
593  typedef typename boost::property_traits<boost::identity_property_map>::key_type key_type;
596  typedef typename std::iterator_traits<iterator>::reference reference;
598  typedef boost::lvalue_property_map_tag category;
599 
600  point_cloud_property_map(const Graph* g, boost::vertex_bundle_t)
601  : data(g->m_point_cloud)
602  , index(boost::identity_property_map())
603  {
604  }
605 
606  iterator
608  {
609  return (data->begin());
610  }
611 
612  iterator
614  {
615  return (data->end());
616  }
617 
620  {
621  return (data->begin());
622  }
623 
625  storage_end() const
626  {
627  return (data->end());
628  }
629 
630  boost::identity_property_map&
632  {
633  return (index);
634  }
635 
636  const boost::identity_property_map&
638  {
639  return (index);
640  }
641 
642  reference
643  operator[](const key_type& v) const
644  {
645  return ((*data)[get(index, v)]);
646  }
647 
648  private:
649 
651  boost::identity_property_map index;
652 
653  };
654 
655 }
656 
657 #define PCG_PARAMS typename P, typename OEL, typename D, typename VP, typename EP, typename EL
658 #define PCG pcl::graph::point_cloud_graph<P, OEL, D, VP, EP, EL>
659 
660 namespace boost
661 {
662 
663  /* In `point_cloud_graph` we use a special kind of property map to access the
664  * vertex bundle. This specialization of the `property_map` traits struct
665  * makes sure that everyone (especially `get` function defined in
666  * 'boost/graph/detail/adjacency_list.hpp' which does the job of creating
667  * these maps) is aware of this fact. */
668 
669  template <PCG_PARAMS>
670  struct property_map<PCG, vertex_bundle_t>
671  {
673  typedef type const_type;
674  };
675 
676  /* The following two functions (source, target) are required by the
677  * EdgeListGraph concept. Implementation is the same as in `adjacency_list`. */
678 
679  template <typename Directed, typename Vertex, PCG_PARAMS>
680  inline Vertex
681  source(const detail::edge_base<Directed, Vertex>& e, const PCG&)
682  {
683  return e.m_source;
684  }
685 
686  template <typename Directed, typename Vertex, PCG_PARAMS>
687  inline Vertex
688  target(const detail::edge_base<Directed, Vertex>& e, const PCG&)
689  {
690  return e.m_target;
691  }
692 
693  template <PCG_PARAMS>
694  struct graph_mutability_traits<PCG>
695  {
696  typedef mutable_property_graph_tag category;
697  };
698 
699 } // namespace boost
700 
701 namespace pcl::graph
702 {
703 
704  /** Retrieve the point cloud stored in a point cloud graph.
705  *
706  * \author Sergey Alexandrov
707  * \ingroup graph */
708  template <PCG_PARAMS>
709  inline typename pcl::PointCloud<P>::Ptr
711  {
712  return (g.m_point_cloud);
713  }
714 
715  /** Retrieve the point cloud stored in a point cloud graph (const version).
716  *
717  * \author Sergey Alexandrov
718  * \ingroup graph */
719  template <PCG_PARAMS>
720  inline typename pcl::PointCloud<P>::ConstPtr
721  point_cloud(const PCG& g)
722  {
723  return (g.m_point_cloud);
724  }
725 
726  /** Retrieve the indices of the points of the point cloud stored in a point
727  * cloud graph that actually belong to the graph.
728  *
729  * Since point_cloud_graph always contain all the points that it stores,
730  * this function always returns a vector with indices from \c 0 to \c N-1,
731  * where \c N is the number of vertices (points) in the graph.
732  *
733  * \author Sergey Alexandrov
734  * \ingroup graph */
735  template <PCG_PARAMS>
736  inline pcl::PointIndices::Ptr
737  indices(const PCG& g)
738  {
739  pcl::PointIndices::Ptr indices(new pcl::PointIndices);
740  indices->indices.resize(g.m_point_cloud->size());
741  for (size_t i = 0; i < g.m_point_cloud->size(); ++i)
742  {
743  indices->indices[i] = i;
744  }
745  return (indices);
746  }
747 
748  /** Retrieve the point cloud stored in a point cloud (sub)graph.
749  *
750  * The behavior of this function will be different for root and child
751  * subgraphs. A root subgraph will return the point cloud stored in it,
752  * whereas a child subgraph will return the point cloud stored in its
753  * parent graph.
754  *
755  * \author Sergey Alexandrov
756  * \ingroup graph */
757  template <PCG_PARAMS>
758  inline typename pcl::PointCloud<P>::Ptr
759  point_cloud(boost::subgraph<PCG>& g)
760  {
761  return (g.root().m_graph.m_point_cloud);
762  }
763 
764  /** Retrieve the point cloud stored in a point cloud (sub)graph (const
765  * version).
766  *
767  * \author Sergey Alexandrov
768  * \ingroup graph */
769  template <PCG_PARAMS>
770  inline typename pcl::PointCloud<P>::ConstPtr
771  point_cloud(const boost::subgraph<PCG>& g)
772  {
773  return (g.root().m_graph.m_point_cloud);
774  }
775 
776  /** Retrieve the indices of the points of the point cloud stored in a point
777  * cloud (sub)graph that actually belong to the (sub)graph.
778  *
779  * A child subgraph contains only a subset of the vertices of the parent
780  * graph. This function provides a vector of indices of the vertices of the
781  * parent graph that belong to this subgraph as well. These vertex indices
782  * are valid point indices for the point cloud stored in the parent graph.
783  *
784  * \author Sergey Alexandrov
785  * \ingroup graph */
786  template <PCG_PARAMS>
787  inline pcl::PointIndices::Ptr
788  indices(const boost::subgraph<PCG>& g)
789  {
790  if (g.is_root())
791  {
792  return indices(g.m_graph);
793  }
794  pcl::PointIndices::Ptr indices(new pcl::PointIndices);
795  indices->indices.resize(boost::num_vertices(g));
796  for (size_t i = 0; i < boost::num_vertices(g); ++i)
797  {
798  indices->indices[i] = g.m_global_vertex[i];
799  }
800  return (indices);
801  }
802 
803 }
804 #endif /* PCL_GRAPH_POINT_CLOUD_GRAPH_H */
805 
boost::property_map< PCG, vertex_bundle_t >::const_type
type const_type
Definition: point_cloud_graph.h:673
pcl::graph::point_cloud_property_map::get_index_map
boost::identity_property_map & get_index_map()
Definition: point_cloud_graph.h:631
pcl::graph::point_cloud_property_map::operator[]
reference operator[](const key_type &v) const
Definition: point_cloud_graph.h:643
pcl::graph::point_cloud_graph::vertex_descriptor
Base::vertex_descriptor vertex_descriptor
Definition: point_cloud_graph.h:374
pcl::graph::point_cloud_graph::operator[]
const vertex_bundled & operator[](vertex_descriptor v) const
Definition: point_cloud_graph.h:529
boost::property_map< PCG, vertex_bundle_t >::type
pcl::graph::point_cloud_property_map< PCG > type
Definition: point_cloud_graph.h:672
PCG
#define PCG
Definition: point_cloud_graph.h:658
pcl::graph::point_cloud_graph::out_edge_list_selector
OutEdgeListS out_edge_list_selector
Definition: point_cloud_graph.h:376
pcl::graph::point_cloud_graph::edge_bundled
Base::edge_bundled edge_bundled
Definition: point_cloud_graph.h:368
pcl::graph::point_cloud_graph::vertices_size_type
Base::vertices_size_type vertices_size_type
Definition: point_cloud_graph.h:371
pcl::graph::point_cloud_property_map::const_iterator
point_cloud_graph_traits< Graph >::point_cloud_type::const_iterator const_iterator
Definition: point_cloud_graph.h:595
pcl::graph::point_cloud_graph::operator=
point_cloud_graph & operator=(const point_cloud_graph &x)
Assignment operator.
Definition: point_cloud_graph.h:435
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
pcl::graph::point_cloud_graph::graph_property_type
pcl::PointCloud< PointT >::Ptr graph_property_type
Definition: point_cloud_graph.h:363
pcl::graph::point_cloud_property_map::storage_begin
const_iterator storage_begin() const
Definition: point_cloud_graph.h:619
boost
Definition: ApplicationOptions.h:37
pcl::graph::point_cloud_graph::m_point_cloud
point_cloud_ptr m_point_cloud
Storage for the internal cloud data.
Definition: point_cloud_graph.h:551
pcl::graph::point_cloud_graph::operator[]
edge_bundled & operator[](edge_descriptor e)
Definition: point_cloud_graph.h:535
pcl::graph::point_cloud_graph::removing_vertex
void removing_vertex(vertex_descriptor vertex)
Definition: point_cloud_graph.h:490
pcl::graph::point_cloud_graph_traits::point_cloud_type
Graph::point_cloud_type point_cloud_type
The type of PCL point cloud the graph can be viewed as.
Definition: point_cloud_graph.h:562
pcl::graph::point_cloud_graph::graph_bundled
pcl::PointCloud< PointT >::Ptr graph_bundled
Definition: point_cloud_graph.h:364
pcl::graph::point_cloud_graph::edge_property_type
Base::edge_property_type edge_property_type
Definition: point_cloud_graph.h:367
pcl::graph::point_cloud_property_map::storage_end
iterator storage_end()
Definition: point_cloud_graph.h:613
armarx::navigation::util::geometry::point_type
boost::geometry::model::d2::point_xy< float > point_type
Definition: geometry.h:33
pcl::graph::point_cloud_graph::point_cloud_const_ptr
point_cloud_type::ConstPtr point_cloud_const_ptr
Definition: point_cloud_graph.h:385
pcl::graph
Definition: common.h:45
pcl::graph::point_cloud_graph::point_cloud_graph
point_cloud_graph(const point_cloud_ptr &p=point_cloud_ptr(new point_cloud_type))
Construct a graph based on existing point cloud.
Definition: point_cloud_graph.h:394
boost::graph_mutability_traits< PCG >::category
mutable_property_graph_tag category
Definition: point_cloud_graph.h:696
pcl::graph::point_cloud_graph::vertex_bundled
PointT vertex_bundled
Definition: point_cloud_graph.h:366
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:926
pcl::graph::indices
pcl::PointIndices::Ptr indices(const PCG &g)
Retrieve the indices of the points of the point cloud stored in a point cloud graph that actually bel...
Definition: point_cloud_graph.h:737
pcl::graph::point_cloud_graph_traits::point_cloud_ptr
Graph::point_cloud_ptr point_cloud_ptr
The type of a shared pointer to PCL point cloud the graph can be viewed as.
Definition: point_cloud_graph.h:565
pcl::graph::point_cloud_graph::added_vertex
void added_vertex(vertex_descriptor)
Definition: point_cloud_graph.h:481
pcl::graph::point_cloud_graph::degree_size_type
Base::degree_size_type degree_size_type
Definition: point_cloud_graph.h:373
pcl::graph::point_cloud_graph::point_type
PointT point_type
Type of PCL points bundled in graph vertices.
Definition: point_cloud_graph.h:382
pcl::graph::point_cloud_property_map::reference
std::iterator_traits< iterator >::reference reference
Definition: point_cloud_graph.h:596
pcl::graph::point_cloud_property_map::value_type
point_cloud_graph_traits< Graph >::point_type value_type
Definition: point_cloud_graph.h:597
armarx::PointT
pcl::PointXYZRGBL PointT
Definition: Common.h:28
pcl::graph::point_cloud_property_map::category
boost::lvalue_property_map_tag category
Definition: point_cloud_graph.h:598
pcl::graph::point_cloud_graph::vertex_property_type
Base::vertex_property_type vertex_property_type
Definition: point_cloud_graph.h:365
pcl::graph::point_cloud
pcl::PointCloud< P >::Ptr point_cloud(PCG &g)
Retrieve the point cloud stored in a point cloud graph.
Definition: point_cloud_graph.h:710
armarx::Graph
boost::subgraph< CloudGraph > Graph
Definition: Common.h:54
pcl::graph::point_cloud_property_map::key_type
boost::property_traits< boost::identity_property_map >::key_type key_type
Definition: point_cloud_graph.h:593
pcl::graph::point_cloud_graph::removing_vertex
void removing_vertex(vertex_descriptor vertex, T)
Definition: point_cloud_graph.h:505
boost::source
Vertex source(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:681
pcl::graph::point_cloud_graph_traits::point_type
Graph::point_type point_type
The type of PCL points bundled in vertices.
Definition: point_cloud_graph.h:560
pcl::graph::point_cloud_graph::edge_list_selector
EdgeListS edge_list_selector
Definition: point_cloud_graph.h:379
pcl::graph::point_cloud_graph::point_cloud_graph
point_cloud_graph(vertices_size_type num_vertices, const point_cloud_ptr &=point_cloud_ptr(new point_cloud_type))
Construct a graph with a given number of vertices.
Definition: point_cloud_graph.h:405
pcl::graph::point_cloud_graph_traits::point_cloud_const_ptr
Graph::point_cloud_const_ptr point_cloud_const_ptr
The type of a shared pointer to const PCL point cloud the graph can be viewed as.
Definition: point_cloud_graph.h:568
pcl::graph::point_cloud_graph
Definition: point_cloud_graph.h:302
pcl::graph::point_cloud_property_map::storage_begin
iterator storage_begin()
Definition: point_cloud_graph.h:607
pcl::graph::point_cloud_graph::clear
void clear()
Remove all of the edges and vertices from the graph.
Definition: point_cloud_graph.h:457
pcl::graph::point_cloud_graph::operator[]
vertex_bundled & operator[](vertex_descriptor v)
Definition: point_cloud_graph.h:523
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
pcl::graph::point_cloud_property_map::storage_end
const_iterator storage_end() const
Definition: point_cloud_graph.h:625
pcl::graph::point_cloud_graph::point_cloud_graph
point_cloud_graph(const point_cloud_graph &x)
Copy constructor.
Definition: point_cloud_graph.h:424
pcl::graph::point_cloud_graph::operator[]
const edge_bundled & operator[](edge_descriptor e) const
Definition: point_cloud_graph.h:541
pcl::graph::point_cloud_property_map::get_index_map
const boost::identity_property_map & get_index_map() const
Definition: point_cloud_graph.h:637
pcl::graph::point_cloud_graph::edge_descriptor
Base::edge_descriptor edge_descriptor
Definition: point_cloud_graph.h:375
pcl::graph::point_cloud_graph_traits
Traits struct to access the types associated with point_cloud_graph.
Definition: point_cloud_graph.h:557
pcl::graph::point_cloud_property_map::point_cloud_property_map
point_cloud_property_map(const Graph *g, boost::vertex_bundle_t)
Definition: point_cloud_graph.h:600
pcl::graph::point_cloud_graph::vertex_list_selector
boost::vecS vertex_list_selector
Definition: point_cloud_graph.h:377
pcl::graph::point_cloud_property_map::iterator
point_cloud_graph_traits< Graph >::point_cloud_type::iterator iterator
Definition: point_cloud_graph.h:594
T
float T
Definition: UnscentedKalmanFilterTest.cpp:35
pcl::graph::point_cloud_graph::directed_selector
DirectedS directed_selector
Definition: point_cloud_graph.h:378
pcl::graph::point_cloud_graph::point_cloud_ptr
point_cloud_type::Ptr point_cloud_ptr
Definition: point_cloud_graph.h:384
pcl::graph::point_cloud_graph::point_cloud_type
pcl::PointCloud< PointT > point_cloud_type
Definition: point_cloud_graph.h:383
pcl::graph::point_cloud_graph::stored_vertex
Base::stored_vertex stored_vertex
Definition: point_cloud_graph.h:370
pcl::graph::point_cloud_property_map
This class is to expose the point cloud stored in the point_cloud_graph as a vertex bundle property m...
Definition: point_cloud_graph.h:582
pcl::graph::point_cloud_graph::edges_size_type
Base::edges_size_type edges_size_type
Definition: point_cloud_graph.h:372