Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/elch.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_ELCH_H_
00042 #define PCL_ELCH_H_
00043 
00044 #include <pcl/pcl_base.h>
00045 #include <pcl/point_types.h>
00046 #include <pcl/point_cloud.h>
00047 #include <pcl/registration/registration.h>
00048 #include <pcl/registration/boost.h>
00049 #include <pcl/registration/eigen.h>
00050 #include <pcl/registration/icp.h>
00051 #include <pcl/registration/boost_graph.h>
00052 
00053 namespace pcl
00054 {
00055   namespace registration
00056   {
00057     /** \brief @b ELCH (Explicit Loop Closing Heuristic) class
00058       * \author Jochen Sprickerhof
00059       * \ingroup registration
00060       */
00061     template <typename PointT>
00062     class ELCH : public PCLBase<PointT>
00063     {
00064       public:
00065         typedef boost::shared_ptr< ELCH<PointT> > Ptr;
00066         typedef boost::shared_ptr< const ELCH<PointT> > ConstPtr;
00067 
00068         typedef pcl::PointCloud<PointT> PointCloud;
00069         typedef typename PointCloud::Ptr PointCloudPtr;
00070         typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00071 
00072         struct Vertex
00073         {
00074           Vertex () : cloud () {}
00075           PointCloudPtr cloud;
00076         };
00077 
00078         /** \brief graph structure to hold the SLAM graph */
00079         typedef boost::adjacency_list<
00080           boost::listS, boost::eigen_vecS, boost::undirectedS,
00081           Vertex,
00082           boost::no_property>
00083         LoopGraph;
00084 
00085         typedef boost::shared_ptr< LoopGraph > LoopGraphPtr;
00086 
00087         typedef typename pcl::Registration<PointT, PointT> Registration;
00088         typedef typename Registration::Ptr RegistrationPtr;
00089         typedef typename Registration::ConstPtr RegistrationConstPtr;
00090 
00091         /** \brief Empty constructor. */
00092         ELCH () : 
00093           loop_graph_ (new LoopGraph), 
00094           loop_start_ (0), 
00095           loop_end_ (0), 
00096           reg_ (new pcl::IterativeClosestPoint<PointT, PointT>), 
00097           loop_transform_ (),
00098           compute_loop_ (true),
00099           vd_ ()
00100         {};
00101       
00102         /** \brief Empty destructor */
00103         virtual ~ELCH () {}
00104 
00105         /** \brief Add a new point cloud to the internal graph.
00106          * \param[in] cloud the new point cloud
00107          */
00108         inline void
00109         addPointCloud (PointCloudPtr cloud)
00110         {
00111           typename boost::graph_traits<LoopGraph>::vertex_descriptor vd = add_vertex (*loop_graph_);
00112           (*loop_graph_)[vd].cloud = cloud;
00113           if (num_vertices (*loop_graph_) > 1)
00114             add_edge (vd_, vd, *loop_graph_);
00115           vd_ = vd;
00116         }
00117 
00118         /** \brief Getter for the internal graph. */
00119         inline LoopGraphPtr
00120         getLoopGraph ()
00121         {
00122           return (loop_graph_);
00123         }
00124 
00125         /** \brief Setter for a new internal graph.
00126          * \param[in] loop_graph the new graph
00127          */
00128         inline void
00129         setLoopGraph (LoopGraphPtr loop_graph)
00130         {
00131           loop_graph_ = loop_graph;
00132         }
00133 
00134         /** \brief Getter for the first scan of a loop. */
00135         inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
00136         getLoopStart ()
00137         {
00138           return (loop_start_);
00139         }
00140 
00141         /** \brief Setter for the first scan of a loop.
00142          * \param[in] loop_start the scan that starts the loop
00143          */
00144         inline void
00145         setLoopStart (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_start)
00146         {
00147           loop_start_ = loop_start;
00148         }
00149 
00150         /** \brief Getter for the last scan of a loop. */
00151         inline typename boost::graph_traits<LoopGraph>::vertex_descriptor
00152         getLoopEnd ()
00153         {
00154           return (loop_end_);
00155         }
00156 
00157         /** \brief Setter for the last scan of a loop.
00158          * \param[in] loop_end the scan that ends the loop
00159          */
00160         inline void
00161         setLoopEnd (const typename boost::graph_traits<LoopGraph>::vertex_descriptor &loop_end)
00162         {
00163           loop_end_ = loop_end;
00164         }
00165 
00166         /** \brief Getter for the registration algorithm. */
00167         inline RegistrationPtr
00168         getReg ()
00169         {
00170           return (reg_);
00171         }
00172 
00173         /** \brief Setter for the registration algorithm.
00174          * \param[in] reg the registration algorithm used to compute the transformation between the start and the end of the loop
00175          */
00176         inline void
00177         setReg (RegistrationPtr reg)
00178         {
00179           reg_ = reg;
00180         }
00181 
00182         /** \brief Getter for the transformation between the first and the last scan. */
00183         inline Eigen::Matrix4f
00184         getLoopTransform ()
00185         {
00186           return (loop_transform_);
00187         }
00188 
00189         /** \brief Setter for the transformation between the first and the last scan.
00190          * \param[in] loop_transform the transformation between the first and the last scan
00191          */
00192         inline void
00193         setLoopTransform (const Eigen::Matrix4f &loop_transform)
00194         {
00195           loop_transform_ = loop_transform;
00196           compute_loop_ = false;
00197         }
00198 
00199         /** \brief Computes now poses for all point clouds by closing the loop
00200          * between start and end point cloud. This will transform all given point
00201          * clouds for now!
00202          */
00203         void
00204         compute ();
00205 
00206       protected:
00207         using PCLBase<PointT>::deinitCompute;
00208 
00209         /** \brief This method should get called before starting the actual computation. */
00210         virtual bool
00211         initCompute ();
00212 
00213       private:
00214         /** \brief graph structure for the internal optimization graph */
00215         typedef boost::adjacency_list<
00216           boost::listS, boost::vecS, boost::undirectedS,
00217           boost::no_property,
00218           boost::property< boost::edge_weight_t, double > >
00219         LOAGraph;
00220 
00221         /**
00222          * graph balancer algorithm computes the weights
00223          * @param[in] g the graph
00224          * @param[in] f index of the first node
00225          * @param[in] l index of the last node
00226          * @param[out] weights array for the weights
00227          */
00228         void
00229         loopOptimizerAlgorithm (LOAGraph &g, double *weights);
00230 
00231         /** \brief The internal loop graph. */
00232         LoopGraphPtr loop_graph_;
00233 
00234         /** \brief The first scan of the loop. */
00235         typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_start_;
00236 
00237         /** \brief The last scan of the loop. */
00238         typename boost::graph_traits<LoopGraph>::vertex_descriptor loop_end_;
00239 
00240         /** \brief The registration object used to close the loop. */
00241         RegistrationPtr reg_;
00242 
00243         /** \brief The transformation between that start and end of the loop. */
00244         Eigen::Matrix4f loop_transform_;
00245         bool compute_loop_;
00246 
00247         /** \brief previously added node in the loop_graph_. */
00248         typename boost::graph_traits<LoopGraph>::vertex_descriptor vd_;
00249 
00250       public:
00251         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00252     };
00253   }
00254 }
00255 
00256 #include <pcl/registration/impl/elch.hpp>
00257 
00258 #endif // PCL_ELCH_H_