icp.hpp
Go to the documentation of this file.
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 
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 
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 
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_ */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:53