Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/transformation_estimation_point_to_plane_weighted.h
00001 /*
00002  *  Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009-2012, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *  Copyright (c) Alexandru-Eugen Ichim
00008  *
00009  *  All rights reserved.
00010  *
00011  *  Redistribution and use in source and binary forms, with or without
00012  *  modification, are permitted provided that the following conditions
00013  *  are met:
00014  *
00015  *  * Redistributions of source code must retain the above copyright
00016  *    notice, this list of conditions and the following disclaimer.
00017  *  * Redistributions in binary form must reproduce the above
00018  *    copyright notice, this list of conditions and the following
00019  *    disclaimer in the documentation and/or other materials provided
00020  *    with the distribution.
00021  *  * Neither the name of the copyright holder(s) nor the names of its
00022  *    contributors may be used to endorse or promote products derived
00023  *    from this software without specific prior written permission.
00024  *
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00027  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00030  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00031  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  */
00038 
00039 
00040 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_H_
00041 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_H_
00042 
00043 #include <pcl/registration/transformation_estimation_point_to_plane.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 TransformationEstimationPointToPlaneWeighted uses Levenberg Marquardt optimization to find the transformation
00052       * that minimizes the point-to-plane distance between the given correspondences. In addition to the
00053       * TransformationEstimationPointToPlane class, this version takes per-correspondence weights and optimizes accordingly.
00054       *
00055       * \author Alexandru-Eugen Ichim
00056       * \ingroup registration
00057       */
00058     template <typename PointSource, typename PointTarget, typename MatScalar = float>
00059     class TransformationEstimationPointToPlaneWeighted : public TransformationEstimationPointToPlane<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<TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> > Ptr;
00072         typedef boost::shared_ptr<const TransformationEstimationPointToPlaneWeighted<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         TransformationEstimationPointToPlaneWeighted ();
00080 
00081         /** \brief Copy constructor. 
00082           * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into this
00083           */
00084         TransformationEstimationPointToPlaneWeighted (const TransformationEstimationPointToPlaneWeighted &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           correspondence_weights_ (src.correspondence_weights_),
00091           use_correspondence_weights_ (src.use_correspondence_weights_)
00092         {};
00093 
00094         /** \brief Copy operator. 
00095           * \param[in] src the TransformationEstimationPointToPlaneWeighted object to copy into this
00096           */
00097         TransformationEstimationPointToPlaneWeighted&
00098         operator = (const TransformationEstimationPointToPlaneWeighted &src)
00099         {
00100           tmp_src_ = src.tmp_src_; 
00101           tmp_tgt_ = src.tmp_tgt_; 
00102           tmp_idx_src_ = src.tmp_idx_src_;
00103           tmp_idx_tgt_ = src.tmp_idx_tgt_; 
00104           warp_point_ = src.warp_point_;
00105           correspondence_weights_ = src.correspondence_weights_;
00106           use_correspondence_weights_ = src.use_correspondence_weights_;
00107         }
00108 
00109          /** \brief Destructor. */
00110         virtual ~TransformationEstimationPointToPlaneWeighted () {};
00111 
00112         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
00113           * \param[in] cloud_src the source point cloud dataset
00114           * \param[in] cloud_tgt the target point cloud dataset
00115           * \param[out] transformation_matrix the resultant transformation matrix
00116           * \note Uses the weights given by setWeights.
00117           */
00118         inline void
00119         estimateRigidTransformation (
00120             const pcl::PointCloud<PointSource> &cloud_src,
00121             const pcl::PointCloud<PointTarget> &cloud_tgt,
00122             Matrix4 &transformation_matrix) const;
00123 
00124         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
00125           * \param[in] cloud_src the source point cloud dataset
00126           * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
00127           * \param[in] cloud_tgt the target point cloud dataset
00128           * \param[out] transformation_matrix the resultant transformation matrix
00129           * \note Uses the weights given by setWeights.
00130           */
00131         inline void
00132         estimateRigidTransformation (
00133             const pcl::PointCloud<PointSource> &cloud_src,
00134             const std::vector<int> &indices_src,
00135             const pcl::PointCloud<PointTarget> &cloud_tgt,
00136             Matrix4 &transformation_matrix) const;
00137 
00138         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
00139           * \param[in] cloud_src the source point cloud dataset
00140           * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
00141           * \param[in] cloud_tgt the target point cloud dataset
00142           * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from 
00143           * \a indices_src
00144           * \param[out] transformation_matrix the resultant transformation matrix
00145           * \note Uses the weights given by setWeights.
00146           */
00147         void
00148         estimateRigidTransformation (
00149             const pcl::PointCloud<PointSource> &cloud_src,
00150             const std::vector<int> &indices_src,
00151             const pcl::PointCloud<PointTarget> &cloud_tgt,
00152             const std::vector<int> &indices_tgt,
00153             Matrix4 &transformation_matrix) const;
00154 
00155         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using LM.
00156           * \param[in] cloud_src the source point cloud dataset
00157           * \param[in] cloud_tgt the target point cloud dataset
00158           * \param[in] correspondences the vector of correspondences between source and target point cloud
00159           * \param[out] transformation_matrix the resultant transformation matrix
00160           * \note Uses the weights given by setWeights.
00161           */
00162         void
00163         estimateRigidTransformation (
00164             const pcl::PointCloud<PointSource> &cloud_src,
00165             const pcl::PointCloud<PointTarget> &cloud_tgt,
00166             const pcl::Correspondences &correspondences,
00167             Matrix4 &transformation_matrix) const;  
00168 
00169 
00170         inline void
00171         setWeights (const std::vector<double> &weights)
00172         { correspondence_weights_ = weights; }
00173 
00174         /// use the weights given in the pcl::CorrespondencesPtr for one of the estimateTransformation (...) methods
00175         inline void
00176         setUseCorrespondenceWeights (bool use_correspondence_weights)
00177         { use_correspondence_weights_ = use_correspondence_weights; }
00178 
00179         /** \brief Set the function we use to warp points. Defaults to rigid 6D warp.
00180           * \param[in] warp_fcn a shared pointer to an object that warps points
00181           */
00182         void
00183         setWarpFunction (const boost::shared_ptr<WarpPointRigid<PointSource, PointTarget, MatScalar> > &warp_fcn)
00184         { warp_point_ = warp_fcn; }
00185 
00186       protected:
00187         bool use_correspondence_weights_;
00188         mutable std::vector<double> correspondence_weights_;
00189 
00190         /** \brief Temporary pointer to the source dataset. */
00191         mutable const PointCloudSource *tmp_src_;
00192 
00193         /** \brief Temporary pointer to the target dataset. */
00194         mutable const PointCloudTarget  *tmp_tgt_;
00195 
00196         /** \brief Temporary pointer to the source dataset indices. */
00197         mutable const std::vector<int> *tmp_idx_src_;
00198 
00199         /** \brief Temporary pointer to the target dataset indices. */
00200         mutable const std::vector<int> *tmp_idx_tgt_;
00201 
00202         /** \brief The parameterized function used to warp the source to the target. */
00203         boost::shared_ptr<pcl::registration::WarpPointRigid<PointSource, PointTarget, MatScalar> > warp_point_;
00204         
00205         /** Base functor all the models that need non linear optimization must
00206           * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
00207           * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) dependening on the choosen _Scalar
00208           */
00209         template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
00210         struct Functor
00211         {
00212           typedef _Scalar Scalar;
00213           enum 
00214           {
00215             InputsAtCompileTime = NX,
00216             ValuesAtCompileTime = NY
00217           };
00218           typedef Eigen::Matrix<_Scalar,InputsAtCompileTime,1> InputType;
00219           typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,1> ValueType;
00220           typedef Eigen::Matrix<_Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
00221 
00222           /** \brief Empty Construtor. */
00223           Functor () : m_data_points_ (ValuesAtCompileTime) {}
00224 
00225           /** \brief Constructor
00226             * \param[in] m_data_points number of data points to evaluate.
00227             */
00228           Functor (int m_data_points) : m_data_points_ (m_data_points) {}
00229         
00230           /** \brief Destructor. */
00231           virtual ~Functor () {}
00232 
00233           /** \brief Get the number of values. */ 
00234           int
00235           values () const { return (m_data_points_); }
00236 
00237           protected:
00238             int m_data_points_;
00239         };
00240 
00241         struct OptimizationFunctor : public Functor<MatScalar>
00242         {
00243           using Functor<MatScalar>::values;
00244 
00245           /** Functor constructor
00246             * \param[in] m_data_points the number of data points to evaluate
00247             * \param[in,out] estimator pointer to the estimator object
00248             */
00249           OptimizationFunctor (int m_data_points, 
00250                                const TransformationEstimationPointToPlaneWeighted *estimator)
00251             :  Functor<MatScalar> (m_data_points), estimator_ (estimator) 
00252           {}
00253 
00254           /** Copy constructor
00255             * \param[in] the optimization functor to copy into this
00256             */
00257           inline OptimizationFunctor (const OptimizationFunctor &src) : 
00258             Functor<MatScalar> (src.m_data_points_), estimator_ ()
00259           {
00260             *this = src;
00261           }
00262 
00263           /** Copy operator
00264             * \param[in] the optimization functor to copy into this
00265             */
00266           inline OptimizationFunctor& 
00267           operator = (const OptimizationFunctor &src) 
00268           { 
00269             Functor<MatScalar>::operator=(src);
00270             estimator_ = src.estimator_; 
00271             return (*this); 
00272           }
00273 
00274           /** \brief Destructor. */
00275           virtual ~OptimizationFunctor () {}
00276 
00277           /** Fill fvec from x. For the current state vector x fill the f values
00278             * \param[in] x state vector
00279             * \param[out] fvec f values vector
00280             */
00281           int 
00282           operator () (const VectorX &x, VectorX &fvec) const;
00283 
00284           const TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> *estimator_;
00285         };
00286 
00287         struct OptimizationFunctorWithIndices : public Functor<MatScalar>
00288         {
00289           using Functor<MatScalar>::values;
00290 
00291           /** Functor constructor
00292             * \param[in] m_data_points the number of data points to evaluate
00293             * \param[in,out] estimator pointer to the estimator object
00294             */
00295           OptimizationFunctorWithIndices (int m_data_points, 
00296                                           const TransformationEstimationPointToPlaneWeighted *estimator)
00297             : Functor<MatScalar> (m_data_points), estimator_ (estimator) 
00298           {}
00299 
00300           /** Copy constructor
00301             * \param[in] the optimization functor to copy into this
00302             */
00303           inline OptimizationFunctorWithIndices (const OptimizationFunctorWithIndices &src)
00304             : Functor<MatScalar> (src.m_data_points_), estimator_ ()
00305           {
00306             *this = src;
00307           }
00308 
00309           /** Copy operator
00310             * \param[in] the optimization functor to copy into this
00311             */
00312           inline OptimizationFunctorWithIndices& 
00313           operator = (const OptimizationFunctorWithIndices &src) 
00314           { 
00315             Functor<MatScalar>::operator=(src);
00316             estimator_ = src.estimator_; 
00317             return (*this); 
00318           }
00319 
00320           /** \brief Destructor. */
00321           virtual ~OptimizationFunctorWithIndices () {}
00322 
00323           /** Fill fvec from x. For the current state vector x fill the f values
00324             * \param[in] x state vector
00325             * \param[out] fvec f values vector
00326             */
00327           int 
00328           operator () (const VectorX &x, VectorX &fvec) const;
00329 
00330           const TransformationEstimationPointToPlaneWeighted<PointSource, PointTarget, MatScalar> *estimator_;
00331         };
00332       public:
00333         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00334     };
00335   }
00336 }
00337 
00338 #include <pcl/registration/impl/transformation_estimation_point_to_plane_weighted.hpp>
00339 
00340 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_WEIGHTED_H_ */
00341