Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/transformation_estimation_lm.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-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 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_
00041 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_
00042 
00043 #include <pcl/registration/transformation_estimation.h>
00044 #include <pcl/registration/warp_point_rigid.h>
00045 #include <pcl/registration/distances.h>
00046 
00047 namespace pcl
00048 {
00049   namespace registration
00050   {
00051     /** @b TransformationEstimationLM implements Levenberg Marquardt-based
00052       * estimation of the transformation aligning the given correspondences.
00053       *
00054       * \note The class is templated on the source and target point types as well as on the output scalar of the transformation matrix (i.e., float or double). Default: float.
00055       * \author Radu B. Rusu
00056       * \ingroup registration
00057       */
00058     template <typename PointSource, typename PointTarget, typename MatScalar = float>
00059     class TransformationEstimationLM : public TransformationEstimation<PointSource, PointTarget, MatScalar>
00060     {
00061       typedef pcl::PointCloud<PointSource> PointCloudSource;
00062       typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00063       typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00064 
00065       typedef pcl::PointCloud<PointTarget> PointCloudTarget;
00066 
00067       typedef PointIndices::Ptr PointIndicesPtr;
00068       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00069 
00070       public:
00071         typedef boost::shared_ptr<TransformationEstimationLM<PointSource, PointTarget, MatScalar> > Ptr;
00072         typedef boost::shared_ptr<const TransformationEstimationLM<PointSource, PointTarget, MatScalar> > ConstPtr;
00073 
00074         typedef Eigen::Matrix<MatScalar, Eigen::Dynamic, 1> VectorX;
00075         typedef Eigen::Matrix<MatScalar, 4, 1> Vector4;
00076         typedef typename TransformationEstimation<PointSource, PointTarget, MatScalar>::Matrix4 Matrix4;
00077         
00078         /** \brief Constructor. */
00079         TransformationEstimationLM ();
00080 
00081         /** \brief Copy constructor. 
00082           * \param[in] src the TransformationEstimationLM object to copy into this 
00083           */
00084         TransformationEstimationLM (const TransformationEstimationLM &src) : 
00085           tmp_src_ (src.tmp_src_), 
00086           tmp_tgt_ (src.tmp_tgt_), 
00087           tmp_idx_src_ (src.tmp_idx_src_), 
00088           tmp_idx_tgt_ (src.tmp_idx_tgt_), 
00089           warp_point_ (src.warp_point_)
00090         {};
00091 
00092         /** \brief Copy operator. 
00093           * \param[in] src the TransformationEstimationLM object to copy into this 
00094           */
00095         TransformationEstimationLM&
00096         operator = (const TransformationEstimationLM &src)
00097         {
00098           tmp_src_ = src.tmp_src_; 
00099           tmp_tgt_ = src.tmp_tgt_; 
00100           tmp_idx_src_ = src.tmp_idx_src_;
00101           tmp_idx_tgt_ = src.tmp_idx_tgt_; 
00102           warp_point_ = src.warp_point_;
00103         }
00104 
00105          /** \brief Destructor. */
00106         virtual ~TransformationEstimationLM () {};
00107 
00108         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
00109           * \param[in] cloud_src the source point cloud dataset
00110           * \param[in] cloud_tgt the target point cloud dataset
00111           * \param[out] transformation_matrix the resultant transformation matrix
00112           */
00113         inline void
00114         estimateRigidTransformation (
00115             const pcl::PointCloud<PointSource> &cloud_src,
00116             const pcl::PointCloud<PointTarget> &cloud_tgt,
00117             Matrix4 &transformation_matrix) const;
00118 
00119         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
00120           * \param[in] cloud_src the source point cloud dataset
00121           * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
00122           * \param[in] cloud_tgt the target point cloud dataset
00123           * \param[out] transformation_matrix the resultant transformation matrix
00124           */
00125         inline void
00126         estimateRigidTransformation (
00127             const pcl::PointCloud<PointSource> &cloud_src,
00128             const std::vector<int> &indices_src,
00129             const pcl::PointCloud<PointTarget> &cloud_tgt,
00130             Matrix4 &transformation_matrix) const;
00131 
00132         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
00133           * \param[in] cloud_src the source point cloud dataset
00134           * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
00135           * \param[in] cloud_tgt the target point cloud dataset
00136           * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from 
00137           * \a indices_src
00138           * \param[out] transformation_matrix the resultant transformation matrix
00139           */
00140         inline void
00141         estimateRigidTransformation (
00142             const pcl::PointCloud<PointSource> &cloud_src,
00143             const std::vector<int> &indices_src,
00144             const pcl::PointCloud<PointTarget> &cloud_tgt,
00145             const std::vector<int> &indices_tgt,
00146             Matrix4 &transformation_matrix) const;
00147 
00148         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
00149           * \param[in] cloud_src the source point cloud dataset
00150           * \param[in] cloud_tgt the target point cloud dataset
00151           * \param[in] correspondences the vector of correspondences between source and target point cloud
00152           * \param[out] transformation_matrix the resultant transformation matrix
00153           */
00154         inline void
00155         estimateRigidTransformation (
00156             const pcl::PointCloud<PointSource> &cloud_src,
00157             const pcl::PointCloud<PointTarget> &cloud_tgt,
00158             const pcl::Correspondences &correspondences,
00159             Matrix4 &transformation_matrix) const;
00160 
00161         /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
00162           * \param[in] warp_fcn a shared pointer to an object that warps points
00163           */
00164         void
00165         setWarpFunction (const boost::shared_ptr<WarpPointRigid<PointSource, PointTarget, MatScalar> > &warp_fcn)
00166         {
00167           warp_point_ = warp_fcn;
00168         }
00169 
00170       protected:
00171         /** \brief Compute the distance between a source point and its corresponding target point
00172           * \param[in] p_src The source point
00173           * \param[in] p_tgt The target point
00174           * \return The distance between \a p_src and \a p_tgt
00175           *
00176           * \note Older versions of PCL used this method internally for calculating the
00177           * optimization gradient. Since PCL 1.7, a switch has been made to the 
00178           * computeDistance method using Vector4 types instead. This method is only 
00179           * kept for API compatibility reasons.
00180           */
00181         virtual MatScalar
00182         computeDistance (const PointSource &p_src, const PointTarget &p_tgt) const
00183         {
00184           Vector4 s (p_src.x, p_src.y, p_src.z, 0);
00185           Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
00186           return ((s - t).norm ());
00187         }
00188 
00189         /** \brief Compute the distance between a source point and its corresponding target point
00190           * \param[in] p_src The source point
00191           * \param[in] p_tgt The target point
00192           * \return The distance between \a p_src and \a p_tgt
00193           *
00194           * \note A different distance function can be defined by creating a subclass of 
00195           * TransformationEstimationLM and overriding this method. 
00196           * (See \a TransformationEstimationPointToPlane)
00197           */
00198         virtual MatScalar
00199         computeDistance (const Vector4 &p_src, const PointTarget &p_tgt) const
00200         {
00201           Vector4 t (p_tgt.x, p_tgt.y, p_tgt.z, 0);
00202           return ((p_src - t).norm ());
00203         }
00204 
00205         /** \brief Temporary pointer to the source dataset. */
00206         mutable const PointCloudSource *tmp_src_;
00207 
00208         /** \brief Temporary pointer to the target dataset. */
00209         mutable const PointCloudTarget  *tmp_tgt_;
00210 
00211         /** \brief Temporary pointer to the source dataset indices. */
00212         mutable const std::vector<int> *tmp_idx_src_;
00213 
00214         /** \brief Temporary pointer to the target dataset indices. */
00215         mutable const std::vector<int> *tmp_idx_tgt_;
00216 
00217         /** \brief The parameterized function used to warp the source to the target. */
00218         boost::shared_ptr<pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar> > warp_point_;
00219         
00220         /** Base functor all the models that need non linear optimization must
00221           * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
00222           * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) dependening on the choosen _Scalar
00223           */
00224         template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
00225         struct Functor
00226         {
00227           typedef _Scalar Scalar;
00228           enum 
00229           {
00230             InputsAtCompileTime = NX,
00231             ValuesAtCompileTime = NY
00232           };
00233           typedef Eigen::Matrix<_Scalar,InputsAtCompileTime,1> InputType;
00234           typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,1> ValueType;
00235           typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
00236 
00237           /** \brief Empty Construtor. */
00238           Functor () : m_data_points_ (ValuesAtCompileTime) {}
00239 
00240           /** \brief Constructor
00241             * \param[in] m_data_points number of data points to evaluate.
00242             */
00243           Functor (int m_data_points) : m_data_points_ (m_data_points) {}
00244         
00245           /** \brief Destructor. */
00246           virtual ~Functor () {}
00247 
00248           /** \brief Get the number of values. */ 
00249           int
00250           values () const { return (m_data_points_); }
00251 
00252           protected:
00253             int m_data_points_;
00254         };
00255 
00256         struct OptimizationFunctor : public Functor<MatScalar>
00257         {
00258           using Functor<MatScalar>::values;
00259 
00260           /** Functor constructor
00261             * \param[in] m_data_points the number of data points to evaluate
00262             * \param[in,out] estimator pointer to the estimator object
00263             */
00264           OptimizationFunctor (int m_data_points, 
00265                                const TransformationEstimationLM *estimator) 
00266             :  Functor<MatScalar> (m_data_points), estimator_ (estimator) 
00267           {}
00268 
00269           /** Copy constructor
00270             * \param[in] the optimization functor to copy into this
00271             */
00272           inline OptimizationFunctor (const OptimizationFunctor &src) : 
00273             Functor<MatScalar> (src.m_data_points_), estimator_ ()
00274           {
00275             *this = src;
00276           }
00277 
00278           /** Copy operator
00279             * \param[in] the optimization functor to copy into this
00280             */
00281           inline OptimizationFunctor& 
00282           operator = (const OptimizationFunctor &src) 
00283           { 
00284             Functor<MatScalar>::operator=(src);
00285             estimator_ = src.estimator_; 
00286             return (*this); 
00287           }
00288 
00289           /** \brief Destructor. */
00290           virtual ~OptimizationFunctor () {}
00291 
00292           /** Fill fvec from x. For the current state vector x fill the f values
00293             * \param[in] x state vector
00294             * \param[out] fvec f values vector
00295             */
00296           int 
00297           operator () (const VectorX &x, VectorX &fvec) const;
00298 
00299           const TransformationEstimationLM<PointSource, PointTarget, MatScalar> *estimator_;
00300         };
00301 
00302         struct OptimizationFunctorWithIndices : public Functor<MatScalar>
00303         {
00304           using Functor<MatScalar>::values;
00305 
00306           /** Functor constructor
00307             * \param[in] m_data_points the number of data points to evaluate
00308             * \param[in,out] estimator pointer to the estimator object
00309             */
00310           OptimizationFunctorWithIndices (int m_data_points, 
00311                                           const TransformationEstimationLM *estimator) 
00312             : Functor<MatScalar> (m_data_points), estimator_ (estimator) 
00313           {}
00314 
00315           /** Copy constructor
00316             * \param[in] the optimization functor to copy into this
00317             */
00318           inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src)
00319             : Functor<MatScalar> (src.m_data_points_), estimator_ ()
00320           {
00321             *this = src;
00322           }
00323 
00324           /** Copy operator
00325             * \param[in] the optimization functor to copy into this
00326             */
00327           inline OptimizationFunctorWithIndices& 
00328           operator = (const OptimizationFunctorWithIndices &src) 
00329           { 
00330             Functor<MatScalar>::operator=(src);
00331             estimator_ = src.estimator_; 
00332             return (*this); 
00333           }
00334 
00335           /** \brief Destructor. */
00336           virtual ~OptimizationFunctorWithIndices () {}
00337 
00338           /** Fill fvec from x. For the current state vector x fill the f values
00339             * \param[in] x state vector
00340             * \param[out] fvec f values vector
00341             */
00342           int 
00343           operator () (const VectorX &x, VectorX &fvec) const;
00344 
00345           const TransformationEstimationLM<PointSource, PointTarget, MatScalar> *estimator_;
00346         };
00347       public:
00348         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00349     };
00350   }
00351 }
00352 
00353 #include <pcl/registration/impl/transformation_estimation_lm.hpp>
00354 
00355 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_LM_H_ */
00356