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-2011, 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 00041 #ifndef PCL_REGISTRATION_NDT_IMPL_H_ 00042 #define PCL_REGISTRATION_NDT_IMPL_H_ 00043 00044 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00045 template<typename PointSource, typename PointTarget> 00046 pcl::NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform () 00047 : target_cells_ () 00048 , resolution_ (1.0f) 00049 , step_size_ (0.1) 00050 , outlier_ratio_ (0.55) 00051 , gauss_d1_ () 00052 , gauss_d2_ () 00053 , trans_probability_ () 00054 , j_ang_a_ (), j_ang_b_ (), j_ang_c_ (), j_ang_d_ (), j_ang_e_ (), j_ang_f_ (), j_ang_g_ (), j_ang_h_ () 00055 , h_ang_a2_ (), h_ang_a3_ (), h_ang_b2_ (), h_ang_b3_ (), h_ang_c2_ (), h_ang_c3_ (), h_ang_d1_ (), h_ang_d2_ () 00056 , h_ang_d3_ (), h_ang_e1_ (), h_ang_e2_ (), h_ang_e3_ (), h_ang_f1_ (), h_ang_f2_ (), h_ang_f3_ () 00057 , point_gradient_ () 00058 , point_hessian_ () 00059 { 00060 reg_name_ = "NormalDistributionsTransform"; 00061 00062 double gauss_c1, gauss_c2, gauss_d3; 00063 00064 // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009] 00065 gauss_c1 = 10.0 * (1 - outlier_ratio_); 00066 gauss_c2 = outlier_ratio_ / pow (resolution_, 3); 00067 gauss_d3 = -log (gauss_c2); 00068 gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3; 00069 gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_); 00070 00071 transformation_epsilon_ = 0.1; 00072 max_iterations_ = 35; 00073 } 00074 00075 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00076 template<typename PointSource, typename PointTarget> void 00077 pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) 00078 { 00079 nr_iterations_ = 0; 00080 converged_ = false; 00081 00082 double gauss_c1, gauss_c2, gauss_d3; 00083 00084 // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009] 00085 gauss_c1 = 10 * (1 - outlier_ratio_); 00086 gauss_c2 = outlier_ratio_ / pow (resolution_, 3); 00087 gauss_d3 = -log (gauss_c2); 00088 gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3; 00089 gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_); 00090 00091 if (guess != Eigen::Matrix4f::Identity ()) 00092 { 00093 // Initialise final transformation to the guessed one 00094 final_transformation_ = guess; 00095 // Apply guessed transformation prior to search for neighbours 00096 transformPointCloud (output, output, guess); 00097 } 00098 00099 // Initialize Point Gradient and Hessian 00100 point_gradient_.setZero (); 00101 point_gradient_.block<3, 3>(0, 0).setIdentity (); 00102 point_hessian_.setZero (); 00103 00104 Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation; 00105 eig_transformation.matrix () = final_transformation_; 00106 00107 // Convert initial guess matrix to 6 element transformation vector 00108 Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient; 00109 Eigen::Vector3f init_translation = eig_transformation.translation (); 00110 Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2); 00111 p << init_translation (0), init_translation (1), init_translation (2), 00112 init_rotation (0), init_rotation (1), init_rotation (2); 00113 00114 Eigen::Matrix<double, 6, 6> hessian; 00115 00116 double score = 0; 00117 double delta_p_norm; 00118 00119 // Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination. 00120 score = computeDerivatives (score_gradient, hessian, output, p); 00121 00122 while (!converged_) 00123 { 00124 // Store previous transformation 00125 previous_transformation_ = transformation_; 00126 00127 // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009] 00128 Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV); 00129 // Negative for maximization as opposed to minimization 00130 delta_p = sv.solve (-score_gradient); 00131 00132 //Calculate step length with guarnteed sufficient decrease [More, Thuente 1994] 00133 delta_p_norm = delta_p.norm (); 00134 00135 if (delta_p_norm == 0 || delta_p_norm != delta_p_norm) 00136 { 00137 trans_probability_ = score / static_cast<double> (input_->points.size ()); 00138 converged_ = delta_p_norm == delta_p_norm; 00139 return; 00140 } 00141 00142 delta_p.normalize (); 00143 delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output); 00144 delta_p *= delta_p_norm; 00145 00146 00147 transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) * 00148 Eigen::AngleAxis<float> (static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) * 00149 Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) * 00150 Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix (); 00151 00152 00153 p = p + delta_p; 00154 00155 // Update Visualizer (untested) 00156 if (update_visualizer_ != 0) 00157 update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() ); 00158 00159 if (nr_iterations_ > max_iterations_ || 00160 (nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_))) 00161 { 00162 converged_ = true; 00163 } 00164 00165 nr_iterations_++; 00166 00167 } 00168 00169 // Store transformation probability. The realtive differences within each scan registration are accurate 00170 // but the normalization constants need to be modified for it to be globally accurate 00171 trans_probability_ = score / static_cast<double> (input_->points.size ()); 00172 } 00173 00174 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00175 template<typename PointSource, typename PointTarget> double 00176 pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient, 00177 Eigen::Matrix<double, 6, 6> &hessian, 00178 PointCloudSource &trans_cloud, 00179 Eigen::Matrix<double, 6, 1> &p, 00180 bool compute_hessian) 00181 { 00182 // Original Point and Transformed Point 00183 PointSource x_pt, x_trans_pt; 00184 // Original Point and Transformed Point (for math) 00185 Eigen::Vector3d x, x_trans; 00186 // Occupied Voxel 00187 TargetGridLeafConstPtr cell; 00188 // Inverse Covariance of Occupied Voxel 00189 Eigen::Matrix3d c_inv; 00190 00191 score_gradient.setZero (); 00192 hessian.setZero (); 00193 double score = 0; 00194 00195 // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009] 00196 computeAngleDerivatives (p); 00197 00198 // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] 00199 for (size_t idx = 0; idx < input_->points.size (); idx++) 00200 { 00201 x_trans_pt = trans_cloud.points[idx]; 00202 00203 // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. 00204 std::vector<TargetGridLeafConstPtr> neighborhood; 00205 std::vector<float> distances; 00206 target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances); 00207 00208 for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++) 00209 { 00210 cell = *neighborhood_it; 00211 x_pt = input_->points[idx]; 00212 x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z); 00213 00214 x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); 00215 00216 // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] 00217 x_trans -= cell->getMean (); 00218 // Uses precomputed covariance for speed. 00219 c_inv = cell->getInverseCov (); 00220 00221 // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009] 00222 computePointDerivatives (x); 00223 // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] 00224 score += updateDerivatives (score_gradient, hessian, x_trans, c_inv, compute_hessian); 00225 00226 } 00227 } 00228 return (score); 00229 } 00230 00231 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00232 template<typename PointSource, typename PointTarget> void 00233 pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives (Eigen::Matrix<double, 6, 1> &p, bool compute_hessian) 00234 { 00235 // Simplified math for near 0 angles 00236 double cx, cy, cz, sx, sy, sz; 00237 if (fabs (p (3)) < 10e-5) 00238 { 00239 //p(3) = 0; 00240 cx = 1.0; 00241 sx = 0.0; 00242 } 00243 else 00244 { 00245 cx = cos (p (3)); 00246 sx = sin (p (3)); 00247 } 00248 if (fabs (p (4)) < 10e-5) 00249 { 00250 //p(4) = 0; 00251 cy = 1.0; 00252 sy = 0.0; 00253 } 00254 else 00255 { 00256 cy = cos (p (4)); 00257 sy = sin (p (4)); 00258 } 00259 00260 if (fabs (p (5)) < 10e-5) 00261 { 00262 //p(5) = 0; 00263 cz = 1.0; 00264 sz = 0.0; 00265 } 00266 else 00267 { 00268 cz = cos (p (5)); 00269 sz = sin (p (5)); 00270 } 00271 00272 // Precomputed angular gradiant components. Letters correspond to Equation 6.19 [Magnusson 2009] 00273 j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy); 00274 j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy); 00275 j_ang_c_ << (-sy * cz), sy * sz, cy; 00276 j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy; 00277 j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy); 00278 j_ang_f_ << (-cy * sz), (-cy * cz), 0; 00279 j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0; 00280 j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0; 00281 00282 if (compute_hessian) 00283 { 00284 // Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers correspond to row index [Magnusson 2009] 00285 h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy; 00286 h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy); 00287 00288 h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy); 00289 h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy); 00290 00291 h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0; 00292 h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0; 00293 00294 h_ang_d1_ << (-cy * cz), (cy * sz), (sy); 00295 h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy); 00296 h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy); 00297 00298 h_ang_e1_ << (sy * sz), (sy * cz), 0; 00299 h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0; 00300 h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0; 00301 00302 h_ang_f1_ << (-cy * cz), (cy * sz), 0; 00303 h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0; 00304 h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0; 00305 } 00306 } 00307 00308 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00309 template<typename PointSource, typename PointTarget> void 00310 pcl::NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian) 00311 { 00312 // Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p. 00313 // Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009] 00314 point_gradient_ (1, 3) = x.dot (j_ang_a_); 00315 point_gradient_ (2, 3) = x.dot (j_ang_b_); 00316 point_gradient_ (0, 4) = x.dot (j_ang_c_); 00317 point_gradient_ (1, 4) = x.dot (j_ang_d_); 00318 point_gradient_ (2, 4) = x.dot (j_ang_e_); 00319 point_gradient_ (0, 5) = x.dot (j_ang_f_); 00320 point_gradient_ (1, 5) = x.dot (j_ang_g_); 00321 point_gradient_ (2, 5) = x.dot (j_ang_h_); 00322 00323 if (compute_hessian) 00324 { 00325 // Vectors from Equation 6.21 [Magnusson 2009] 00326 Eigen::Vector3d a, b, c, d, e, f; 00327 00328 a << 0, x.dot (h_ang_a2_), x.dot (h_ang_a3_); 00329 b << 0, x.dot (h_ang_b2_), x.dot (h_ang_b3_); 00330 c << 0, x.dot (h_ang_c2_), x.dot (h_ang_c3_); 00331 d << x.dot (h_ang_d1_), x.dot (h_ang_d2_), x.dot (h_ang_d3_); 00332 e << x.dot (h_ang_e1_), x.dot (h_ang_e2_), x.dot (h_ang_e3_); 00333 f << x.dot (h_ang_f1_), x.dot (h_ang_f2_), x.dot (h_ang_f3_); 00334 00335 // Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p. 00336 // Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009] 00337 point_hessian_.block<3, 1>(9, 3) = a; 00338 point_hessian_.block<3, 1>(12, 3) = b; 00339 point_hessian_.block<3, 1>(15, 3) = c; 00340 point_hessian_.block<3, 1>(9, 4) = b; 00341 point_hessian_.block<3, 1>(12, 4) = d; 00342 point_hessian_.block<3, 1>(15, 4) = e; 00343 point_hessian_.block<3, 1>(9, 5) = c; 00344 point_hessian_.block<3, 1>(12, 5) = e; 00345 point_hessian_.block<3, 1>(15, 5) = f; 00346 } 00347 } 00348 00349 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00350 template<typename PointSource, typename PointTarget> double 00351 pcl::NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient, 00352 Eigen::Matrix<double, 6, 6> &hessian, 00353 Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv, 00354 bool compute_hessian) 00355 { 00356 Eigen::Vector3d cov_dxd_pi; 00357 // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] 00358 double e_x_cov_x = exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2); 00359 // Calculate probability of transtormed points existance, Equation 6.9 [Magnusson 2009] 00360 double score_inc = -gauss_d1_ * e_x_cov_x; 00361 00362 e_x_cov_x = gauss_d2_ * e_x_cov_x; 00363 00364 // Error checking for invalid values. 00365 if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) 00366 return (0); 00367 00368 // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 00369 e_x_cov_x *= gauss_d1_; 00370 00371 00372 for (int i = 0; i < 6; i++) 00373 { 00374 // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 00375 cov_dxd_pi = c_inv * point_gradient_.col (i); 00376 00377 // Update gradient, Equation 6.12 [Magnusson 2009] 00378 score_gradient (i) += x_trans.dot (cov_dxd_pi) * e_x_cov_x; 00379 00380 if (compute_hessian) 00381 { 00382 for (int j = 0; j < hessian.cols (); j++) 00383 { 00384 // Update hessian, Equation 6.13 [Magnusson 2009] 00385 hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) + 00386 x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) + 00387 point_gradient_.col (j).dot (cov_dxd_pi) ); 00388 } 00389 } 00390 } 00391 00392 return (score_inc); 00393 } 00394 00395 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00396 template<typename PointSource, typename PointTarget> void 00397 pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeHessian (Eigen::Matrix<double, 6, 6> &hessian, 00398 PointCloudSource &trans_cloud, Eigen::Matrix<double, 6, 1> &) 00399 { 00400 // Original Point and Transformed Point 00401 PointSource x_pt, x_trans_pt; 00402 // Original Point and Transformed Point (for math) 00403 Eigen::Vector3d x, x_trans; 00404 // Occupied Voxel 00405 TargetGridLeafConstPtr cell; 00406 // Inverse Covariance of Occupied Voxel 00407 Eigen::Matrix3d c_inv; 00408 00409 hessian.setZero (); 00410 00411 // Precompute Angular Derivatives unessisary because only used after regular derivative calculation 00412 00413 // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009] 00414 for (size_t idx = 0; idx < input_->points.size (); idx++) 00415 { 00416 x_trans_pt = trans_cloud.points[idx]; 00417 00418 // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking. 00419 std::vector<TargetGridLeafConstPtr> neighborhood; 00420 std::vector<float> distances; 00421 target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances); 00422 00423 for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++) 00424 { 00425 cell = *neighborhood_it; 00426 00427 { 00428 x_pt = input_->points[idx]; 00429 x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z); 00430 00431 x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z); 00432 00433 // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009] 00434 x_trans -= cell->getMean (); 00435 // Uses precomputed covariance for speed. 00436 c_inv = cell->getInverseCov (); 00437 00438 // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009] 00439 computePointDerivatives (x); 00440 // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009] 00441 updateHessian (hessian, x_trans, c_inv); 00442 } 00443 } 00444 } 00445 } 00446 00447 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00448 template<typename PointSource, typename PointTarget> void 00449 pcl::NormalDistributionsTransform<PointSource, PointTarget>::updateHessian (Eigen::Matrix<double, 6, 6> &hessian, Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv) 00450 { 00451 Eigen::Vector3d cov_dxd_pi; 00452 // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009] 00453 double e_x_cov_x = gauss_d2_ * exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2); 00454 00455 // Error checking for invalid values. 00456 if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x) 00457 return; 00458 00459 // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 00460 e_x_cov_x *= gauss_d1_; 00461 00462 for (int i = 0; i < 6; i++) 00463 { 00464 // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009] 00465 cov_dxd_pi = c_inv * point_gradient_.col (i); 00466 00467 for (int j = 0; j < hessian.cols (); j++) 00468 { 00469 // Update hessian, Equation 6.13 [Magnusson 2009] 00470 hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) + 00471 x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) + 00472 point_gradient_.col (j).dot (cov_dxd_pi) ); 00473 } 00474 } 00475 00476 } 00477 00478 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00479 template<typename PointSource, typename PointTarget> bool 00480 pcl::NormalDistributionsTransform<PointSource, PointTarget>::updateIntervalMT (double &a_l, double &f_l, double &g_l, 00481 double &a_u, double &f_u, double &g_u, 00482 double a_t, double f_t, double g_t) 00483 { 00484 // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994] 00485 if (f_t > f_l) 00486 { 00487 a_u = a_t; 00488 f_u = f_t; 00489 g_u = g_t; 00490 return (false); 00491 } 00492 // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994] 00493 else 00494 if (g_t * (a_l - a_t) > 0) 00495 { 00496 a_l = a_t; 00497 f_l = f_t; 00498 g_l = g_t; 00499 return (false); 00500 } 00501 // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994] 00502 else 00503 if (g_t * (a_l - a_t) < 0) 00504 { 00505 a_u = a_l; 00506 f_u = f_l; 00507 g_u = g_l; 00508 00509 a_l = a_t; 00510 f_l = f_t; 00511 g_l = g_t; 00512 return (false); 00513 } 00514 // Interval Converged 00515 else 00516 return (true); 00517 } 00518 00519 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00520 template<typename PointSource, typename PointTarget> double 00521 pcl::NormalDistributionsTransform<PointSource, PointTarget>::trialValueSelectionMT (double a_l, double f_l, double g_l, 00522 double a_u, double f_u, double g_u, 00523 double a_t, double f_t, double g_t) 00524 { 00525 // Case 1 in Trial Value Selection [More, Thuente 1994] 00526 if (f_t > f_l) 00527 { 00528 // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t 00529 // Equation 2.4.52 [Sun, Yuan 2006] 00530 double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; 00531 double w = std::sqrt (z * z - g_t * g_l); 00532 // Equation 2.4.56 [Sun, Yuan 2006] 00533 double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); 00534 00535 // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l 00536 // Equation 2.4.2 [Sun, Yuan 2006] 00537 double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t)); 00538 00539 if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l)) 00540 return (a_c); 00541 else 00542 return (0.5 * (a_q + a_c)); 00543 } 00544 // Case 2 in Trial Value Selection [More, Thuente 1994] 00545 else 00546 if (g_t * g_l < 0) 00547 { 00548 // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t 00549 // Equation 2.4.52 [Sun, Yuan 2006] 00550 double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; 00551 double w = std::sqrt (z * z - g_t * g_l); 00552 // Equation 2.4.56 [Sun, Yuan 2006] 00553 double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); 00554 00555 // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t 00556 // Equation 2.4.5 [Sun, Yuan 2006] 00557 double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; 00558 00559 if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t)) 00560 return (a_c); 00561 else 00562 return (a_s); 00563 } 00564 // Case 3 in Trial Value Selection [More, Thuente 1994] 00565 else 00566 if (std::fabs (g_t) <= std::fabs (g_l)) 00567 { 00568 // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t 00569 // Equation 2.4.52 [Sun, Yuan 2006] 00570 double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l; 00571 double w = std::sqrt (z * z - g_t * g_l); 00572 double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w); 00573 00574 // Calculate the minimizer of the quadratic that interpolates g_l and g_t 00575 // Equation 2.4.5 [Sun, Yuan 2006] 00576 double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l; 00577 00578 double a_t_next; 00579 00580 if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t)) 00581 a_t_next = a_c; 00582 else 00583 a_t_next = a_s; 00584 00585 if (a_t > a_l) 00586 return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next)); 00587 else 00588 return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next)); 00589 } 00590 // Case 4 in Trial Value Selection [More, Thuente 1994] 00591 else 00592 { 00593 // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t 00594 // Equation 2.4.52 [Sun, Yuan 2006] 00595 double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u; 00596 double w = std::sqrt (z * z - g_t * g_u); 00597 // Equation 2.4.56 [Sun, Yuan 2006] 00598 return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w)); 00599 } 00600 } 00601 00602 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00603 template<typename PointSource, typename PointTarget> double 00604 pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max, 00605 double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian, 00606 PointCloudSource &trans_cloud) 00607 { 00608 // Set the value of phi(0), Equation 1.3 [More, Thuente 1994] 00609 double phi_0 = -score; 00610 // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994] 00611 double d_phi_0 = -(score_gradient.dot (step_dir)); 00612 00613 Eigen::Matrix<double, 6, 1> x_t; 00614 00615 if (d_phi_0 >= 0) 00616 { 00617 // Not a decent direction 00618 if (d_phi_0 == 0) 00619 return 0; 00620 else 00621 { 00622 // Reverse step direction and calculate optimal step. 00623 d_phi_0 *= -1; 00624 step_dir *= -1; 00625 00626 } 00627 } 00628 00629 // The Search Algorithm for T(mu) [More, Thuente 1994] 00630 00631 int max_step_iterations = 10; 00632 int step_iterations = 0; 00633 00634 // Sufficient decreace constant, Equation 1.1 [More, Thuete 1994] 00635 double mu = 1.e-4; 00636 // Curvature condition constant, Equation 1.2 [More, Thuete 1994] 00637 double nu = 0.9; 00638 00639 // Initial endpoints of Interval I, 00640 double a_l = 0, a_u = 0; 00641 00642 // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994] 00643 double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu); 00644 double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu); 00645 00646 double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu); 00647 double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu); 00648 00649 // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max 00650 bool interval_converged = (step_max - step_min) > 0, open_interval = true; 00651 00652 double a_t = step_init; 00653 a_t = std::min (a_t, step_max); 00654 a_t = std::max (a_t, step_min); 00655 00656 x_t = x + step_dir * a_t; 00657 00658 final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) * 00659 Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) * 00660 Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) * 00661 Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix (); 00662 00663 // New transformed point cloud 00664 transformPointCloud (*input_, trans_cloud, final_transformation_); 00665 00666 // Updates score, gradient and hessian. Hessian calculation is unessisary but testing showed that most step calculations use the 00667 // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time. 00668 score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true); 00669 00670 // Calculate phi(alpha_t) 00671 double phi_t = -score; 00672 // Calculate phi'(alpha_t) 00673 double d_phi_t = -(score_gradient.dot (step_dir)); 00674 00675 // Calculate psi(alpha_t) 00676 double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu); 00677 // Calculate psi'(alpha_t) 00678 double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu); 00679 00680 // Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994] 00681 while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) 00682 { 00683 // Use auxilary function if interval I is not closed 00684 if (open_interval) 00685 { 00686 a_t = trialValueSelectionMT (a_l, f_l, g_l, 00687 a_u, f_u, g_u, 00688 a_t, psi_t, d_psi_t); 00689 } 00690 else 00691 { 00692 a_t = trialValueSelectionMT (a_l, f_l, g_l, 00693 a_u, f_u, g_u, 00694 a_t, phi_t, d_phi_t); 00695 } 00696 00697 a_t = std::min (a_t, step_max); 00698 a_t = std::max (a_t, step_min); 00699 00700 x_t = x + step_dir * a_t; 00701 00702 final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) * 00703 Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) * 00704 Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) * 00705 Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix (); 00706 00707 // New transformed point cloud 00708 // Done on final cloud to prevent wasted computation 00709 transformPointCloud (*input_, trans_cloud, final_transformation_); 00710 00711 // Updates score, gradient. Values stored to prevent wasted computation. 00712 score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false); 00713 00714 // Calculate phi(alpha_t+) 00715 phi_t = -score; 00716 // Calculate phi'(alpha_t+) 00717 d_phi_t = -(score_gradient.dot (step_dir)); 00718 00719 // Calculate psi(alpha_t+) 00720 psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu); 00721 // Calculate psi'(alpha_t+) 00722 d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu); 00723 00724 // Check if I is now a closed interval 00725 if (open_interval && (psi_t <= 0 && d_psi_t >= 0)) 00726 { 00727 open_interval = false; 00728 00729 // Converts f_l and g_l from psi to phi 00730 f_l = f_l + phi_0 - mu * d_phi_0 * a_l; 00731 g_l = g_l + mu * d_phi_0; 00732 00733 // Converts f_u and g_u from psi to phi 00734 f_u = f_u + phi_0 - mu * d_phi_0 * a_u; 00735 g_u = g_u + mu * d_phi_0; 00736 } 00737 00738 if (open_interval) 00739 { 00740 // Update interval end points using Updating Algorithm [More, Thuente 1994] 00741 interval_converged = updateIntervalMT (a_l, f_l, g_l, 00742 a_u, f_u, g_u, 00743 a_t, psi_t, d_psi_t); 00744 } 00745 else 00746 { 00747 // Update interval end points using Modified Updating Algorithm [More, Thuente 1994] 00748 interval_converged = updateIntervalMT (a_l, f_l, g_l, 00749 a_u, f_u, g_u, 00750 a_t, phi_t, d_phi_t); 00751 } 00752 00753 step_iterations++; 00754 } 00755 00756 // If inner loop was run then hessian needs to be calculated. 00757 // Hessian is unnessisary for step length determination but gradients are required 00758 // so derivative and transform data is stored for the next iteration. 00759 if (step_iterations) 00760 computeHessian (hessian, trans_cloud, x_t); 00761 00762 return (a_t); 00763 } 00764 00765 #endif // PCL_REGISTRATION_NDT_IMPL_H_