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, 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 00046 /////////////////////////////////////////////////////////////////////////////////////////// 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 00054 //////////////////////////////////////////////////////////////////////////////////////// 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 00133 //////////////////////////////////////////////////////////////////////////////////////// 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 00188 //////////////////////////////////////////////////////////////////////////////////////// 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 00252 //////////////////////////////////////////////////////////////////////////////////////// 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 00277 //////////////////////////////////////////////////////////////////////////////////////// 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 00313 //////////////////////////////////////////////////////////////////////////////////////// 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 00350 //////////////////////////////////////////////////////////////////////////////////////// 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_