Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/ndt_2d.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_NDT_2D_H_
00042 #define PCL_NDT_2D_H_
00043 
00044 #include <pcl/registration/registration.h>
00045 
00046 namespace pcl
00047 {
00048   /** \brief @b NormalDistributionsTransform2D provides an implementation of the
00049     * Normal Distributions Transform algorithm for scan matching.
00050     *
00051     * This implementation is intended to match the definition:
00052     * Peter Biber and Wolfgang Straßer. The normal distributions transform: A
00053     * new approach to laser scan matching. In Proceedings of the IEEE In-
00054     * ternational Conference on Intelligent Robots and Systems (IROS), pages
00055     * 2743–2748, Las Vegas, USA, October 2003.
00056     *
00057     * \author James Crosby
00058     */
00059   template <typename PointSource, typename PointTarget>
00060   class NormalDistributionsTransform2D : public Registration<PointSource, PointTarget>
00061   {
00062     typedef typename Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource;
00063     typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00064     typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00065 
00066     typedef typename Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
00067 
00068     typedef PointIndices::Ptr PointIndicesPtr;
00069     typedef PointIndices::ConstPtr PointIndicesConstPtr;
00070 
00071     public:
00072 
00073         typedef boost::shared_ptr< NormalDistributionsTransform2D<PointSource, PointTarget> > Ptr;
00074         typedef boost::shared_ptr< const NormalDistributionsTransform2D<PointSource, PointTarget> > ConstPtr;
00075 
00076       /** \brief Empty constructor. */
00077       NormalDistributionsTransform2D ()
00078         : Registration<PointSource,PointTarget> (),
00079           grid_centre_ (0,0), grid_step_ (1,1), grid_extent_ (20,20), newton_lambda_ (1,1,1)
00080       {
00081         reg_name_ = "NormalDistributionsTransform2D";
00082       }
00083       
00084       /** \brief Empty destructor */
00085       virtual ~NormalDistributionsTransform2D () {}
00086  
00087       /** \brief centre of the ndt grid (target coordinate system)
00088         * \param centre value to set
00089         */
00090       virtual void
00091       setGridCentre (const Eigen::Vector2f& centre) { grid_centre_ = centre; }
00092 
00093       /** \brief Grid spacing (step) of the NDT grid
00094         * \param[in] step value to set
00095         */
00096       virtual void
00097       setGridStep (const Eigen::Vector2f& step) { grid_step_ = step; }
00098 
00099       /** \brief NDT Grid extent (in either direction from the grid centre)
00100         * \param[in] extent value to set
00101         */
00102       virtual void
00103       setGridExtent (const Eigen::Vector2f& extent) { grid_extent_ = extent; }
00104 
00105       /** \brief NDT Newton optimisation step size parameter
00106         * \param[in] lambda step size: 1 is simple newton optimisation, smaller values may improve convergence
00107         */
00108        virtual void
00109        setOptimizationStepSize (const double& lambda) { newton_lambda_ = Eigen::Vector3d (lambda, lambda, lambda); }
00110 
00111       /** \brief NDT Newton optimisation step size parameter
00112         * \param[in] lambda step size: (1,1,1) is simple newton optimisation,
00113         * smaller values may improve convergence, or elements may be set to
00114         * zero to prevent optimisation over some parameters
00115         *
00116         * This overload allows control of updates to the individual (x, y,
00117         * theta) free parameters in the optimisation. If, for example, theta is
00118         * believed to be close to the correct value a small value of lambda[2]
00119         * should be used.
00120         */
00121        virtual void
00122        setOptimizationStepSize (const Eigen::Vector3d& lambda) { newton_lambda_ = lambda; }
00123 
00124     protected:
00125       /** \brief Rigid transformation computation method with initial guess.
00126         * \param[out] output the transformed input point cloud dataset using the rigid transformation found
00127         * \param[in] guess the initial guess of the transformation to compute
00128         */
00129       virtual void 
00130       computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess);
00131 
00132       using Registration<PointSource, PointTarget>::reg_name_;
00133       using Registration<PointSource, PointTarget>::target_;
00134       using Registration<PointSource, PointTarget>::converged_;
00135       using Registration<PointSource, PointTarget>::nr_iterations_;
00136       using Registration<PointSource, PointTarget>::max_iterations_;
00137       using Registration<PointSource, PointTarget>::transformation_epsilon_;
00138       using Registration<PointSource, PointTarget>::transformation_;
00139       using Registration<PointSource, PointTarget>::previous_transformation_;      
00140       using Registration<PointSource, PointTarget>::final_transformation_;
00141       using Registration<PointSource, PointTarget>::update_visualizer_;
00142       using Registration<PointSource, PointTarget>::indices_;
00143 
00144       Eigen::Vector2f grid_centre_;
00145       Eigen::Vector2f grid_step_;
00146       Eigen::Vector2f grid_extent_;
00147       Eigen::Vector3d newton_lambda_;
00148     public:
00149       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00150   };
00151 
00152 } // namespace pcl
00153 
00154 #include <pcl/registration/impl/ndt_2d.hpp>
00155 
00156 #endif // ndef PCL_NDT_2D_H_
00157