Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/graph_handler.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2011, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 #ifndef PCL_GRAPH_HANDLER_H_
00042 #define PCL_GRAPH_HANDLER_H_
00043 
00044 #include <pcl/registration/vertex_estimates.h>
00045 #include <pcl/registration/edge_measurements.h>
00046 #include <pcl/exceptions.h>
00047 #include "boost/graph/graph_traits.hpp"
00048 
00049 namespace pcl
00050 {
00051   namespace registration
00052   {
00053     /** \brief @b GraphHandler class is a wrapper for a general SLAM graph
00054       * The actual graph class must fulfil the following boost::graph concepts:
00055       * - BidirectionalGraph
00056       * - AdjacencyGraph
00057       * - VertexAndEdgeListGraph
00058       * - MutableGraph
00059       *
00060       * Other valid expressions:
00061       * - add_edge (m,g)          add a new edge according to the given measurement. Return type: std::pair<edge_descriptor, bool>
00062       * - add_vertex (e,g)        add a new vertex according to the given estimate. Return type: vertex_descriptor
00063       * - get_pose (v,g)          retrieve the pose estimate for v, if any. Return type: Eigen::Matrix4f
00064       * - get_cloud (v,g)         retrieve the cloud pointer associated to v, if any. Return type: pcl::PointCloud<PointT>::ConstPtr
00065       * - set_estimate (v,e,g)    set the estimate for an existing vertex. Return type: void.
00066       * - set_measurement (d,m,g) set the measurement for an existing edge. Return type: void.
00067       * Notation:
00068       * - m                       an edge measurement
00069       * - e                       a vertex estimate
00070       * - v                       a vertex
00071       * - d                       an edge
00072       * - g                       a graph
00073       * A valid graph implementation should accept at least the PoseEstimate estimate and the PoseMeasurement measurement
00074       *
00075       * If a specific graph implementation needs initialization and/or finalization,
00076       * specialize the protected methods init() and deinit() for your graph type
00077       *
00078       * \author Nicola Fioraio
00079       * \ingroup registration
00080       */
00081     template <typename GraphT>
00082     class GraphHandler
00083     {
00084       public:
00085         typedef boost::shared_ptr<GraphHandler<GraphT> > Ptr;
00086         typedef boost::shared_ptr<const GraphHandler<GraphT> > ConstPtr;
00087         typedef boost::shared_ptr<GraphT> GraphPtr;
00088         typedef boost::shared_ptr<const GraphT> GraphConstPtr;
00089 
00090         typedef typename boost::graph_traits<GraphT>::vertex_descriptor Vertex;
00091         typedef typename boost::graph_traits<GraphT>::edge_descriptor Edge;
00092 
00093         /** \brief Empty constructor. */
00094         GraphHandler () : graph_impl_ (new GraphT ())
00095         {
00096           if (!init ())
00097             throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::GraphHandler ()", __LINE__);
00098         }
00099 
00100         /** \brief Destructor. */
00101         ~GraphHandler ()
00102         {
00103           deinit ();
00104         }
00105 
00106         /** \brief Clear the graph */
00107         inline void
00108         clear ()
00109         {
00110           deinit ();
00111           graph_impl_.reset (new GraphT ());
00112           if (!init ())
00113             throw InitFailedException ("Graph Initialization Failed", __FILE__, "pcl::registration::GraphHandler::clear ()", __LINE__);
00114         }
00115 
00116         /** \brief Get a pointer to the BGL graph */
00117         inline GraphConstPtr
00118         getGraph () const
00119         {
00120           return graph_impl_;
00121         }
00122 
00123         /** \brief Get a pointer to the BGL graph */
00124         inline GraphPtr
00125         getGraph ()
00126         {
00127           return graph_impl_;
00128         }
00129 
00130         /** \brief Add a new point cloud to the graph and return the new vertex
00131           * \param cloud the new point cloud
00132           * \param pose the pose estimate
00133           * \return a reference to the new vertex
00134           */
00135         template <class PointT> inline Vertex
00136         addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud, const Eigen::Matrix4f& pose)
00137         {
00138           return add_vertex (PoseEstimate<PointT> (cloud, pose), *graph_impl_);
00139         }
00140 
00141         /** \brief Add a new generic vertex created according to the given estimate
00142           * \param estimate the parameters' estimate
00143           * \return a reference to the new vertex
00144           */
00145         template <class EstimateT> inline Vertex
00146         addGenericVertex (const EstimateT& estimate)
00147         {
00148           return add_vertex (estimate, *graph_impl_);
00149         }
00150 
00151         /** \brief Add a new constraint between two poses
00152           * \param v_start the first pose
00153           * \param v_end the second pose
00154           * \param relative_transformation the transformation from v_start to v_end
00155           * \param information_matrix the uncertainty
00156           * \return a reference to the new edge
00157           */
00158         template <class InformationT> inline Edge
00159         addPoseConstraint ( const Vertex& v_start, const Vertex& v_end,
00160                             const Eigen::Matrix4f& relative_transformation,
00161                             const InformationT& information_matrix)
00162         {
00163           return add_edge ( PoseMeasurement<Vertex, InformationT> (v_start, v_end, relative_transformation, information_matrix),
00164                             *graph_impl_);
00165         }
00166 
00167         /** \brief Add a generic constraint created according to the given measurement
00168           * \param measurement the measurement
00169           * \return a reference to the new edge
00170           */
00171         template <class MeasurementT> inline Edge
00172         addGenericConstraint (const MeasurementT& measurement)
00173         {
00174           return add_edge (measurement, *graph_impl_);
00175         }
00176 
00177         /** \brief Remove a vertex from the graph
00178           * \param v the vertex
00179           */
00180         inline void
00181         removeVertex (const Vertex& v)
00182         {
00183           remove_vertex (v.v_, *graph_impl_);
00184         }
00185 
00186         /** \brief Remove a constraint from the graph
00187           * \param e the edge
00188           */
00189         inline void
00190         removeConstraint (const Edge& e)
00191         {
00192           remove_edge(e.e_, *graph_impl_);
00193         }
00194 
00195       protected:
00196         /** \brief This method is called right after the creation of graph_impl_ */
00197         inline bool
00198         init ()
00199         {
00200           return true;
00201         }
00202 
00203         /** \brief This method is called when graph_impl_ is going to be destroyed */
00204         inline bool
00205         deinit ()
00206         {
00207           return true;
00208         }
00209 
00210       private:
00211         GraphPtr graph_impl_;
00212     };
00213   }
00214 }
00215 
00216 #endif // PCL_GRAPH_HANDLER_H_