Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/icp.h
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010, 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_ICP_H_
00042 #define PCL_ICP_H_
00043 
00044 // PCL includes
00045 #include <pcl/sample_consensus/ransac.h>
00046 #include <pcl/sample_consensus/sac_model_registration.h>
00047 #include <pcl/registration/registration.h>
00048 #include <pcl/registration/transformation_estimation_svd.h>
00049 #include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
00050 #include <pcl/registration/correspondence_estimation.h>
00051 #include <pcl/registration/default_convergence_criteria.h>
00052 
00053 namespace pcl
00054 {
00055   /** \brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm. 
00056     * The transformation is estimated based on Singular Value Decomposition (SVD).
00057     *
00058     * The algorithm has several termination criteria:
00059     *
00060     * <ol>
00061     * <li>Number of iterations has reached the maximum user imposed number of iterations (via \ref setMaximumIterations)</li>
00062     * <li>The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via \ref setTransformationEpsilon)</li>
00063     * <li>The sum of Euclidean squared errors is smaller than a user defined threshold (via \ref setEuclideanFitnessEpsilon)</li>
00064     * </ol>
00065     *
00066     *
00067     * Usage example:
00068     * \code
00069     * IterativeClosestPoint<PointXYZ, PointXYZ> icp;
00070     * // Set the input source and target
00071     * icp.setInputCloud (cloud_source);
00072     * icp.setInputTarget (cloud_target);
00073     *
00074     * // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
00075     * icp.setMaxCorrespondenceDistance (0.05);
00076     * // Set the maximum number of iterations (criterion 1)
00077     * icp.setMaximumIterations (50);
00078     * // Set the transformation epsilon (criterion 2)
00079     * icp.setTransformationEpsilon (1e-8);
00080     * // Set the euclidean distance difference epsilon (criterion 3)
00081     * icp.setEuclideanFitnessEpsilon (1);
00082     *
00083     * // Perform the alignment
00084     * icp.align (cloud_source_registered);
00085     *
00086     * // Obtain the transformation that aligned cloud_source to cloud_source_registered
00087     * Eigen::Matrix4f transformation = icp.getFinalTransformation ();
00088     * \endcode
00089     *
00090     * \author Radu B. Rusu, Michael Dixon
00091     * \ingroup registration
00092     */
00093   template <typename PointSource, typename PointTarget, typename Scalar = float>
00094   class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>
00095   {
00096     public:
00097       typedef typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource PointCloudSource;
00098       typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00099       typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00100 
00101       typedef typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget PointCloudTarget;
00102       typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
00103       typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
00104 
00105       typedef PointIndices::Ptr PointIndicesPtr;
00106       typedef PointIndices::ConstPtr PointIndicesConstPtr;
00107 
00108       typedef boost::shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> > Ptr;
00109       typedef boost::shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> > ConstPtr;
00110 
00111       using Registration<PointSource, PointTarget, Scalar>::reg_name_;
00112       using Registration<PointSource, PointTarget, Scalar>::getClassName;
00113       using Registration<PointSource, PointTarget, Scalar>::setInputSource;
00114       using Registration<PointSource, PointTarget, Scalar>::input_;
00115       using Registration<PointSource, PointTarget, Scalar>::indices_;
00116       using Registration<PointSource, PointTarget, Scalar>::target_;
00117       using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
00118       using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
00119       using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
00120       using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
00121       using Registration<PointSource, PointTarget, Scalar>::transformation_;
00122       using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
00123       using Registration<PointSource, PointTarget, Scalar>::converged_;
00124       using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
00125       using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
00126       using Registration<PointSource, PointTarget, Scalar>::min_number_correspondences_;
00127       using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
00128       using Registration<PointSource, PointTarget, Scalar>::euclidean_fitness_epsilon_;
00129       using Registration<PointSource, PointTarget, Scalar>::correspondences_;
00130       using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_;
00131       using Registration<PointSource, PointTarget, Scalar>::correspondence_estimation_;
00132       using Registration<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
00133 
00134       typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr convergence_criteria_;
00135       typedef typename Registration<PointSource, PointTarget, Scalar>::Matrix4 Matrix4;
00136 
00137       /** \brief Empty constructor. */
00138       IterativeClosestPoint () 
00139         : x_idx_offset_ (0)
00140         , y_idx_offset_ (0)
00141         , z_idx_offset_ (0)
00142         , nx_idx_offset_ (0)
00143         , ny_idx_offset_ (0)
00144         , nz_idx_offset_ (0)
00145         , use_reciprocal_correspondence_ (false)
00146         , source_has_normals_ (false)
00147         , target_has_normals_ (false)
00148       {
00149         reg_name_ = "IterativeClosestPoint";
00150         transformation_estimation_.reset (new pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar> ());
00151         correspondence_estimation_.reset (new pcl::registration::CorrespondenceEstimation<PointSource, PointTarget, Scalar>);
00152         convergence_criteria_.reset(new pcl::registration::DefaultConvergenceCriteria<Scalar> (nr_iterations_, transformation_, *correspondences_));
00153       };
00154 
00155       /** \brief Empty destructor */
00156       virtual ~IterativeClosestPoint () {}
00157 
00158       /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the IterativeClosestPoint class.
00159         * This allows to check the convergence state after the align() method as well as to configure
00160         * DefaultConvergenceCriteria's parameters not available through the ICP API before the align()
00161         * method is called. Please note that the align method sets max_iterations_,
00162         * euclidean_fitness_epsilon_ and transformation_epsilon_ and therefore overrides the default / set
00163         * values of the DefaultConvergenceCriteria instance.
00164         * \param[out] Pointer to the IterativeClosestPoint's DefaultConvergenceCriteria.
00165         */
00166       inline typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
00167       getConvergeCriteria ()
00168       {
00169         return convergence_criteria_;
00170       }
00171 
00172       /** \brief Provide a pointer to the input source 
00173         * (e.g., the point cloud that we want to align to the target)
00174         *
00175         * \param[in] cloud the input point cloud source
00176         */
00177       virtual void
00178       setInputSource (const PointCloudSourceConstPtr &cloud)
00179       {
00180         Registration<PointSource, PointTarget, Scalar>::setInputSource (cloud);
00181         std::vector<pcl::PCLPointField> fields;
00182         pcl::getFields (*cloud, fields);
00183         source_has_normals_ = false;
00184         for (size_t i = 0; i < fields.size (); ++i)
00185         {
00186           if      (fields[i].name == "x") x_idx_offset_ = fields[i].offset;
00187           else if (fields[i].name == "y") y_idx_offset_ = fields[i].offset;
00188           else if (fields[i].name == "z") z_idx_offset_ = fields[i].offset;
00189           else if (fields[i].name == "normal_x") 
00190           {
00191             source_has_normals_ = true;
00192             nx_idx_offset_ = fields[i].offset;
00193           }
00194           else if (fields[i].name == "normal_y") 
00195           {
00196             source_has_normals_ = true;
00197             ny_idx_offset_ = fields[i].offset;
00198           }
00199           else if (fields[i].name == "normal_z") 
00200           {
00201             source_has_normals_ = true;
00202             nz_idx_offset_ = fields[i].offset;
00203           }
00204         }
00205       }
00206       
00207       /** \brief Provide a pointer to the input target 
00208         * (e.g., the point cloud that we want to align to the target)
00209         *
00210         * \param[in] cloud the input point cloud target
00211         */
00212       virtual void
00213       setInputTarget (const PointCloudTargetConstPtr &cloud)
00214       {
00215         Registration<PointSource, PointTarget, Scalar>::setInputTarget (cloud);
00216         std::vector<pcl::PCLPointField> fields;
00217         pcl::getFields (*cloud, fields);
00218         target_has_normals_ = false;
00219         for (size_t i = 0; i < fields.size (); ++i)
00220         {
00221           if (fields[i].name == "normal_x" || fields[i].name == "normal_y" || fields[i].name == "normal_z") 
00222           {
00223             target_has_normals_ = true;
00224             break;
00225           }
00226         }
00227       }
00228 
00229       /** \brief Set whether to use reciprocal correspondence or not
00230         *
00231         * \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence or not
00232         */
00233       inline void
00234       setUseReciprocalCorrespondences (bool use_reciprocal_correspondence)
00235       {
00236         use_reciprocal_correspondence_ = use_reciprocal_correspondence;
00237       }
00238 
00239       /** \brief Obtain whether reciprocal correspondence are used or not */
00240       inline bool
00241       getUseReciprocalCorrespondences () const
00242       {
00243         return (use_reciprocal_correspondence_);
00244       }
00245 
00246     protected:
00247 
00248       /** \brief Apply a rigid transform to a given dataset. Here we check whether whether
00249         * the dataset has surface normals in addition to XYZ, and rotate normals as well.
00250         * \param[in] input the input point cloud
00251         * \param[out] output the resultant output point cloud
00252         * \param[in] transform a 4x4 rigid transformation
00253         * \note Can be used with cloud_in equal to cloud_out
00254         */
00255       virtual void 
00256       transformCloud (const PointCloudSource &input, 
00257                       PointCloudSource &output, 
00258                       const Matrix4 &transform);
00259 
00260       /** \brief Rigid transformation computation method  with initial guess.
00261         * \param output the transformed input point cloud dataset using the rigid transformation found
00262         * \param guess the initial guess of the transformation to compute
00263         */
00264       virtual void 
00265       computeTransformation (PointCloudSource &output, const Matrix4 &guess);
00266 
00267       /** \brief XYZ fields offset. */
00268       size_t x_idx_offset_, y_idx_offset_, z_idx_offset_;
00269 
00270       /** \brief Normal fields offset. */
00271       size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_;
00272 
00273       /** \brief The correspondence type used for correspondence estimation. */
00274       bool use_reciprocal_correspondence_;
00275 
00276       /** \brief Internal check whether source dataset has normals or not. */
00277       bool source_has_normals_;
00278       /** \brief Internal check whether target dataset has normals or not. */
00279       bool target_has_normals_;
00280   };
00281 
00282   /** \brief @b IterativeClosestPointWithNormals is a special case of
00283     * IterativeClosestPoint, that uses a transformation estimated based on
00284     * Point to Plane distances by default.
00285     *
00286     * \author Radu B. Rusu
00287     * \ingroup registration
00288     */
00289   template <typename PointSource, typename PointTarget, typename Scalar = float>
00290   class IterativeClosestPointWithNormals : public IterativeClosestPoint<PointSource, PointTarget, Scalar>
00291   {
00292     public:
00293       typedef typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudSource PointCloudSource;
00294       typedef typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::PointCloudTarget PointCloudTarget;
00295       typedef typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4 Matrix4;
00296 
00297       using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
00298       using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_estimation_;
00299       using IterativeClosestPoint<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
00300 
00301       typedef boost::shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar> > Ptr;
00302       typedef boost::shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar> > ConstPtr;
00303 
00304       /** \brief Empty constructor. */
00305       IterativeClosestPointWithNormals () 
00306       {
00307         reg_name_ = "IterativeClosestPointWithNormals";
00308         transformation_estimation_.reset (new pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget, Scalar> ());
00309         //correspondence_rejectors_.add
00310       };
00311       
00312       /** \brief Empty destructor */
00313       virtual ~IterativeClosestPointWithNormals () {}
00314 
00315     protected:
00316 
00317       /** \brief Apply a rigid transform to a given dataset
00318         * \param[in] input the input point cloud
00319         * \param[out] output the resultant output point cloud
00320         * \param[in] transform a 4x4 rigid transformation
00321         * \note Can be used with cloud_in equal to cloud_out
00322         */
00323       virtual void 
00324       transformCloud (const PointCloudSource &input, 
00325                       PointCloudSource &output, 
00326                       const Matrix4 &transform);
00327   };
00328 }
00329 
00330 #include <pcl/registration/impl/icp.hpp>
00331 
00332 #endif  //#ifndef PCL_ICP_H_