1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2010, Willow Garage, Inc. 6 * Copyright (c) 2012-, Open Perception, Inc. 7 * 8 * All rights reserved. 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 14 * * Redistributions of source code must retain the above copyright 15 * notice, this list of conditions and the following disclaimer. 16 * * Redistributions in binary form must reproduce the above 17 * copyright notice, this list of conditions and the following 18 * disclaimer in the documentation and/or other materials provided 19 * with the distribution. 20 * * Neither the name of the copyright holder(s) nor the names of its 21 * contributors may be used to endorse or promote products derived 22 * from this software without specific prior written permission. 23 * 24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 * POSSIBILITY OF SUCH DAMAGE. 36 * 37 * $Id$ 38 * 39 */ 40 41 #pragma once 42 43 // PCL includes 44 #include <pcl/registration/correspondence_estimation.h> 45 #include <pcl/registration/default_convergence_criteria.h> 46 #include <pcl/registration/registration.h> 47 #include <pcl/registration/transformation_estimation_point_to_plane_lls.h> 48 #include <pcl/registration/transformation_estimation_svd.h> 49 #include <pcl/registration/transformation_estimation_symmetric_point_to_plane_lls.h> 50 #include <pcl/memory.h> // for dynamic_pointer_cast, pcl::make_shared, shared_ptr 51 52 namespace pcl { 53 /** \brief @b IterativeClosestPoint provides a base implementation of the Iterative 54 * Closest Point algorithm. The transformation is estimated based on Singular Value 55 * Decomposition (SVD). 56 * 57 * The algorithm has several termination criteria: 58 * 59 * <ol> 60 * <li>Number of iterations has reached the maximum user imposed number of iterations 61 * (via \ref setMaximumIterations)</li> <li>The epsilon (difference) between the 62 * previous transformation and the current estimated transformation is smaller than an 63 * user imposed value (via \ref setTransformationEpsilon)</li> <li>The sum of Euclidean 64 * squared errors is smaller than a user defined threshold (via \ref 65 * setEuclideanFitnessEpsilon)</li> 66 * </ol> 67 * 68 * 69 * Usage example: 70 * \code 71 * IterativeClosestPoint<PointXYZ, PointXYZ> icp; 72 * // Set the input source and target 73 * icp.setInputSource (cloud_source); 74 * icp.setInputTarget (cloud_target); 75 * 76 * // Set the max correspondence distance to 5cm (e.g., correspondences with higher 77 * // distances will be ignored) 78 * icp.setMaxCorrespondenceDistance (0.05); 79 * // Set the maximum number of iterations (criterion 1) 80 * icp.setMaximumIterations (50); 81 * // Set the transformation epsilon (criterion 2) 82 * icp.setTransformationEpsilon (1e-8); 83 * // Set the euclidean distance difference epsilon (criterion 3) 84 * icp.setEuclideanFitnessEpsilon (1); 85 * 86 * // Perform the alignment 87 * icp.align (cloud_source_registered); 88 * 89 * // Obtain the transformation that aligned cloud_source to cloud_source_registered 90 * Eigen::Matrix4f transformation = icp.getFinalTransformation (); 91 * \endcode 92 * 93 * \author Radu B. Rusu, Michael Dixon 94 * \ingroup registration 95 */ 96 template <typename PointSource, typename PointTarget, typename Scalar = float> 97 class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar> { 98 public: 99 using PointCloudSource = 100 typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource; 101 using PointCloudSourcePtr = typename PointCloudSource::Ptr; 102 using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 103 104 using PointCloudTarget = 105 typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget; 106 using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 107 using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 108 109 using PointIndicesPtr = PointIndices::Ptr; 110 using PointIndicesConstPtr = PointIndices::ConstPtr; 111 112 using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>; 113 using ConstPtr = 114 shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>; 115 116 using Registration<PointSource, PointTarget, Scalar>::reg_name_; 117 using Registration<PointSource, PointTarget, Scalar>::getClassName; 118 using Registration<PointSource, PointTarget, Scalar>::input_; 119 using Registration<PointSource, PointTarget, Scalar>::indices_; 120 using Registration<PointSource, PointTarget, Scalar>::target_; 121 using Registration<PointSource, PointTarget, Scalar>::nr_iterations_; 122 using Registration<PointSource, PointTarget, Scalar>::max_iterations_; 123 using Registration<PointSource, PointTarget, Scalar>::previous_transformation_; 124 using Registration<PointSource, PointTarget, Scalar>::final_transformation_; 125 using Registration<PointSource, PointTarget, Scalar>::transformation_; 126 using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_; 127 using Registration<PointSource, PointTarget, Scalar>:: 128 transformation_rotation_epsilon_; 129 using Registration<PointSource, PointTarget, Scalar>::converged_; 130 using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_; 131 using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_; 132 using Registration<PointSource, PointTarget, Scalar>::min_number_correspondences_; 133 using Registration<PointSource, PointTarget, Scalar>::update_visualizer_; 134 using Registration<PointSource, PointTarget, Scalar>::euclidean_fitness_epsilon_; 135 using Registration<PointSource, PointTarget, Scalar>::correspondences_; 136 using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_; 137 using Registration<PointSource, PointTarget, Scalar>::correspondence_estimation_; 138 using Registration<PointSource, PointTarget, Scalar>::correspondence_rejectors_; 139 140 typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr 141 convergence_criteria_; 142 using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4; 143 144 /** \brief Empty constructor. */ IterativeClosestPoint()145 IterativeClosestPoint() 146 : x_idx_offset_(0) 147 , y_idx_offset_(0) 148 , z_idx_offset_(0) 149 , nx_idx_offset_(0) 150 , ny_idx_offset_(0) 151 , nz_idx_offset_(0) 152 , use_reciprocal_correspondence_(false) 153 , source_has_normals_(false) 154 , target_has_normals_(false) 155 { 156 reg_name_ = "IterativeClosestPoint"; 157 transformation_estimation_.reset( 158 new pcl::registration:: 159 TransformationEstimationSVD<PointSource, PointTarget, Scalar>()); 160 correspondence_estimation_.reset( 161 new pcl::registration:: 162 CorrespondenceEstimation<PointSource, PointTarget, Scalar>); 163 convergence_criteria_.reset( 164 new pcl::registration::DefaultConvergenceCriteria<Scalar>( 165 nr_iterations_, transformation_, *correspondences_)); 166 }; 167 168 /** 169 * \brief Due to `convergence_criteria_` holding references to the class members, 170 * it is tricky to correctly implement its copy and move operations correctly. This 171 * can result in subtle bugs and to prevent them, these operations for ICP have 172 * been disabled. 173 * 174 * \todo: remove deleted ctors and assignments operations after resolving the issue 175 */ 176 IterativeClosestPoint(const IterativeClosestPoint&) = delete; 177 IterativeClosestPoint(IterativeClosestPoint&&) = delete; 178 IterativeClosestPoint& 179 operator=(const IterativeClosestPoint&) = delete; 180 IterativeClosestPoint& 181 operator=(IterativeClosestPoint&&) = delete; 182 183 /** \brief Empty destructor */ ~IterativeClosestPoint()184 ~IterativeClosestPoint() {} 185 186 /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the 187 * IterativeClosestPoint class. This allows to check the convergence state after the 188 * align() method as well as to configure DefaultConvergenceCriteria's parameters not 189 * available through the ICP API before the align() method is called. Please note that 190 * the align method sets max_iterations_, euclidean_fitness_epsilon_ and 191 * transformation_epsilon_ and therefore overrides the default / set values of the 192 * DefaultConvergenceCriteria instance. \return Pointer to the IterativeClosestPoint's 193 * DefaultConvergenceCriteria. 194 */ 195 inline typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr getConvergeCriteria()196 getConvergeCriteria() 197 { 198 return convergence_criteria_; 199 } 200 201 /** \brief Provide a pointer to the input source 202 * (e.g., the point cloud that we want to align to the target) 203 * 204 * \param[in] cloud the input point cloud source 205 */ 206 void setInputSource(const PointCloudSourceConstPtr & cloud)207 setInputSource(const PointCloudSourceConstPtr& cloud) override 208 { 209 Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud); 210 const auto fields = pcl::getFields<PointSource>(); 211 source_has_normals_ = false; 212 for (const auto& field : fields) { 213 if (field.name == "x") 214 x_idx_offset_ = field.offset; 215 else if (field.name == "y") 216 y_idx_offset_ = field.offset; 217 else if (field.name == "z") 218 z_idx_offset_ = field.offset; 219 else if (field.name == "normal_x") { 220 source_has_normals_ = true; 221 nx_idx_offset_ = field.offset; 222 } 223 else if (field.name == "normal_y") { 224 source_has_normals_ = true; 225 ny_idx_offset_ = field.offset; 226 } 227 else if (field.name == "normal_z") { 228 source_has_normals_ = true; 229 nz_idx_offset_ = field.offset; 230 } 231 } 232 } 233 234 /** \brief Provide a pointer to the input target 235 * (e.g., the point cloud that we want to align to the target) 236 * 237 * \param[in] cloud the input point cloud target 238 */ 239 void setInputTarget(const PointCloudTargetConstPtr & cloud)240 setInputTarget(const PointCloudTargetConstPtr& cloud) override 241 { 242 Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud); 243 const auto fields = pcl::getFields<PointSource>(); 244 target_has_normals_ = false; 245 for (const auto& field : fields) { 246 if (field.name == "normal_x" || field.name == "normal_y" || 247 field.name == "normal_z") { 248 target_has_normals_ = true; 249 break; 250 } 251 } 252 } 253 254 /** \brief Set whether to use reciprocal correspondence or not 255 * 256 * \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence 257 * or not 258 */ 259 inline void setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)260 setUseReciprocalCorrespondences(bool use_reciprocal_correspondence) 261 { 262 use_reciprocal_correspondence_ = use_reciprocal_correspondence; 263 } 264 265 /** \brief Obtain whether reciprocal correspondence are used or not */ 266 inline bool getUseReciprocalCorrespondences()267 getUseReciprocalCorrespondences() const 268 { 269 return (use_reciprocal_correspondence_); 270 } 271 272 protected: 273 /** \brief Apply a rigid transform to a given dataset. Here we check whether 274 * the dataset has surface normals in addition to XYZ, and rotate normals as well. 275 * \param[in] input the input point cloud 276 * \param[out] output the resultant output point cloud 277 * \param[in] transform a 4x4 rigid transformation 278 * \note Can be used with cloud_in equal to cloud_out 279 */ 280 virtual void 281 transformCloud(const PointCloudSource& input, 282 PointCloudSource& output, 283 const Matrix4& transform); 284 285 /** \brief Rigid transformation computation method with initial guess. 286 * \param output the transformed input point cloud dataset using the rigid 287 * transformation found \param guess the initial guess of the transformation to 288 * compute 289 */ 290 void 291 computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 292 293 /** \brief Looks at the Estimators and Rejectors and determines whether their 294 * blob-setter methods need to be called */ 295 virtual void 296 determineRequiredBlobData(); 297 298 /** \brief XYZ fields offset. */ 299 std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_; 300 301 /** \brief Normal fields offset. */ 302 std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_; 303 304 /** \brief The correspondence type used for correspondence estimation. */ 305 bool use_reciprocal_correspondence_; 306 307 /** \brief Internal check whether source dataset has normals or not. */ 308 bool source_has_normals_; 309 /** \brief Internal check whether target dataset has normals or not. */ 310 bool target_has_normals_; 311 312 /** \brief Checks for whether estimators and rejectors need various data */ 313 bool need_source_blob_, need_target_blob_; 314 }; 315 316 /** \brief @b IterativeClosestPointWithNormals is a special case of 317 * IterativeClosestPoint, that uses a transformation estimated based on 318 * Point to Plane distances by default. 319 * 320 * By default, this implementation uses the traditional point to plane objective 321 * and computes point to plane distances using the normals of the target point 322 * cloud. It also provides the option (through setUseSymmetricObjective) of 323 * using the symmetric objective function of [Rusinkiewicz 2019]. This objective 324 * uses the normals of both the source and target point cloud and has a similar 325 * computational cost to the traditional point to plane objective while also 326 * offering improved convergence speed and a wider basin of convergence. 327 * 328 * Note that this implementation not demean the point clouds which can lead 329 * to increased numerical error. If desired, a user can demean the point cloud, 330 * run iterative closest point, and composite the resulting ICP transformation 331 * with the translations from demeaning to obtain a transformation between 332 * the original point clouds. 333 * 334 * \author Radu B. Rusu, Matthew Cong 335 * \ingroup registration 336 */ 337 template <typename PointSource, typename PointTarget, typename Scalar = float> 338 class IterativeClosestPointWithNormals 339 : public IterativeClosestPoint<PointSource, PointTarget, Scalar> { 340 public: 341 using PointCloudSource = typename IterativeClosestPoint<PointSource, 342 PointTarget, 343 Scalar>::PointCloudSource; 344 using PointCloudTarget = typename IterativeClosestPoint<PointSource, 345 PointTarget, 346 Scalar>::PointCloudTarget; 347 using Matrix4 = 348 typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4; 349 350 using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_; 351 using IterativeClosestPoint<PointSource, PointTarget, Scalar>:: 352 transformation_estimation_; 353 using IterativeClosestPoint<PointSource, PointTarget, Scalar>:: 354 correspondence_rejectors_; 355 356 using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>; 357 using ConstPtr = 358 shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>; 359 360 /** \brief Empty constructor. */ IterativeClosestPointWithNormals()361 IterativeClosestPointWithNormals() 362 { 363 reg_name_ = "IterativeClosestPointWithNormals"; 364 setUseSymmetricObjective(false); 365 setEnforceSameDirectionNormals(true); 366 // correspondence_rejectors_.add 367 }; 368 369 /** \brief Empty destructor */ ~IterativeClosestPointWithNormals()370 virtual ~IterativeClosestPointWithNormals() {} 371 372 /** \brief Set whether to use a symmetric objective function or not 373 * 374 * \param[in] use_symmetric_objective whether to use a symmetric objective function or 375 * not 376 */ 377 inline void setUseSymmetricObjective(bool use_symmetric_objective)378 setUseSymmetricObjective(bool use_symmetric_objective) 379 { 380 use_symmetric_objective_ = use_symmetric_objective; 381 if (use_symmetric_objective_) { 382 auto symmetric_transformation_estimation = pcl::make_shared< 383 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS< 384 PointSource, 385 PointTarget, 386 Scalar>>(); 387 symmetric_transformation_estimation->setEnforceSameDirectionNormals( 388 enforce_same_direction_normals_); 389 transformation_estimation_ = symmetric_transformation_estimation; 390 } 391 else { 392 transformation_estimation_.reset( 393 new pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, 394 PointTarget, 395 Scalar>()); 396 } 397 } 398 399 /** \brief Obtain whether a symmetric objective is used or not */ 400 inline bool getUseSymmetricObjective()401 getUseSymmetricObjective() const 402 { 403 return use_symmetric_objective_; 404 } 405 406 /** \brief Set whether or not to negate source or target normals on a per-point basis 407 * such that they point in the same direction. Only applicable to the symmetric 408 * objective function. 409 * 410 * \param[in] enforce_same_direction_normals whether to negate source or target 411 * normals on a per-point basis such that they point in the same direction. 412 */ 413 inline void setEnforceSameDirectionNormals(bool enforce_same_direction_normals)414 setEnforceSameDirectionNormals(bool enforce_same_direction_normals) 415 { 416 enforce_same_direction_normals_ = enforce_same_direction_normals; 417 auto symmetric_transformation_estimation = dynamic_pointer_cast< 418 pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<PointSource, 419 PointTarget, 420 Scalar>>( 421 transformation_estimation_); 422 if (symmetric_transformation_estimation) 423 symmetric_transformation_estimation->setEnforceSameDirectionNormals( 424 enforce_same_direction_normals_); 425 } 426 427 /** \brief Obtain whether source or target normals are negated on a per-point basis 428 * such that they point in the same direction or not */ 429 inline bool getEnforceSameDirectionNormals()430 getEnforceSameDirectionNormals() const 431 { 432 return enforce_same_direction_normals_; 433 } 434 435 protected: 436 /** \brief Apply a rigid transform to a given dataset 437 * \param[in] input the input point cloud 438 * \param[out] output the resultant output point cloud 439 * \param[in] transform a 4x4 rigid transformation 440 * \note Can be used with cloud_in equal to cloud_out 441 */ 442 virtual void 443 transformCloud(const PointCloudSource& input, 444 PointCloudSource& output, 445 const Matrix4& transform); 446 447 /** \brief Type of objective function (asymmetric vs. symmetric) used for transform 448 * estimation */ 449 bool use_symmetric_objective_; 450 /** \brief Whether or not to negate source and/or target normals such that they point 451 * in the same direction in the symmetric objective function */ 452 bool enforce_same_direction_normals_; 453 }; 454 455 } // namespace pcl 456 457 #include <pcl/registration/impl/icp.hpp> 458