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 
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 
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 
395  : Base(p->size())
396  , m_point_cloud(p)
397  {
398  }
399 
407  : Base(num_vertices)
408  , m_point_cloud(new point_cloud_type(num_vertices, 1))
409  {
410  }
411 
425  {
426  *this = x;
427  }
428 
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);
449  m_point_cloud.reset(new point_cloud_type(*x.m_point_cloud));
450  }
451  return (*this);
452  }
453 
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 
520 
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 
547 
549  // This is public for the same reasons as everything in `boost::graph`
550  // is public.
552 
553  };
554 
556  template <typename Graph>
558  {
560  typedef typename Graph::point_type point_type;
562  typedef typename Graph::point_cloud_type point_cloud_type;
565  typedef typename Graph::point_cloud_ptr point_cloud_ptr;
568  typedef typename Graph::point_cloud_const_ptr point_cloud_const_ptr;
569  };
570 
572  template <typename Graph>
573  struct point_cloud_graph_traits<boost::subgraph<Graph> > : point_cloud_graph_traits<Graph>
574  { };
575 
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 
708  template <PCG_PARAMS>
709  inline typename pcl::PointCloud<P>::Ptr
711  {
712  return (g.m_point_cloud);
713  }
714 
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 
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 
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 
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 
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
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:35
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
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
pcl::graph::point_cloud_graph::edges_size_type
Base::edges_size_type edges_size_type
Definition: point_cloud_graph.h:372