Point Cloud Library (PCL)
1.3.1
|
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 }