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-2011, 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_REGISTRATION_IMPL_ICP_HPP_ 00042 #define PCL_REGISTRATION_IMPL_ICP_HPP_ 00043 00044 #include <pcl/registration/boost.h> 00045 #include <pcl/correspondence.h> 00046 00047 /////////////////////////////////////////////////////////////////////////////////////////// 00048 template <typename PointSource, typename PointTarget, typename Scalar> void 00049 pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud ( 00050 const PointCloudSource &input, 00051 PointCloudSource &output, 00052 const Matrix4 &transform) 00053 { 00054 Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t; 00055 Eigen::Matrix4f tr = transform.template cast<float> (); 00056 00057 // XYZ is ALWAYS present due to the templatization, so we only have to check for normals 00058 if (source_has_normals_) 00059 { 00060 Eigen::Vector3f nt, nt_t; 00061 Eigen::Matrix3f rot = tr.block<3, 3> (0, 0); 00062 00063 for (size_t i = 0; i < input.size (); ++i) 00064 { 00065 const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]); 00066 uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]); 00067 memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float)); 00068 memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float)); 00069 memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float)); 00070 00071 if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2])) 00072 continue; 00073 00074 pt_t = tr * pt; 00075 00076 memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float)); 00077 memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float)); 00078 memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float)); 00079 00080 memcpy (&nt[0], data_in + nx_idx_offset_, sizeof (float)); 00081 memcpy (&nt[1], data_in + ny_idx_offset_, sizeof (float)); 00082 memcpy (&nt[2], data_in + nz_idx_offset_, sizeof (float)); 00083 00084 if (!pcl_isfinite (nt[0]) || !pcl_isfinite (nt[1]) || !pcl_isfinite (nt[2])) 00085 continue; 00086 00087 nt_t = rot * nt; 00088 00089 memcpy (data_out + nx_idx_offset_, &nt_t[0], sizeof (float)); 00090 memcpy (data_out + ny_idx_offset_, &nt_t[1], sizeof (float)); 00091 memcpy (data_out + nz_idx_offset_, &nt_t[2], sizeof (float)); 00092 } 00093 } 00094 else 00095 { 00096 for (size_t i = 0; i < input.size (); ++i) 00097 { 00098 const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]); 00099 uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]); 00100 memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float)); 00101 memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float)); 00102 memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float)); 00103 00104 if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2])) 00105 continue; 00106 00107 pt_t = tr * pt; 00108 00109 memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float)); 00110 memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float)); 00111 memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float)); 00112 } 00113 } 00114 00115 } 00116 00117 /////////////////////////////////////////////////////////////////////////////////////////// 00118 template <typename PointSource, typename PointTarget, typename Scalar> void 00119 pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation ( 00120 PointCloudSource &output, const Matrix4 &guess) 00121 { 00122 // Point cloud containing the correspondences of each point in <input, indices> 00123 PointCloudSourcePtr input_transformed (new PointCloudSource); 00124 00125 nr_iterations_ = 0; 00126 converged_ = false; 00127 00128 // Initialise final transformation to the guessed one 00129 final_transformation_ = guess; 00130 00131 // If the guessed transformation is non identity 00132 if (guess != Matrix4::Identity ()) 00133 { 00134 input_transformed->resize (input_->size ()); 00135 // Apply guessed transformation prior to search for neighbours 00136 transformCloud (*input_, *input_transformed, guess); 00137 } 00138 else 00139 *input_transformed = *input_; 00140 00141 transformation_ = Matrix4::Identity (); 00142 00143 // Pass in the default target for the Correspondence Estimation/Rejection code 00144 correspondence_estimation_->setInputTarget (target_); 00145 // We should be doing something like this 00146 // for (size_t i = 0; i < correspondence_rejectors_.size (); ++i) 00147 // { 00148 // correspondence_rejectors_[i]->setTargetCloud (target_); 00149 // if (target_has_normals_) 00150 // correspondence_rejectors_[i]->setTargetNormals (target_); 00151 // } 00152 00153 convergence_criteria_->setMaximumIterations (max_iterations_); 00154 convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_); 00155 convergence_criteria_->setTranslationThreshold (transformation_epsilon_); 00156 convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_); 00157 00158 // Repeat until convergence 00159 do 00160 { 00161 // Save the previously estimated transformation 00162 previous_transformation_ = transformation_; 00163 00164 // Set the source each iteration, to ensure the dirty flag is updated 00165 correspondence_estimation_->setInputSource (input_transformed); 00166 // Estimate correspondences 00167 if (use_reciprocal_correspondence_) 00168 correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_); 00169 else 00170 correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_); 00171 00172 //if (correspondence_rejectors_.empty ()) 00173 CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_)); 00174 for (size_t i = 0; i < correspondence_rejectors_.size (); ++i) 00175 { 00176 PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", correspondence_rejectors_[i]->getClassName ().c_str ()); 00177 // We should be doing something like this 00178 // correspondence_rejectors_[i]->setInputSource (input_transformed); 00179 // if (source_has_normals_) 00180 // correspondence_rejectors_[i]->setInputNormals (input_transformed); 00181 correspondence_rejectors_[i]->setInputCorrespondences (temp_correspondences); 00182 correspondence_rejectors_[i]->getCorrespondences (*correspondences_); 00183 // Modify input for the next iteration 00184 if (i < correspondence_rejectors_.size () - 1) 00185 *temp_correspondences = *correspondences_; 00186 } 00187 00188 size_t cnt = correspondences_->size (); 00189 // Check whether we have enough correspondences 00190 if (cnt < min_number_correspondences_) 00191 { 00192 PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); 00193 convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES); 00194 converged_ = false; 00195 break; 00196 } 00197 00198 // Estimate the transform 00199 transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_); 00200 00201 // Tranform the data 00202 transformCloud (*input_transformed, *input_transformed, transformation_); 00203 00204 // Obtain the final transformation 00205 final_transformation_ = transformation_ * final_transformation_; 00206 00207 ++nr_iterations_; 00208 00209 // Update the vizualization of icp convergence 00210 //if (update_visualizer_ != 0) 00211 // update_visualizer_(output, source_indices_good, *target_, target_indices_good ); 00212 00213 converged_ = static_cast<bool> ((*convergence_criteria_)); 00214 } 00215 while (!converged_); 00216 00217 // Transform the input cloud using the final transformation 00218 PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n", 00219 final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3), 00220 final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3), 00221 final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3), 00222 final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3)); 00223 00224 // Copy all the values 00225 output = *input_; 00226 // Transform the XYZ + normals 00227 transformCloud (*input_, output, final_transformation_); 00228 } 00229 00230 /////////////////////////////////////////////////////////////////////////////////////////// 00231 template <typename PointSource, typename PointTarget, typename Scalar> void 00232 pcl::IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar>::transformCloud ( 00233 const PointCloudSource &input, 00234 PointCloudSource &output, 00235 const Matrix4 &transform) 00236 { 00237 pcl::transformPointCloudWithNormals (input, output, transform); 00238 } 00239 00240 #endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */