ndt.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-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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:44