gicp.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, 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 #ifndef PCL_REGISTRATION_IMPL_GICP_HPP_
00041 #define PCL_REGISTRATION_IMPL_GICP_HPP_
00042 
00043 #include <pcl/registration/boost.h>
00044 #include <pcl/registration/exceptions.h>
00045 
00047 template <typename PointSource, typename PointTarget> void
00048 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::setInputCloud (
00049     const typename pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::PointCloudSourceConstPtr &cloud)
00050 {
00051   setInputSource (cloud);
00052 }
00053 
00055 template <typename PointSource, typename PointTarget> 
00056 template<typename PointT> void
00057 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 
00058                                                                                     const typename pcl::search::KdTree<PointT>::Ptr kdtree,
00059                                                                                     std::vector<Eigen::Matrix3d>& cloud_covariances)
00060 {
00061   if (k_correspondences_ > int (cloud->size ()))
00062   {
00063     PCL_ERROR ("[pcl::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%zu) is less than k_correspondences_ (%zu)!\n", cloud->size (), k_correspondences_);
00064     return;
00065   }
00066 
00067   Eigen::Vector3d mean;
00068   std::vector<int> nn_indecies; nn_indecies.reserve (k_correspondences_);
00069   std::vector<float> nn_dist_sq; nn_dist_sq.reserve (k_correspondences_);
00070 
00071   // We should never get there but who knows
00072   if(cloud_covariances.size () < cloud->size ())
00073     cloud_covariances.resize (cloud->size ());
00074 
00075   typename pcl::PointCloud<PointT>::const_iterator points_iterator = cloud->begin ();
00076   std::vector<Eigen::Matrix3d>::iterator matrices_iterator = cloud_covariances.begin ();
00077   for(;
00078       points_iterator != cloud->end ();
00079       ++points_iterator, ++matrices_iterator)
00080   {
00081     const PointT &query_point = *points_iterator;
00082     Eigen::Matrix3d &cov = *matrices_iterator;
00083     // Zero out the cov and mean
00084     cov.setZero ();
00085     mean.setZero ();
00086 
00087     // Search for the K nearest neighbours
00088     kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
00089     
00090     // Find the covariance matrix
00091     for(int j = 0; j < k_correspondences_; j++) {
00092       const PointT &pt = (*cloud)[nn_indecies[j]];
00093       
00094       mean[0] += pt.x;
00095       mean[1] += pt.y;
00096       mean[2] += pt.z;
00097       
00098       cov(0,0) += pt.x*pt.x;
00099       
00100       cov(1,0) += pt.y*pt.x;
00101       cov(1,1) += pt.y*pt.y;
00102       
00103       cov(2,0) += pt.z*pt.x;
00104       cov(2,1) += pt.z*pt.y;
00105       cov(2,2) += pt.z*pt.z;    
00106     }
00107   
00108     mean /= static_cast<double> (k_correspondences_);
00109     // Get the actual covariance
00110     for (int k = 0; k < 3; k++)
00111       for (int l = 0; l <= k; l++) 
00112       {
00113         cov(k,l) /= static_cast<double> (k_correspondences_);
00114         cov(k,l) -= mean[k]*mean[l];
00115         cov(l,k) = cov(k,l);
00116       }
00117     
00118     // Compute the SVD (covariance matrix is symmetric so U = V')
00119     Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
00120     cov.setZero ();
00121     Eigen::Matrix3d U = svd.matrixU ();
00122     // Reconstitute the covariance matrix with modified singular values using the column     // vectors in V.
00123     for(int k = 0; k < 3; k++) {
00124       Eigen::Vector3d col = U.col(k);
00125       double v = 1.; // biggest 2 singular values replaced by 1
00126       if(k == 2)   // smallest singular value replaced by gicp_epsilon
00127         v = gicp_epsilon_;
00128       cov+= v * col * col.transpose(); 
00129     }
00130   }
00131 }
00132 
00134 template <typename PointSource, typename PointTarget> void
00135 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const
00136 {
00137   Eigen::Matrix3d dR_dPhi;
00138   Eigen::Matrix3d dR_dTheta;
00139   Eigen::Matrix3d dR_dPsi;
00140 
00141   double phi = x[3], theta = x[4], psi = x[5];
00142   
00143   double cphi = cos(phi), sphi = sin(phi);
00144   double ctheta = cos(theta), stheta = sin(theta);
00145   double cpsi = cos(psi), spsi = sin(psi);
00146       
00147   dR_dPhi(0,0) = 0.;
00148   dR_dPhi(1,0) = 0.;
00149   dR_dPhi(2,0) = 0.;
00150 
00151   dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
00152   dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
00153   dR_dPhi(2,1) = cphi*ctheta;
00154 
00155   dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
00156   dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
00157   dR_dPhi(2,2) = -ctheta*sphi;
00158 
00159   dR_dTheta(0,0) = -cpsi*stheta;
00160   dR_dTheta(1,0) = -spsi*stheta;
00161   dR_dTheta(2,0) = -ctheta;
00162 
00163   dR_dTheta(0,1) = cpsi*ctheta*sphi;
00164   dR_dTheta(1,1) = ctheta*sphi*spsi;
00165   dR_dTheta(2,1) = -sphi*stheta;
00166 
00167   dR_dTheta(0,2) = cphi*cpsi*ctheta;
00168   dR_dTheta(1,2) = cphi*ctheta*spsi;
00169   dR_dTheta(2,2) = -cphi*stheta;
00170 
00171   dR_dPsi(0,0) = -ctheta*spsi;
00172   dR_dPsi(1,0) = cpsi*ctheta;
00173   dR_dPsi(2,0) = 0.;
00174 
00175   dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
00176   dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
00177   dR_dPsi(2,1) = 0.;
00178 
00179   dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
00180   dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
00181   dR_dPsi(2,2) = 0.;
00182       
00183   g[3] = matricesInnerProd(dR_dPhi, R);
00184   g[4] = matricesInnerProd(dR_dTheta, R);
00185   g[5] = matricesInnerProd(dR_dPsi, R);
00186 }
00187 
00189 template <typename PointSource, typename PointTarget> void
00190 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, 
00191                                                                                                   const std::vector<int> &indices_src, 
00192                                                                                                   const PointCloudTarget &cloud_tgt, 
00193                                                                                                   const std::vector<int> &indices_tgt, 
00194                                                                                                   Eigen::Matrix4f &transformation_matrix)
00195 {
00196   if (indices_src.size () < 4)     // need at least 4 samples
00197   {
00198     PCL_THROW_EXCEPTION (NotEnoughPointsException, 
00199                          "[pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!");
00200     return;
00201   }
00202   // Set the initial solution
00203   Vector6d x = Vector6d::Zero ();
00204   x[0] = transformation_matrix (0,3);
00205   x[1] = transformation_matrix (1,3);
00206   x[2] = transformation_matrix (2,3);
00207   x[3] = atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
00208   x[4] = asin (-transformation_matrix (2,0));
00209   x[5] = atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
00210 
00211   // Set temporary pointers
00212   tmp_src_ = &cloud_src;
00213   tmp_tgt_ = &cloud_tgt;
00214   tmp_idx_src_ = &indices_src;
00215   tmp_idx_tgt_ = &indices_tgt;
00216 
00217   // Optimize using forward-difference approximation LM
00218   const double gradient_tol = 1e-2;
00219   OptimizationFunctorWithIndices functor(this);
00220   BFGS<OptimizationFunctorWithIndices> bfgs (functor);
00221   bfgs.parameters.sigma = 0.01;
00222   bfgs.parameters.rho = 0.01;
00223   bfgs.parameters.tau1 = 9;
00224   bfgs.parameters.tau2 = 0.05;
00225   bfgs.parameters.tau3 = 0.5;
00226   bfgs.parameters.order = 3;
00227 
00228   int inner_iterations_ = 0;
00229   int result = bfgs.minimizeInit (x);
00230   do
00231   {
00232     inner_iterations_++;
00233     result = bfgs.minimizeOneStep (x);
00234     if(result)
00235     {
00236       break;
00237     }
00238     result = bfgs.testGradient(gradient_tol);
00239   } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
00240   if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
00241   {
00242     PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
00243     PCL_DEBUG ("BFGS solver finished with exit code %i \n", result);
00244     transformation_matrix.setIdentity();
00245     applyState(transformation_matrix, x);
00246   }
00247   else
00248     PCL_THROW_EXCEPTION(SolverDidntConvergeException, 
00249                         "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
00250 }
00251 
00253 template <typename PointSource, typename PointTarget> inline double
00254 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::operator() (const Vector6d& x)
00255 {
00256   Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
00257   gicp_->applyState(transformation_matrix, x);
00258   double f = 0;
00259   int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
00260   for (int i = 0; i < m; ++i)
00261   {
00262     // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
00263     Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
00264     // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
00265     Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
00266     Eigen::Vector4f pp (transformation_matrix * p_src);
00267     // Estimate the distance (cost function)
00268     // The last coordiante is still guaranteed to be set to 1.0
00269     Eigen::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
00270     Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
00271     //increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone 1/num_matches after the loop closes)
00272     f+= double(res.transpose() * temp);
00273   }
00274   return f/m;
00275 }
00276 
00278 template <typename PointSource, typename PointTarget> inline void
00279 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g)
00280 {
00281   Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
00282   gicp_->applyState(transformation_matrix, x);
00283   //Zero out g
00284   g.setZero ();
00285   //Eigen::Vector3d g_t = g.head<3> ();
00286   Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
00287   int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
00288   for (int i = 0; i < m; ++i)
00289   {
00290     // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
00291     Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
00292     // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
00293     Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
00294 
00295     Eigen::Vector4f pp (transformation_matrix * p_src);
00296     // The last coordiante is still guaranteed to be set to 1.0
00297     Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
00298     // temp = M*res
00299     Eigen::Vector3d temp (gicp_->mahalanobis ((*gicp_->tmp_idx_src_)[i]) * res);
00300     // Increment translation gradient
00301     // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
00302     g.head<3> ()+= temp;
00303     // Increment rotation gradient
00304     pp = gicp_->base_transformation_ * p_src;
00305     Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
00306     R+= p_src3 * temp.transpose();
00307   }
00308   g.head<3> ()*= 2.0/m;
00309   R*= 2.0/m;
00310   gicp_->computeRDerivative(x, R, g);
00311 }
00312 
00314 template <typename PointSource, typename PointTarget> inline void
00315 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g)
00316 {
00317   Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
00318   gicp_->applyState(transformation_matrix, x);
00319   f = 0;
00320   g.setZero ();
00321   Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
00322   const int m = static_cast<const int> (gicp_->tmp_idx_src_->size ());
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 = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
00327     // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
00328     Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
00329     Eigen::Vector4f pp (transformation_matrix * p_src);
00330     // The last coordiante is still guaranteed to be set to 1.0
00331     Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
00332     // temp = M*res
00333     Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
00334     // Increment total error
00335     f+= double(res.transpose() * temp);
00336     // Increment translation gradient
00337     // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
00338     g.head<3> ()+= temp;
00339     pp = gicp_->base_transformation_ * p_src;
00340     Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
00341     // Increment rotation gradient
00342     R+= p_src3 * temp.transpose();    
00343   }
00344   f/= double(m);
00345   g.head<3> ()*= double(2.0/m);
00346   R*= 2.0/m;
00347   gicp_->computeRDerivative(x, R, g);
00348 }
00349 
00351 template <typename PointSource, typename PointTarget> inline void
00352 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
00353 {
00354   pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal ();
00355   using namespace std;
00356   // Difference between consecutive transforms
00357   double delta = 0;
00358   // Get the size of the target
00359   const size_t N = indices_->size ();
00360   // Set the mahalanobis matrices to identity
00361   mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
00362   // Compute target cloud covariance matrices
00363   computeCovariances<PointTarget> (target_, tree_, target_covariances_);
00364   // Compute input cloud covariance matrices
00365   computeCovariances<PointSource> (input_, tree_reciprocal_, input_covariances_);
00366 
00367   base_transformation_ = guess;
00368   nr_iterations_ = 0;
00369   converged_ = false;
00370   double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
00371   std::vector<int> nn_indices (1);
00372   std::vector<float> nn_dists (1);
00373 
00374   while(!converged_)
00375   {
00376     size_t cnt = 0;
00377     std::vector<int> source_indices (indices_->size ());
00378     std::vector<int> target_indices (indices_->size ());
00379 
00380     // guess corresponds to base_t and transformation_ to t
00381     Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
00382     for(size_t i = 0; i < 4; i++)
00383       for(size_t j = 0; j < 4; j++)
00384         for(size_t k = 0; k < 4; k++)
00385           transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));
00386 
00387     Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
00388 
00389     for (size_t i = 0; i < N; i++)
00390     {
00391       PointSource query = output[i];
00392       query.getVector4fMap () = guess * query.getVector4fMap ();
00393       query.getVector4fMap () = transformation_ * query.getVector4fMap ();
00394 
00395       if (!searchForNeighbors (query, nn_indices, nn_dists))
00396       {
00397         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]);
00398         return;
00399       }
00400       
00401       // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
00402       if (nn_dists[0] < dist_threshold)
00403       {
00404         Eigen::Matrix3d &C1 = input_covariances_[i];
00405         Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]];
00406         Eigen::Matrix3d &M = mahalanobis_[i];
00407         // M = R*C1
00408         M = R * C1;
00409         // temp = M*R' + C2 = R*C1*R' + C2
00410         Eigen::Matrix3d temp = M * R.transpose();        
00411         temp+= C2;
00412         // M = temp^-1
00413         M = temp.inverse ();
00414         source_indices[cnt] = static_cast<int> (i);
00415         target_indices[cnt] = nn_indices[0];
00416         cnt++;
00417       }
00418     }
00419     // Resize to the actual number of valid correspondences
00420     source_indices.resize(cnt); target_indices.resize(cnt);
00421     /* optimize transformation using the current assignment and Mahalanobis metrics*/
00422     previous_transformation_ = transformation_;
00423     //optimization right here
00424     try
00425     {
00426       rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
00427       /* compute the delta from this iteration */
00428       delta = 0.;
00429       for(int k = 0; k < 4; k++) {
00430         for(int l = 0; l < 4; l++) {
00431           double ratio = 1;
00432           if(k < 3 && l < 3) // rotation part of the transform
00433             ratio = 1./rotation_epsilon_;
00434           else
00435             ratio = 1./transformation_epsilon_;
00436           double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l));
00437           if(c_delta > delta)
00438             delta = c_delta;
00439         }
00440       }
00441     } 
00442     catch (PCLException &e)
00443     {
00444       PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
00445       break;
00446     }
00447     nr_iterations_++;
00448     // Check for convergence
00449     if (nr_iterations_ >= max_iterations_ || delta < 1)
00450     {
00451       converged_ = true;
00452       previous_transformation_ = transformation_;
00453       PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
00454                  getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
00455     } 
00456     else
00457       PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
00458   }
00459   //for some reason the static equivalent methode raises an error
00460   // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0));
00461   // final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3);
00462   final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3);
00463   final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3);
00464   final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3);
00465   final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3);
00466 }
00467 
00468 template <typename PointSource, typename PointTarget> void
00469 pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::applyState(Eigen::Matrix4f &t, const Vector6d& x) const
00470 {
00471   // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention
00472   Eigen::Matrix3f R;
00473   R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
00474     * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
00475     * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
00476   t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
00477   Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
00478   t.col (3) += T;
00479 }
00480 
00481 #endif //PCL_REGISTRATION_IMPL_GICP_HPP_


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