graph_handler.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  Copyright (c) 2012-, 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.
 *
 * $Id$
 *
 */

#ifndef PCL_GRAPH_HANDLER_H_
#define PCL_GRAPH_HANDLER_H_

#include <pcl/registration/vertex_estimates.h>
#include <pcl/registration/edge_measurements.h>
#include <pcl/exceptions.h>
#include "boost/graph/graph_traits.hpp"

namespace pcl
{
  namespace registration
  {
    template <typename GraphT>
    class GraphHandler
    {
      public:
        typedef boost::shared_ptr<GraphHandler<GraphT> > Ptr;
        typedef boost::shared_ptr<const GraphHandler<GraphT> > ConstPtr;
        typedef boost::shared_ptr<GraphT> GraphPtr;
        typedef boost::shared_ptr<const GraphT> GraphConstPtr;

        typedef typename boost::graph_traits<GraphT>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<GraphT>::edge_descriptor Edge;

        GraphHandler () : graph_impl_ (new GraphT ())
        {
          if (!init ())
            throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::GraphHandler ()", __LINE__);
        }

        ~GraphHandler ()
        {
          deinit ();
        }

        inline void
        clear ()
        {
          deinit ();
          graph_impl_.reset (new GraphT ());
          if (!init ())
            throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::clear ()", __LINE__);
        }

        inline GraphConstPtr
        getGraph () const
        {
          return graph_impl_;
        }

        inline GraphPtr
        getGraph ()
        {
          return graph_impl_;
        }

        template <class PointT> inline Vertex
        addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud, const Eigen::Matrix4f& pose)
        {
          return add_vertex (PoseEstimate<PointT> (cloud, pose), *graph_impl_);
        }

        template <class EstimateT> inline Vertex
        addGenericVertex (const EstimateT& estimate)
        {
          return add_vertex (estimate, *graph_impl_);
        }

        template <class InformationT> inline Edge
        addPoseConstraint ( const Vertex& v_start, const Vertex& v_end,
                            const Eigen::Matrix4f& relative_transformation,
                            const InformationT& information_matrix)
        {
          return add_edge ( PoseMeasurement<Vertex, InformationT> (v_start, v_end, relative_transformation, information_matrix),
                            *graph_impl_);
        }

        template <class MeasurementT> inline Edge
        addGenericConstraint (const MeasurementT& measurement)
        {
          return add_edge (measurement, *graph_impl_);
        }

        inline void
        removeVertex (const Vertex& v)
        {
          remove_vertex (v.v_, *graph_impl_);
        }

        inline void
        removeConstraint (const Edge& e)
        {
          remove_edge(e.e_, *graph_impl_);
        }

      protected:
        inline bool
        init ()
        {
          return true;
        }

        inline bool
        deinit ()
        {
          return true;
        }

      private:
        GraphPtr graph_impl_;
    };
  }
}

#endif // PCL_GRAPH_HANDLER_H_