point_cloud_graph.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2014-, Open Perception, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */
 
#ifndef PCL_GRAPH_POINT_CLOUD_GRAPH_H
#define PCL_GRAPH_POINT_CLOUD_GRAPH_H
 
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/subgraph.hpp>
 
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/PointIndices.h>
 
namespace pcl::graph
{
 
    template <typename PointT,
              typename OutEdgeListS = boost::vecS,
              typename DirectedS = boost::undirectedS,
              typename VertexProperty = boost::no_property,
              typename EdgeProperty = boost::no_property,
              typename EdgeListS = boost::listS>
    class point_cloud_graph
        : public boost::detail::adj_list_gen <
          point_cloud_graph <
          PointT
        , OutEdgeListS
        , DirectedS
        , VertexProperty
        , EdgeProperty
        , EdgeListS
          >
        , boost::vecS
        , OutEdgeListS
        , DirectedS
        , VertexProperty
        , EdgeProperty
        , typename pcl::PointCloud<PointT>::Ptr
        , EdgeListS
          >::type
    // Not even sure what `maybe_named_graph` is, but without it
    // CopyConstructible concept is not satisfied because of missing
    // `vertex_by_property` (and some others) member functions
        , public boost::graph::maybe_named_graph <
          point_cloud_graph <
          PointT
        , OutEdgeListS
        , DirectedS
        , VertexProperty
        , EdgeProperty
        , EdgeListS
          >
        , typename boost::adjacency_list_traits <
          OutEdgeListS
        , boost::vecS
        , DirectedS
        , EdgeListS
          >::vertex_descriptor
        , VertexProperty
          >
    {
 
    private:
 
        typedef point_cloud_graph self;
        typedef
        typename boost::detail::adj_list_gen <
        self
        , boost::vecS
        , OutEdgeListS
        , DirectedS
        , VertexProperty
        , EdgeProperty
        , typename pcl::PointCloud<PointT>::Ptr
        , EdgeListS
        >::type
        Base;
 
        // Ensure that the user did not provide his own Bundle for vertices
        BOOST_STATIC_ASSERT((boost::is_same<typename Base::vertex_bundled, boost::no_property>::value));
 
    public:
 
        typedef typename pcl::PointCloud<PointT>::Ptr graph_property_type;
        typedef typename pcl::PointCloud<PointT>::Ptr graph_bundled;
        typedef typename Base::vertex_property_type vertex_property_type;
        typedef PointT vertex_bundled;
        typedef typename Base::edge_property_type edge_property_type;
        typedef typename Base::edge_bundled edge_bundled;
 
        typedef typename Base::stored_vertex stored_vertex;
        typedef typename Base::vertices_size_type vertices_size_type;
        typedef typename Base::edges_size_type edges_size_type;
        typedef typename Base::degree_size_type degree_size_type;
        typedef typename Base::vertex_descriptor vertex_descriptor;
        typedef typename Base::edge_descriptor edge_descriptor;
        typedef OutEdgeListS out_edge_list_selector;
        typedef boost::vecS vertex_list_selector;
        typedef DirectedS directed_selector;
        typedef EdgeListS edge_list_selector;
 
        typedef PointT point_type;
        typedef pcl::PointCloud<PointT> point_cloud_type;
        typedef typename point_cloud_type::Ptr point_cloud_ptr;
        typedef typename point_cloud_type::ConstPtr point_cloud_const_ptr;
 
        point_cloud_graph(const point_cloud_ptr& p = point_cloud_ptr(new point_cloud_type))
            : Base(p->size())
            , m_point_cloud(p)
        {
        }
 
        point_cloud_graph(vertices_size_type num_vertices,
                          const point_cloud_ptr& = point_cloud_ptr(new point_cloud_type))
            : Base(num_vertices)
            , m_point_cloud(new point_cloud_type(num_vertices, 1))
        {
        }
 
        point_cloud_graph(const point_cloud_graph& x)
        {
            *this = x;
        }
 
        point_cloud_graph&
        operator= (const point_cloud_graph& x)
        {
            if (&x != this)
            {
                /* The motivation behind this `reset` invocation is as follows. The
                 * `operator=` function of the base class (`vec_adj_list_impl`) will
                 * use the standard `add_vertex` to append each vertex of the source
                 * graph one after another. Apparently, this is not the fastest way
                 * to copy the underlying point cloud. Resetting the point cloud
                 * pointer effectively makes `added_vertex` a no-op, so the time is
                 * not wasted copying points one by one. Rather in the next line we
                 * make a complete copy with one call. */
                m_point_cloud.reset();
                Base::operator= (x);
                m_point_cloud.reset(new point_cloud_type(*x.m_point_cloud));
            }
            return (*this);
        }
 
        void clear()
        {
            this->clearing_graph();
            m_point_cloud->clear();
            Base::clear();
        }
 
        /* A note on the implementation of vertex addition/removal.
         *
         * In BGL vertices are added and removed to a graph via non-member
         * functions `add_vertex` and `remove_vertex`. The implementation that is
         * provided for `vec_adj_list_impl` (from which this class actually
         * derives) does everything that is needed, except to adding/removing
         * points from internal point cloud (for obvious reasons). In order to
         * augment the behavior we could have specialized these functions for
         * `point_cloud_graph` class, however that would involve copy-pasting of
         * quite some code. Luckily, these functions have some sort of hooks
         * which are member functions of graph object and got called in the end,
         * namely `added_vertex` and `removing_vertex`. (In fact, they are
         * designated to support some `named_graph` extension, but who cares.)
         * Therefore these two hooks are implemented in `point_cloud_graph` to
         * perform the desired point cloud maintenance. */
 
        void
        added_vertex(vertex_descriptor)
        {
            if (m_point_cloud)
            {
                m_point_cloud->push_back(point_type());
            }
        }
 
        void
        removing_vertex(vertex_descriptor vertex)
        {
            if (m_point_cloud)
            {
                m_point_cloud->erase(m_point_cloud->begin() + vertex);
            }
        }
 
        /* This second version of `removing_vertex` is to account for the
         * change introduced in Boost 1.55, which added a second parameter to
         * this function. The type of the second parameter is unimportant and
         * likely is not present in earlier versions of Boost, hence generic
         * templated version. */
 
        template <typename T> void
        removing_vertex(vertex_descriptor vertex, T)
        {
            removing_vertex(vertex);
        }
 
 
        vertex_bundled&
        operator[](vertex_descriptor v)
        {
            return (boost::get(boost::vertex_bundle, *this)[v]);
        }
 
        const vertex_bundled&
        operator[](vertex_descriptor v) const
        {
            return (boost::get(boost::vertex_bundle, *this)[v]);
        }
 
        edge_bundled&
        operator[](edge_descriptor e)
        {
            return (boost::get(boost::edge_bundle, *this)[e]);
        }
 
        const edge_bundled&
        operator[](edge_descriptor e) const
        {
            return (boost::get(boost::edge_bundle, *this)[e]);
        }
 
 
        //  This is public for the same reasons as everything in `boost::graph`
        //  is public.
        point_cloud_ptr m_point_cloud;
 
    };
 
    template <typename Graph>
    struct point_cloud_graph_traits
    {
        typedef typename Graph::point_type point_type;
        typedef typename Graph::point_cloud_type point_cloud_type;
        typedef typename Graph::point_cloud_ptr point_cloud_ptr;
        typedef typename Graph::point_cloud_const_ptr point_cloud_const_ptr;
    };
 
    template <typename Graph>
    struct point_cloud_graph_traits<boost::subgraph<Graph> > : point_cloud_graph_traits<Graph>
    { };
 
    template <typename Graph>
    class point_cloud_property_map
        : public boost::put_get_helper <
          typename std::iterator_traits <
          typename point_cloud_graph_traits<Graph>::point_cloud_type::iterator
          >::reference
        , point_cloud_property_map<Graph>
          >
    {
 
    public:
 
        typedef typename boost::property_traits<boost::identity_property_map>::key_type key_type;
        typedef typename point_cloud_graph_traits<Graph>::point_cloud_type::iterator iterator;
        typedef typename point_cloud_graph_traits<Graph>::point_cloud_type::const_iterator const_iterator;
        typedef typename std::iterator_traits<iterator>::reference reference;
        typedef typename point_cloud_graph_traits<Graph>::point_type value_type;
        typedef boost::lvalue_property_map_tag category;
 
        point_cloud_property_map(const Graph* g, boost::vertex_bundle_t)
            : data(g->m_point_cloud)
            , index(boost::identity_property_map())
        {
        }
 
        iterator
        storage_begin()
        {
            return (data->begin());
        }
 
        iterator
        storage_end()
        {
            return (data->end());
        }
 
        const_iterator
        storage_begin() const
        {
            return (data->begin());
        }
 
        const_iterator
        storage_end() const
        {
            return (data->end());
        }
 
        boost::identity_property_map&
        get_index_map()
        {
            return (index);
        }
 
        const boost::identity_property_map&
        get_index_map() const
        {
            return (index);
        }
 
        reference
        operator[](const key_type& v) const
        {
            return ((*data)[get(index, v)]);
        }
 
    private:
 
        typename point_cloud_graph_traits<Graph>::point_cloud_ptr data;
        boost::identity_property_map index;
 
    };
 
}
 
#define PCG_PARAMS typename P, typename OEL, typename D, typename VP, typename EP, typename EL
#define PCG pcl::graph::point_cloud_graph<P, OEL, D, VP, EP, EL>
 
namespace boost
{
 
    /* In `point_cloud_graph` we use a special kind of property map to access the
     * vertex bundle. This specialization of the `property_map` traits struct
     * makes sure that everyone (especially `get` function defined in
     * 'boost/graph/detail/adjacency_list.hpp' which does the job of creating
     * these maps) is aware of this fact. */
 
    template <PCG_PARAMS>
    struct property_map<PCG, vertex_bundle_t>
    {
        typedef pcl::graph::point_cloud_property_map<PCG> type;
        typedef type const_type;
    };
 
    /* The following two functions (source, target) are required by the
     * EdgeListGraph concept. Implementation is the same as in `adjacency_list`. */
 
    template <typename Directed, typename Vertex, PCG_PARAMS>
    inline Vertex
    source(const detail::edge_base<Directed, Vertex>& e, const PCG&)
    {
        return e.m_source;
    }
 
    template <typename Directed, typename Vertex, PCG_PARAMS>
    inline Vertex
    target(const detail::edge_base<Directed, Vertex>& e, const PCG&)
    {
        return e.m_target;
    }
 
    template <PCG_PARAMS>
    struct graph_mutability_traits<PCG>
    {
        typedef mutable_property_graph_tag category;
    };
 
} // namespace boost
 
namespace pcl::graph
{
 
    template <PCG_PARAMS>
    inline typename pcl::PointCloud<P>::Ptr
    point_cloud(PCG& g)
    {
        return (g.m_point_cloud);
    }
 
    template <PCG_PARAMS>
    inline typename pcl::PointCloud<P>::ConstPtr
    point_cloud(const PCG& g)
    {
        return (g.m_point_cloud);
    }
 
    template <PCG_PARAMS>
    inline pcl::PointIndices::Ptr
    indices(const PCG& g)
    {
        pcl::PointIndices::Ptr indices(new pcl::PointIndices);
        indices->indices.resize(g.m_point_cloud->size());
        for (size_t i = 0; i < g.m_point_cloud->size(); ++i)
        {
            indices->indices[i] = i;
        }
        return (indices);
    }
 
    template <PCG_PARAMS>
    inline typename pcl::PointCloud<P>::Ptr
    point_cloud(boost::subgraph<PCG>& g)
    {
        return (g.root().m_graph.m_point_cloud);
    }
 
    template <PCG_PARAMS>
    inline typename pcl::PointCloud<P>::ConstPtr
    point_cloud(const boost::subgraph<PCG>& g)
    {
        return (g.root().m_graph.m_point_cloud);
    }
 
    template <PCG_PARAMS>
    inline pcl::PointIndices::Ptr
    indices(const boost::subgraph<PCG>& g)
    {
        if (g.is_root())
        {
            return indices(g.m_graph);
        }
        pcl::PointIndices::Ptr indices(new pcl::PointIndices);
        indices->indices.resize(boost::num_vertices(g));
        for (size_t i = 0; i < boost::num_vertices(g); ++i)
        {
            indices->indices[i] = g.m_global_vertex[i];
        }
        return (indices);
    }
 
}
#endif /* PCL_GRAPH_POINT_CLOUD_GRAPH_H */