Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/transformation_estimation_point_to_plane_lls_weighted.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012-, Open Perception, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the copyright holder(s) nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id$
00037  *
00038  */
00039 #ifndef PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_H_ 
00040 #define PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_H_ 
00041 
00042 #include <pcl/registration/transformation_estimation.h>
00043 #include <pcl/registration/warp_point_rigid.h>
00044 #include <pcl/cloud_iterator.h>
00045 
00046 namespace pcl
00047 {
00048   namespace registration
00049   {
00050     /** \brief @b TransformationEstimationPointToPlaneLLSWeighted implements a Linear Least Squares (LLS) approximation
00051       * for minimizing the point-to-plane distance between two clouds of corresponding points with normals, with the
00052       * possibility of assigning weights to the correspondences.
00053       *
00054       * For additional details, see 
00055       *   "Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration", Kok-Lim Low, 2004
00056       *
00057       * \note The class is templated on the source and target point types as well as on the output scalar of the
00058       * transformation matrix (i.e., float or double). Default: float.
00059       * \author Alex Ichim
00060       * \ingroup registration
00061       */
00062     template <typename PointSource, typename PointTarget, typename Scalar = float>
00063     class TransformationEstimationPointToPlaneLLSWeighted : public TransformationEstimation<PointSource, PointTarget, Scalar>
00064     {
00065       public:
00066         typedef boost::shared_ptr<TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar> > Ptr;
00067         typedef boost::shared_ptr<const TransformationEstimationPointToPlaneLLSWeighted<PointSource, PointTarget, Scalar> > ConstPtr;
00068 
00069         typedef typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4 Matrix4;
00070         
00071         TransformationEstimationPointToPlaneLLSWeighted () { };
00072         virtual ~TransformationEstimationPointToPlaneLLSWeighted () { };
00073 
00074         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
00075           * \param[in] cloud_src the source point cloud dataset
00076           * \param[in] cloud_tgt the target point cloud dataset
00077           * \param[out] transformation_matrix the resultant transformation matrix
00078           */
00079         inline void
00080         estimateRigidTransformation (
00081             const pcl::PointCloud<PointSource> &cloud_src,
00082             const pcl::PointCloud<PointTarget> &cloud_tgt,
00083             Matrix4 &transformation_matrix) const;
00084 
00085         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
00086           * \param[in] cloud_src the source point cloud dataset
00087           * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
00088           * \param[in] cloud_tgt the target point cloud dataset
00089           * \param[out] transformation_matrix the resultant transformation matrix
00090           */
00091         inline void
00092         estimateRigidTransformation (
00093             const pcl::PointCloud<PointSource> &cloud_src,
00094             const std::vector<int> &indices_src,
00095             const pcl::PointCloud<PointTarget> &cloud_tgt,
00096             Matrix4 &transformation_matrix) const;
00097 
00098         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
00099           * \param[in] cloud_src the source point cloud dataset
00100           * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
00101           * \param[in] cloud_tgt the target point cloud dataset
00102           * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from \a indices_src
00103           * \param[out] transformation_matrix the resultant transformation matrix
00104           */
00105         inline void
00106         estimateRigidTransformation (
00107             const pcl::PointCloud<PointSource> &cloud_src,
00108             const std::vector<int> &indices_src,
00109             const pcl::PointCloud<PointTarget> &cloud_tgt,
00110             const std::vector<int> &indices_tgt,
00111             Matrix4 &transformation_matrix) const;
00112 
00113         /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
00114           * \param[in] cloud_src the source point cloud dataset
00115           * \param[in] cloud_tgt the target point cloud dataset
00116           * \param[in] correspondences the vector of correspondences between source and target point cloud
00117           * \param[out] transformation_matrix the resultant transformation matrix
00118           */
00119         inline void
00120         estimateRigidTransformation (
00121             const pcl::PointCloud<PointSource> &cloud_src,
00122             const pcl::PointCloud<PointTarget> &cloud_tgt,
00123             const pcl::Correspondences &correspondences,
00124             Matrix4 &transformation_matrix) const;
00125 
00126 
00127         /** \brief Set the weights for the correspondences.
00128           * \param[in] weights the weights for each correspondence
00129           */
00130         inline void
00131         setCorrespondenceWeights (const std::vector<Scalar> &weights)
00132         { weights_ = weights; }
00133 
00134       protected:
00135         
00136         /** \brief Estimate a rigid rotation transformation between a source and a target
00137           * \param[in] source_it an iterator over the source point cloud dataset
00138           * \param[in] target_it an iterator over the target point cloud dataset
00139           * \param[out] transformation_matrix the resultant transformation matrix
00140           */
00141         void 
00142         estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, 
00143                                      ConstCloudIterator<PointTarget>& target_it, 
00144                                      typename std::vector<Scalar>::const_iterator& weights_it,
00145                                      Matrix4 &transformation_matrix) const;
00146 
00147         /** \brief Construct a 4 by 4 tranformation matrix from the provided rotation and translation.
00148           * \param[in] alpha the rotation about the x-axis
00149           * \param[in] beta the rotation about the y-axis
00150           * \param[in] gamma the rotation about the z-axis
00151           * \param[in] tx the x translation
00152           * \param[in] ty the y translation
00153           * \param[in] tz the z translation
00154           * \param[out] transformation the resultant transformation matrix
00155           */
00156         inline void
00157         constructTransformationMatrix (const double & alpha, const double & beta, const double & gamma,
00158                                        const double & tx,    const double & ty,   const double & tz,
00159                                        Matrix4 &transformation_matrix) const;
00160 
00161         std::vector<Scalar> weights_;
00162     };
00163   }
00164 }
00165 
00166 #include <pcl/registration/impl/transformation_estimation_point_to_plane_lls_weighted.hpp>
00167 
00168 #endif /* PCL_REGISTRATION_TRANSFORMATION_ESTIMATION_POINT_TO_PLANE_LLS_WEIGHTED_H_  */