icp_correspondences_check.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Hozefa Indorewala <indorewala@ias.in.tum.de>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Based on orignal code written by Radu Rusu.
00032  * Modified by Hozefa Indorewala
00033  */
00034 #include <pointcloud_registration/icp/registration_correspondences_check.h>
00035 
00036 #ifndef _ICP_CORRESPONDENCES_CHECK_H_
00037 #define _ICP_CORRESPONDENCES_CHECK_H_
00038 
00039 namespace pcl
00040 {
00044 
00045   template <typename PointSource, typename PointTarget>
00046   class IterativeClosestPointCorrespondencesCheck : public RegistrationCorrespondencesCheck<PointSource, PointTarget>
00047   {
00048     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::reg_name_;
00049     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::getClassName;
00050     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::input_;
00051     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::indices_;
00052     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::target_;
00053     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::nr_iterations_;
00054     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::max_iterations_;
00055     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::previous_transformation_;
00056     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::final_transformation_;
00057     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::transformation_;
00058     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::transformation_epsilon_;
00059     using RegistrationCorrespondencesCheck<PointSource, PointTarget>::converged_;
00060 
00061     typedef typename RegistrationCorrespondencesCheck<PointSource, PointTarget>::PointCloudSource PointCloudSource;
00062     typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00063     typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00064 
00065     typedef typename RegistrationCorrespondencesCheck<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
00066 
00067     typedef PointIndices::Ptr PointIndicesPtr;
00068     typedef PointIndices::ConstPtr PointIndicesConstPtr;
00069 
00070     double radius_, epsilon_z_, epsilon_curvature_;
00071     int max_nn_;
00072     bool curvature_check_;
00073     time_t start, end;
00074     std::string field_;
00075     public:
00077 
00079       IterativeClosestPointCorrespondencesCheck(){reg_name_ = "IterativeClosestPointCorrespondencesCheck";};
00081     IterativeClosestPointCorrespondencesCheck ( double radius, int max_nn, double epsilon_z, double epsilon_curvature, bool curvature_check):
00082                                                   radius_(radius),
00083                                                   max_nn_(max_nn),
00084                                                   epsilon_z_(epsilon_z),
00085                                                   epsilon_curvature_(epsilon_curvature),
00086                                                   curvature_check_(curvature_check){reg_name_ = "IterativeClosestPointCorrespondencesCheck";};
00087 
00089 
00090       virtual ~IterativeClosestPointCorrespondencesCheck () {};
00091 
00092     protected:
00093 
00095 
00098       virtual void computeTransformation (PointCloudSource &output)
00099       {
00100         // Allocate enough space to hold the results
00101         std::vector<int> nn_indices (max_nn_);
00102         std::vector<float> nn_dists (max_nn_);
00103 
00104         int count = 0;
00105         this->nr_iterations_ = 0;
00106         while (!this->converged_)           // repeat until convergence
00107         {
00108           ROS_INFO("[IterativeClosestPointCorrespondencesCheck:] Iteration Number: %d", this->nr_iterations_);
00109           // Point cloud containing the correspondences of each point in <input, indices>
00110           PointCloudTarget model_corresp;
00111           PointCloudSource source_corresp;
00112 
00113           count = 0;
00114           source_corresp.points.reserve(this->indices_->size());
00115           model_corresp.points.reserve(this->indices_->size());
00116           // Save the previously estimated transformation
00117           this->previous_transformation_ = this->transformation_;
00118           ROS_INFO("[IterativeClosestPointCorrespondencesCheck:] finding correpondences for %ld points, %d, %lf", 
00119                    this->indices_->size(), max_nn_, radius_);
00120           start = time(NULL);
00121           // Iterating over the entire index vector and  find all correspondences
00122           for (size_t idx = 0; idx < this->indices_->size (); idx++)
00123           //for( std::vector<int>::iterator it = this->indices_->begin(); it != this->indices_->end(); ++it)
00124           {
00125             // Use radius search to look for neighbors within a user specified radius
00126             if (!searchForNeighbors (output, idx, radius_, max_nn_, nn_indices, nn_dists))
00127             {
00128               //ROS_INFO("No neigbor found for idx = %d", (int) idx);
00129               continue;
00130             }
00131             for(size_t i = 0 ; i < nn_indices.size(); i++)
00132             {
00133               // Check if the difference between the z coordinates is within user specified limits
00134               if(fabs(output.points[idx].z - this->target_->points[nn_indices[i]].z) < epsilon_z_ && field_ == "z") 
00135               {
00136                 if(curvature_check_)
00137                 {
00138                   // Check if the difference between the curvature values is within user specified limits
00139                   if(fabs(output.points[idx].curvature - this->target_->points[nn_indices[i]].curvature) > epsilon_curvature_)
00140                   {
00141                     continue;
00142                   }
00143                 }
00144                 count++;
00145                 source_corresp.points.push_back (output.points[idx]);
00146                 model_corresp.points.push_back (this->target_->points[nn_indices[i]]);
00147                 break;
00148               }
00149               else if(fabs(output.points[idx].y - this->target_->points[nn_indices[i]].y) < epsilon_z_ && field_ == "y") 
00150               {
00151                 if(curvature_check_)
00152                 {
00153                   // Check if the difference between the curvature values is within user specified limits
00154                   if(fabs(output.points[idx].curvature - this->target_->points[nn_indices[i]].curvature) > epsilon_curvature_)
00155                   {
00156                     continue;
00157                   }
00158                 }
00159                 count++;
00160                 source_corresp.points.push_back (output.points[idx]);
00161                 model_corresp.points.push_back (this->target_->points[nn_indices[i]]);
00162                 break;
00163               }
00164               else if(fabs(output.points[idx].x - this->target_->points[nn_indices[i]].x) < epsilon_z_ && field_ == "x") 
00165               {
00166                 if(curvature_check_)
00167                 {
00168                   // Check if the difference between the curvature values is within user specified limits
00169                   if(fabs(output.points[idx].curvature - this->target_->points[nn_indices[i]].curvature) > epsilon_curvature_)
00170                   {
00171                     continue;
00172                   }
00173                 }
00174                 count++;
00175                 source_corresp.points.push_back (output.points[idx]);
00176                 model_corresp.points.push_back (this->target_->points[nn_indices[i]]);
00177                 break;
00178               }
00179               else
00180               {
00181 //                 if (field_ == "x")
00182 //                   ROS_WARN("[IterativeClosestPointCorrespondencesCheck:] field_ %s. fabs(output.points[idx].x - this->target_->points[nn_indices[i]].x): %lf", field_.c_str(), fabs(output.points[idx].x - this->target_->points[nn_indices[i]].x));
00183 //                 if (field_ == "y")
00184 //                   ROS_WARN("[IterativeClosestPointCorrespondencesCheck:] field_ %s. fabs(output.points[idx].y - this->target_->points[nn_indices[i]].y): %lf", field_.c_str(), fabs(output.points[idx].y - this->target_->points[nn_indices[i]].y));
00185 //                 if (field_ == "z")
00186 //                   ROS_WARN("[IterativeClosestPointCorrespondencesCheck:] field_ %s. fabs(output.points[idx].z - this->target_->points[nn_indices[i]].z): %lf", field_.c_str(), fabs(output.points[idx].z - this->target_->points[nn_indices[i]].z));
00187 // //                return;
00188               }
00189             }
00190           }
00191           end = time(NULL);
00192           ROS_INFO("[IterativeClosestPointCorrespondencesCheck:] found correpondences in %d seconds", (int)(end - start));
00193 
00194 
00195           if(source_corresp.points.size() == 0)
00196           {
00197             ROS_ERROR("[IterativeClosestPointCorrespondencesCheck:] No correspondences found. Try to relax the conditions.", getClassName().c_str());
00198             return;
00199           }
00200           //ROS_INFO("Correspondences: %d", count);
00201           // Zero the Z coordinate value since the robot moved in the x & y plane only
00202           for(size_t i = 0; i < source_corresp.points.size(); i++)
00203           {
00204             if (field_ == "z")
00205             {
00206               source_corresp.points[i].z = 0.0;
00207               model_corresp.points[i].z = 0.0;
00208             }
00209             else if (field_ == "y")
00210             {
00211               source_corresp.points[i].y = 0.0;
00212               model_corresp.points[i].y = 0.0;
00213             }
00214             else if (field_ == "x")
00215             {
00216               source_corresp.points[i].x = 0.0;
00217               model_corresp.points[i].x = 0.0;
00218             }
00219             else
00220             {
00221               ROS_WARN("[IterativeClosestPointCorrespondencesCheck:] Unknown field_ %s. Has to be x, y or z.", field_.c_str());
00222               return;
00223             }
00224           }
00225 
00226           start = time(NULL);
00227           // Estimate the transform
00228           estimateRigidTransformationSVD (source_corresp, model_corresp, this->transformation_);
00229           end = time(NULL);
00230           ROS_INFO("[IterativeClosestPointCorrespondencesCheck:] estimateRigidTransformationSVD in %d seconds", (int)(end - start));
00231 
00232           // Tranform the data
00233           transformPointCloud (output, output, this->transformation_);
00234 
00235           // Obtain the final transformation
00236           this->final_transformation_ = this->transformation_ * this->final_transformation_;
00237 
00238           //Compute transformation change
00239           double transformation_change = fabs ((this->transformation_ - this->previous_transformation_).sum ());
00240 
00241           //ROS_INFO("Transformation change: %f", transformation_change);
00242 
00243           this->nr_iterations_++;
00244           ROS_INFO("[IterativeClosestPointCorrespondencesCheck] number of iterations: %d", this->nr_iterations_);
00245           // Check for convergence
00246           if (this->nr_iterations_ >= this->max_iterations_ ||
00247               transformation_change < this->transformation_epsilon_)
00248           {
00249             this->converged_ = true;
00250             ROS_INFO ("[IterativeClosestPointCorrespondencesCheck:] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %g",
00251                       getClassName().c_str(), this->nr_iterations_, this->max_iterations_, fabs ((this->transformation_ - this->previous_transformation_).sum ()));
00252           }
00253         }
00254       }
00255 
00256     public:
00258 
00264       void estimateRigidTransformationSVD (const PointCloudSource &cloud_src, const PointCloudTarget &cloud_tgt, Eigen::Matrix4f &transformation_matrix)
00265       {
00266         ROS_ASSERT (cloud_src.points.size () == cloud_tgt.points.size ());
00267 
00268         // <cloud_src,cloud_src> is the source dataset
00269         transformation_matrix.setIdentity ();
00270 
00271         Eigen::Vector4f centroid_src, centroid_tgt;
00272         // Estimate the centroids of source, target
00273         compute3DCentroid (cloud_src, centroid_src);
00274         compute3DCentroid (cloud_tgt, centroid_tgt);
00275 
00276         // Subtract the centroids from source, target
00277         Eigen::MatrixXf cloud_src_demean;
00278         demeanPointCloud (cloud_src, centroid_src, cloud_src_demean);
00279 
00280 
00281         Eigen::MatrixXf cloud_tgt_demean;
00282         demeanPointCloud (cloud_tgt, centroid_tgt, cloud_tgt_demean);
00283 
00284         // Assemble the correlation matrix H = source * target'
00285         Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
00286 
00287         // Compute the Singular Value Decomposition
00288         Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
00289         Eigen::Matrix3f u = svd.matrixU ();
00290         Eigen::Matrix3f v = svd.matrixV ();
00291 
00292         // Compute R = V * U'
00293         //if (u.determinant () * v.determinant () < 0)
00294         //{
00295         //  ROS_WARN ("[pcl::estimateRigidTransformationSVD] Determinant < 0!");
00296         //  for (int x = 0; x < 3; ++x)
00297         //    v (x, 2) *= -1;
00298         //}
00299 
00300         Eigen::Matrix3f R = v * u.transpose ();
00301 
00302         // Return the correct transformation
00303         transformation_matrix.topLeftCorner<3, 3> () = R;
00304         Eigen::Vector3f Rc = R * centroid_src.head<3> ();
00305         transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
00306 
00307         // Assemble the correlation matrix H = source' * target
00308         //Eigen::Matrix3f H = (cloud_src_demean.transpose () * cloud_tgt_demean).corner<3, 3>(Eigen::TopLeft);
00309 
00310         // Compute the Singular Value Decomposition
00311         //Eigen::SVD<Eigen::Matrix3f> svd (H);
00312         //Eigen::Matrix3f u = svd.matrixU ();
00313         //Eigen::Matrix3f v = svd.matrixV ();
00314 
00315         // Compute R = V * U'
00316         //if (u.determinant () * v.determinant () < 0)
00317           //for (int x = 0; x < 3; ++x)
00318             //v (x, 2) *= -1;
00319 
00320         //Eigen::Matrix3f R = u * v.transpose ();
00321         //Eigen::Matrix3f Rinv;
00322         //R.computeInverse (&Rinv);
00323         // Return the correct transformation
00324         //transformation_matrix.corner<3, 3> (Eigen::TopLeft) = Rinv;
00325         //Eigen::Vector3f Rc = Rinv * centroid_src.start<3> ();
00326         //transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.start<3> () - Rc;
00327       }
00328 
00330 
00331     void setParameters( double radius, int max_nn, double epsilon_z, double epsilon_curvature, bool curvature_check, std::string field)
00332       {
00333         radius_ = radius;
00334         max_nn_ = max_nn;
00335         epsilon_z_ = epsilon_z;
00336         epsilon_curvature_ = epsilon_curvature;
00337         curvature_check_ = curvature_check;
00338         field_ = field;
00339       }
00340   };
00341 }
00342 
00343 #endif  //#ifndef _ICP_CORRESPONDENCES_CHECK_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pointcloud_registration
Author(s): Hozefa Indorewala
autogenerated on Sun Oct 6 2013 11:55:57