Point Cloud Library (PCL)  1.3.1
gicp.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: gicp.hpp 3042 2011-11-01 04:44:47Z svn $
00035  *
00036  */
00037 
00038 #include <boost/unordered_map.hpp>
00039 
00041 template <typename PointSource, typename PointTarget> 
00042 template<typename PointT> void
00043 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 
00044                                                                                     const typename pcl::KdTree<PointT>::Ptr kdtree,
00045                                                                                     std::vector<Eigen::Matrix3d>& cloud_covariances)
00046 {
00047   if((size_t)k_correspondences_ > cloud->size ())
00048   {
00049     PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", (unsigned long)cloud->size (), (unsigned long)k_correspondences_);
00050     return;
00051   }
00052 
00053   Eigen::Vector3d mean;
00054   std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
00055   std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
00056 
00057   // We should never get there but who knows
00058   if(cloud_covariances.size () < cloud->size ())
00059     cloud_covariances.reserve (cloud->size ());
00060 
00061   typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
00062   std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
00063   for(;
00064       points_iterator != cloud->end ();
00065       ++points_iterator, ++matrices_iterator)
00066   {
00067     const PointT &query_point = *points_iterator;
00068     Eigen::Matrix3d &cov = *matrices_iterator;
00069     // Zero out the cov and mean
00070     cov.setZero ();
00071     mean.setZero ();
00072 
00073     // Search for the K nearest neighbours
00074     kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
00075     
00076     // Find the covariance matrix
00077     for(int j = 0; j < k_correspondences_; j++) {
00078       const PointT &pt = (*cloud)[nn_indecies[j]];
00079       
00080       mean[0] += pt.x;
00081       mean[1] += pt.y;
00082       mean[2] += pt.z;
00083       
00084       cov(0,0) += pt.x*pt.x;
00085       
00086       cov(1,0) += pt.y*pt.x;
00087       cov(1,1) += pt.y*pt.y;
00088       
00089       cov(2,0) += pt.z*pt.x;
00090       cov(2,1) += pt.z*pt.y;
00091       cov(2,2) += pt.z*pt.z;    
00092     }
00093   
00094     mean/= (double)k_correspondences_;
00095     // Get the actual covariance
00096     for(int k = 0; k < 3; k++) {
00097       for(int l = 0; l <= k; l++) {
00098         cov(k,l) /= (double)k_correspondences_;
00099         cov(k,l) -= mean[k]*mean[l];
00100         cov(l,k) = cov(k,l);
00101       }
00102     }
00103     
00104     // Compute the SVD (covariance matrix is symmetric so U = V')
00105     Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
00106     cov.setZero ();
00107     Eigen::Matrix3d U = svd.matrixU ();
00108     // Reconstitute the covariance matrix with modified singular values using the column     // vectors in V.
00109     for(int k = 0; k < 3; k++) {
00110       Eigen::Vector3d col = U.col(k);
00111       double v = 1.; // biggest 2 singular values replaced by 1
00112       if(k == 2)   // smallest singular value replaced by gicp_epsilon
00113         v = gicp_epsilon_;
00114       cov+= v * col * col.transpose(); 
00115     }
00116   }
00117 }
00118 
00120 template <typename PointSource, typename PointTarget> void
00121 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeRDerivative(const double x[], const Eigen::Matrix3d &R, double g[]) 
00122 {
00123   Eigen::Matrix3d dR_dPhi;
00124   Eigen::Matrix3d dR_dTheta;
00125   Eigen::Matrix3d dR_dPsi;
00126 
00127   double phi = x[3], theta = x[4], psi = x[5];
00128   
00129   double cphi = cos(phi), sphi = sin(phi);
00130   double ctheta = cos(theta), stheta = sin(theta);
00131   double cpsi = cos(psi), spsi = sin(psi);
00132       
00133   dR_dPhi(0,0) = 0.;
00134   dR_dPhi(1,0) = 0.;
00135   dR_dPhi(2,0) = 0.;
00136 
00137   dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
00138   dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
00139   dR_dPhi(2,1) = cphi*ctheta;
00140 
00141   dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
00142   dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
00143   dR_dPhi(2,2) = -ctheta*sphi;
00144 
00145   dR_dTheta(0,0) = -cpsi*stheta;
00146   dR_dTheta(1,0) = -spsi*stheta;
00147   dR_dTheta(2,0) = -ctheta;
00148 
00149   dR_dTheta(0,1) = cpsi*ctheta*sphi;
00150   dR_dTheta(1,1) = ctheta*sphi*spsi;
00151   dR_dTheta(2,1) = -sphi*stheta;
00152 
00153   dR_dTheta(0,2) = cphi*cpsi*ctheta;
00154   dR_dTheta(1,2) = cphi*ctheta*spsi;
00155   dR_dTheta(2,2) = -cphi*stheta;
00156 
00157   dR_dPsi(0,0) = -ctheta*spsi;
00158   dR_dPsi(1,0) = cpsi*ctheta;
00159   dR_dPsi(2,0) = 0.;
00160 
00161   dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
00162   dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
00163   dR_dPsi(2,1) = 0.;
00164 
00165   dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
00166   dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
00167   dR_dPsi(2,2) = 0.;
00168       
00169   // set d/d_rx = tr(dR_dPhi'*gsl_temp_mat_r) [= <dR_dPhi, gsl_temp_mat_r>]
00170   g[3] = matricesInnerProd(dR_dPhi, R);
00171   // set d/d_ry = tr(dR_dTheta'*gsl_temp_mat_r) = [<dR_dTheta, gsl_temp_mat_r>]
00172   g[4] = matricesInnerProd(dR_dTheta, R);
00173   // set d/d_rz = tr(dR_dPsi'*gsl_temp_mat_r) = [<dR_dPsi, gsl_temp_mat_r>]
00174   g[5] = matricesInnerProd(dR_dPsi, R);
00175 }
00176 
00178 template <typename PointSource, typename PointTarget> void
00179 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationLM (
00180       const PointCloudSource &cloud_src, const PointCloudTarget &cloud_tgt, Eigen::Matrix4f &transformation_matrix)
00181 {
00182   boost::mutex::scoped_lock lock (tmp_mutex_);
00183 
00184   static const int n_unknowns = 6;      // 6 unknowns: 3 translation + 3 rotation (quaternion)
00185 
00186   if (cloud_src.size () != cloud_tgt.size ())
00187   {
00188     PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationLM] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)cloud_src.size (), (unsigned long)cloud_tgt.size ());
00189     return;
00190   }
00191   if (cloud_src.size () < 4)     // need at least 4 samples
00192   {
00193     PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationLM] Need at least 4 points to estimate a transform! Source and target have %lu points!\n", (unsigned long)cloud_src.size ());
00194     return;
00195   }
00196 
00197   int m = cloud_src.size ();
00198   double *fvec = new double[m];
00199   double *fjac = new double[m*n_unknowns];
00200   int *iwa = new int[n_unknowns];
00201   int lwa = m * n_unknowns + 5 * n_unknowns + m;
00202   double *wa = new double[lwa];
00203   int ldfjac = m;
00204 
00205   // Set the initial solution
00206   double *x = new double[n_unknowns];
00207   // Translation estimates - initial guess
00208   x[0] = 0; x[1] = 0; x[2] = 0;
00209   // Rotation estimates - initial guess quaternion: rx-ry-rz
00210   x[3] = 0; x[4] = 0; x[5] = 0;
00211 
00212   // Set temporary pointers
00213   tmp_src_ = &cloud_src;
00214   tmp_tgt_ = &cloud_tgt;
00215 
00216   // Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
00217   double tol = sqrt (dpmpar (1));
00218 
00219   // Optimize using forward-difference approximation LM
00220   int info = lmder1 (&pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::functionToOptimize, this, m, n_unknowns, x, fvec, fjac, ldfjac, tol, iwa, wa, lwa);
00221 
00222   // Compute the norm of the residuals
00223   PCL_DEBUG ("[pcl::%s::estimateRigidTransformationLM] LM solver finished with exit code %i, having a residual norm of %g. \n",
00224              //"\nFinal solution: [%f %f %f %f] [%f %f %f]", 
00225              getClassName ().c_str (), info, enorm (m, fvec));
00226              //x[0], x[1], x[2], x[3], x[4], x[5], x[6]);
00227 
00228   delete [] wa; delete [] fvec; delete [] fjac;
00229   // Return the correct transformation
00230   transformation_matrix.setIdentity();
00231   apply_state(transformation_matrix, x);
00232   tmp_src_ = tmp_tgt_ = NULL;
00233 
00234   delete[] iwa;
00235   delete[] x;
00236 }
00237 
00239 template <typename PointSource, typename PointTarget> void
00240   pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationLM (
00241       const PointCloudSource &cloud_src, const std::vector<int> &indices_src, const PointCloudTarget &cloud_tgt, const std::vector<int> &indices_tgt, Eigen::Matrix4f &transformation_matrix)
00242 {
00243   boost::mutex::scoped_lock lock (tmp_mutex_);
00244 
00245   static const int n_unknowns = 6;      // 6 unknowns:  3 translation + 3 rotation (quaternion)
00246 
00247   if (indices_src.size () != indices_tgt.size ())
00248   {
00249     PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationLM] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
00250     return;
00251   }
00252   if (indices_src.size () < 4)     // need at least 4 samples
00253   {
00254     PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationLM] Need at least 4 points to estimate a transform! Source and target have %lu points!", (unsigned long)indices_src.size ());
00255     return;
00256   }
00257 
00258   int m = indices_src.size ();
00259   double* fvec = new double[m];
00260   double* fjac = new double[m * n_unknowns];
00261   int *iwa = new int[n_unknowns];
00262   int lwa = m * n_unknowns + 5 * n_unknowns + m;
00263   double *wa = new double[lwa];
00264   int ldfjac = m;
00265   // Set the initial solution
00266   double *x = new double[n_unknowns];
00267   get_translation(transformation_matrix, x[0], x[1], x[2]);
00268   get_rotation(transformation_matrix, x[3], x[4], x[5]);
00269   // Set temporary pointers
00270   tmp_src_ = &cloud_src;
00271   tmp_tgt_ = &cloud_tgt;
00272   tmp_idx_src_ = &indices_src;
00273   tmp_idx_tgt_ = &indices_tgt;
00274 
00275   // Set tol to the square root of the machine. Unless high solutions are required, these are the recommended settings.
00276   double tol = sqrt (dpmpar (1));
00277 
00278   // Optimize using forward-difference approximation LM
00279   int info = lmder1 (&pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::functionToOptimizeIndices, this, m, n_unknowns, x, fvec, fjac, ldfjac, tol, iwa, wa, lwa);
00280 
00281   // Compute the norm of the residuals
00282   PCL_DEBUG ("[pcl::%s::estimateRigidTransformationLM] LM solver finished with exit code %i, having a residual norm of %g. \n",
00283              //"\nFinal solution: [%f %f %f %f] [%f %f %f]", 
00284              getClassName ().c_str (), info, enorm (m, fvec));
00285              //x[0], x[1], x[2], x[3], x[4], x[5], x[6]);
00286 
00287   delete [] wa; delete [] fvec; delete [] fjac;
00288 
00289   // Return the correct transformation
00290   transformation_matrix.setIdentity();
00291   apply_state(transformation_matrix, x);
00292   tmp_src_ = tmp_tgt_ = NULL;
00293 
00294   tmp_src_ = tmp_tgt_ = NULL;
00295   tmp_idx_src_ = tmp_idx_tgt_ = NULL;
00296 
00297   delete[] iwa;
00298   delete[] x;
00299 }
00300 
00302 template <typename PointSource, typename PointTarget> inline int
00303 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::functionToOptimize (void *p, int m, int n, const double *x, double *fvec, double* fjac, int ldfjac, int iflag)
00304 {
00305   GeneralizedIterativeClosestPoint *model = (GeneralizedIterativeClosestPoint*)p;
00306 
00307   // Copy the rotation and translation components
00308   Eigen::Vector4f t (x[0], x[1], x[2], 1.0);
00309   // Compute w from the unit quaternion
00310   Eigen::Quaternionf q (0, x[3], x[4], x[5]);
00311   q.w () = sqrt (1 - q.dot (q));
00312   // If the quaternion is used to rotate several points (>1) then it is much more efficient to first convert it
00313   // to a 3x3 Matrix. Comparison of the operation cost for n transformations:
00314   // * Quaternion: 30n
00315   // * Via a Matrix3: 24 + 15n
00316   Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Zero ();
00317   transformation_matrix.topLeftCorner<3, 3> () = q.toRotationMatrix ();
00318   transformation_matrix.block <4, 1> (0, 3) = t;
00319   transformation_matrix = transformation_matrix * model->base_transformation_;
00320   // If iflag == 1 compute fvec at x
00321   if(iflag == 1)
00322   {
00323     for (int i = 0; i < m; i++)
00324     {
00325       // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
00326       Vector4fMapConst p_src = model->tmp_src_->points[i].getVector4fMap ();
00327       // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
00328       Vector4fMapConst p_tgt = model->tmp_tgt_->points[i].getVector4fMap ();
00329       
00330       Eigen::Vector4f pp = transformation_matrix * p_src;
00331       // The last coordiante is still guaranteed to be set to 1.0
00332       Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
00333       Eigen::Vector3d temp = model->mahalanobis(i) * res;
00334       // Cost function
00335       // Originally this is divided by m but as we are using LM it is discarded
00336       fvec[i] = double(res.transpose() * temp);
00337     }
00338   }
00339   else {
00340     // If iflag == 2 compute fjac at x
00341     if(iflag == 2)
00342     {
00343       for (int i = 0; i < m; i++)
00344       {
00345         // Map the jacobian translation component
00346         Eigen::Map<Eigen::Vector3d> g_t(&fjac[i*n]);
00347         // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
00348         Vector4fMapConst p_src = model->tmp_src_->points[i].getVector4fMap ();
00349         // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
00350         Vector4fMapConst p_tgt = model->tmp_tgt_->points[i].getVector4fMap ();
00351         
00352         Eigen::Vector4f pp = transformation_matrix * p_src;
00353         // The last coordiante is still guaranteed to be set to 1.0
00354         Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
00355         // temp = M*res
00356         Eigen::Vector3d temp = model->mahalanobis(i) * res;
00357         // temp_double = res'*temp = temp'*M*res
00358         //double temp_double = res.transpose() * temp;
00359         // Increment translation gradient
00360         // orignally g_t+= 2*M*res/m
00361         g_t= model->mahalanobis(i) * res;
00362         pp = model->base_transformation_ * p_src;
00363         Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
00364         // R = 2/m*pt1*temp^T + R with R set to zeros originally i.e.
00365         // R = 2/m*pt1*temp^T, we discard the 2/m
00366         Eigen::Matrix3d Ri = p_src3 * temp.transpose();
00367         model->computeRDerivative(x, Ri, &fjac[i*n]);
00368       }
00369     }
00370   }
00371   return (0);
00372 }
00373 
00375 template <typename PointSource, typename PointTarget> inline int
00376 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::functionToOptimizeIndices (void *p, int m, int n, const double *x, double *fvec, double *fjac, int ldfjac, int iflag)
00377 {
00378   std::cout << "m "<< m << std::endl;
00379   GeneralizedIterativeClosestPoint *model = (GeneralizedIterativeClosestPoint*)p;
00380 
00381   Eigen::Matrix4f transformation_matrix = model->base_transformation_;
00382   model->apply_state(transformation_matrix, x);
00383 
00384   if (iflag == 1)
00385   {
00386     for (int i = 0; i < m; ++i)
00387     {
00388       // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
00389       Vector4fMapConst p_src = model->tmp_src_->points[(*model->tmp_idx_src_)[i]].getVector4fMap ();
00390       // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
00391       Vector4fMapConst p_tgt = model->tmp_tgt_->points[(*model->tmp_idx_tgt_)[i]].getVector4fMap ();
00392       
00393       Eigen::Vector4f pp = transformation_matrix * p_src;
00394       // Estimate the distance (cost function)
00395       // The last coordiante is still guaranteed to be set to 1.0
00396       Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
00397       Eigen::Vector3d temp = model->mahalanobis((*model->tmp_idx_src_)[i]) * res;
00398       fvec[i] = double(res.transpose() * temp);
00399     }
00400   }
00401   else
00402   {
00403     if (iflag == 2)
00404     {
00405       for (int i = 0; i < m; ++i)
00406       {
00407         // Map the jacobian translation component
00408         Eigen::Map<Eigen::Vector3d> g_t(&fjac[i*n]);
00409         // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
00410         Vector4fMapConst p_src = model->tmp_src_->points[(*model->tmp_idx_src_)[i]].getVector4fMap ();
00411         // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
00412         Vector4fMapConst p_tgt = model->tmp_tgt_->points[(*model->tmp_idx_tgt_)[i]].getVector4fMap ();
00413       
00414         Eigen::Vector4f pp = transformation_matrix * p_src;
00415         // The last coordiante is still guaranteed to be set to 1.0
00416         Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
00417         // temp = M*res
00418         Eigen::Vector3d temp = model->mahalanobis((*model->tmp_idx_src_)[i]) * res;
00419         // Increment translation gradient
00420         // g_t+= 2*M*res/num_matches
00421         g_t= model->mahalanobis((*model->tmp_idx_src_)[i]) * res;
00422         pp = model->base_transformation_ * p_src;
00423         Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
00424         Eigen::Matrix3d Ri = p_src3 * temp.transpose();
00425         model->computeRDerivative(x, Ri, &fjac[i*n]);
00426       }
00427     }
00428   }
00429   return (0);
00430 }
00431 
00433 template <typename PointSource, typename PointTarget> inline void
00434 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
00435 {
00436   using namespace std;
00437   // Difference between consecutive transforms
00438   double delta = 0;
00439   // Get the size of the target
00440   const size_t N = indices_->size ();
00441   std::cout << "N " << N << std::endl;
00442   // Set the mahalanobis matrices to identity
00443   mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
00444   // Compute target cloud covariance matrices
00445   computeCovariances<PointTarget> (target_, tree_, target_covariances_);
00446   // std::ofstream c2_file("c2.txt");
00447   // for(size_t i = 0; i < target_->size(); i++)
00448   // {
00449   //   c2_file << target_covariances_[i] << std::endl;
00450   // }
00451   // c2_file.close();
00452   // Compute input cloud covariance matrices
00453   computeCovariances<PointSource> (input_, input_tree_, input_covariances_);
00454   // std::ofstream c1_file("c1.txt");
00455   // for(size_t i = 0; i < input_->size(); i++)
00456   // {
00457   //   c1_file << input_covariances_[i] << std::endl;
00458   // }
00459   // c1_file.close();
00460   base_transformation_ = guess;
00461   nr_iterations_ = 0;
00462   converged_ = false;
00463   double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
00464   std::vector<int> nn_indices (1);
00465   std::vector<float> nn_dists (1);
00466 
00467   while(!converged_)
00468   {
00469     size_t cnt = 0;
00470     std::vector<int> source_indices (indices_->size ());
00471     std::vector<int> target_indices (indices_->size ());
00472 
00473     // guess corresponds to base_t and transformation_ to t
00474     // dgc_transform_t transform_R;
00475     // dgc_transform_copy(transform_R, base_t);
00476     // dgc_transform_left_multiply(transform_R, t);
00477     Eigen::Matrix4d transform_R;
00478     heteregenious_product(transformation_, guess, transform_R);
00479     Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
00480     for(size_t i = 0; i < N; i++)
00481     {
00482       PointSource query = output[i];
00483       query.getVector4fMap () = guess * query.getVector4fMap ();
00484       query.getVector4fMap () = transformation_ * query.getVector4fMap ();
00485 
00486       if (!searchForNeighbors (query, nn_indices, nn_dists))
00487       {
00488         PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]);
00489         return;
00490       }
00491 
00492       // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
00493       if (nn_dists[0] < dist_threshold)
00494       {
00495         Eigen::Matrix3d &C1 = input_covariances_[i];
00496         Eigen::Matrix3d &C2 = target_covariances_[i];
00497         Eigen::Matrix3d &M = mahalanobis_[i];
00498         // M = R*C1
00499         M = R * C1;
00500         // temp = M*R' + C2 = R*C1*R' + C2
00501         Eigen::Matrix3d temp = M * R.transpose() + C2;
00502         // M = temp^-1
00503         M = temp.inverse();
00504         source_indices[cnt] = i;
00505         target_indices[cnt] = nn_indices[0];
00506         cnt++;
00507       }
00508     }
00509 
00510     // Resize to the actual number of valid correspondences
00511     source_indices.resize(cnt); target_indices.resize(cnt);
00512     /* optimize transformation using the current assignment and Mahalanobis metrics*/
00513     previous_transformation_ = transformation_;
00514     //optimization right here
00515     rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
00516     /* compute the delta from this iteration */
00517     delta = 0.;
00518     for(int k = 0; k < 4; k++) {
00519       for(int l = 0; l < 4; l++) {
00520         double ratio = 1;
00521         if(k < 3 && l < 3) // rotation part of the transform
00522           ratio = 1./rotation_epsilon_;
00523         else
00524           ratio = 1./transformation_epsilon_;
00525         double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l));
00526         if(c_delta > delta)
00527           delta = c_delta;
00528       }
00529     }
00530 
00531     nr_iterations_++;
00532     // Check for convergence
00533     std::cout << "iteration " << nr_iterations_ << ": delta "<< delta << std::endl;
00534     if (nr_iterations_ >= max_iterations_ || delta < 1)
00535     {
00536       if(delta <  1)
00537         std::cout << "delta is < 1" << std::endl;
00538       if(nr_iterations_ >= max_iterations_)
00539         std::cout << "nr_iterations_ >= max_iterations_" << std::endl;
00540 
00541       converged_ = true;
00542       PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
00543                  getClassName ().c_str (), nr_iterations_, max_iterations_, fabs ((transformation_ - previous_transformation_).sum ()));
00544     } 
00545     else {
00546       PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed");      
00547     }
00548   }
00549   final_transformation_ = transformation_;
00550 }
00551 
00552 template <typename PointSource, typename PointTarget> void
00553 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::apply_state(Eigen::Matrix4f &t, const double x[])
00554 {
00555   // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention
00556   Eigen::Matrix3f R;
00557   R = Eigen::AngleAxisf(x[5], Eigen::Vector3f::UnitZ())
00558     * Eigen::AngleAxisf(x[4], Eigen::Vector3f::UnitY())
00559     * Eigen::AngleAxisf(x[3], Eigen::Vector3f::UnitX());
00560   t.topLeftCorner<3,3> () = R * t.topLeftCorner<3,3> ();
00561   Eigen::Vector4f T (x[0], x[1], x[2], 0);
00562   t.block <4, 1> (0, 3) += T;
00563 }
00564 
00565 template <typename PointSource, typename PointTarget> void 
00566 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::get_rotation(const Eigen::Matrix4f &t, double &rx, double &ry, double &rz) 
00567 {
00568   rx = atan2(t(2,1), t(2,2));
00569   ry = asin(-t(2,0));
00570   rz = atan2(t(1,0), t(0,0));                 
00571 }
00572 
00573 template <typename PointSource, typename PointTarget> void 
00574 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::get_translation(const Eigen::Matrix4f &t, double &x, double &y, double &z) 
00575 {
00576   x = t(0,3);
00577   y = t(1,3);
00578   z = t(2,3);
00579 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines