Point Cloud Library (PCL)
1.7.0
|
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_