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-2012, 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_H_ 00042 #define PCL_REGISTRATION_NDT_H_ 00043 00044 #include <pcl/registration/registration.h> 00045 #include <pcl/filters/voxel_grid_covariance.h> 00046 00047 #include <unsupported/Eigen/NonLinearOptimization> 00048 00049 namespace pcl 00050 { 00051 /** \brief A 3D Normal Distribution Transform registration implementation for point cloud data. 00052 * \note For more information please see 00053 * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform — 00054 * an Efficient Representation for Registration, Surface Analysis, and Loop Detection. 00055 * PhD thesis, Orebro University. Orebro Studies in Technology 36.</b>, 00056 * <b>More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease 00057 * In ACM Transactions on Mathematical Software.</b> and 00058 * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100 00059 * \note Math refactored by Todor Stoyanov. 00060 * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific) 00061 */ 00062 template<typename PointSource, typename PointTarget> 00063 class NormalDistributionsTransform : public Registration<PointSource, PointTarget> 00064 { 00065 protected: 00066 00067 typedef typename Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource; 00068 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00069 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00070 00071 typedef typename Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget; 00072 typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 00073 typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 00074 00075 typedef PointIndices::Ptr PointIndicesPtr; 00076 typedef PointIndices::ConstPtr PointIndicesConstPtr; 00077 00078 /** \brief Typename of searchable voxel grid containing mean and covariance. */ 00079 typedef VoxelGridCovariance<PointTarget> TargetGrid; 00080 /** \brief Typename of pointer to searchable voxel grid. */ 00081 typedef TargetGrid* TargetGridPtr; 00082 /** \brief Typename of const pointer to searchable voxel grid. */ 00083 typedef const TargetGrid* TargetGridConstPtr; 00084 /** \brief Typename of const pointer to searchable voxel grid leaf. */ 00085 typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr; 00086 00087 00088 public: 00089 00090 typedef boost::shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> > Ptr; 00091 typedef boost::shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> > ConstPtr; 00092 00093 00094 /** \brief Constructor. 00095 * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0 00096 */ 00097 NormalDistributionsTransform (); 00098 00099 /** \brief Empty destructor */ 00100 virtual ~NormalDistributionsTransform () {} 00101 00102 /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to). 00103 * \param[in] cloud the input point cloud target 00104 */ 00105 inline void 00106 setInputTarget (const PointCloudTargetConstPtr &cloud) 00107 { 00108 Registration<PointSource, PointTarget>::setInputTarget (cloud); 00109 init (); 00110 } 00111 00112 /** \brief Set/change the voxel grid resolution. 00113 * \param[in] resolution side length of voxels 00114 */ 00115 inline void 00116 setResolution (float resolution) 00117 { 00118 // Prevents unnessary voxel initiations 00119 if (resolution_ != resolution) 00120 { 00121 resolution_ = resolution; 00122 if (input_) 00123 init (); 00124 } 00125 } 00126 00127 /** \brief Get voxel grid resolution. 00128 * \return side length of voxels 00129 */ 00130 inline float 00131 getResolution () const 00132 { 00133 return (resolution_); 00134 } 00135 00136 /** \brief Get the newton line search maximum step length. 00137 * \return maximum step length 00138 */ 00139 inline double 00140 getStepSize () const 00141 { 00142 return (step_size_); 00143 } 00144 00145 /** \brief Set/change the newton line search maximum step length. 00146 * \param[in] step_size maximum step length 00147 */ 00148 inline void 00149 setStepSize (double step_size) 00150 { 00151 step_size_ = step_size; 00152 } 00153 00154 /** \brief Get the point cloud outlier ratio. 00155 * \return outlier ratio 00156 */ 00157 inline double 00158 getOulierRatio () const 00159 { 00160 return (outlier_ratio_); 00161 } 00162 00163 /** \brief Set/change the point cloud outlier ratio. 00164 * \param[in] outlier_ratio outlier ratio 00165 */ 00166 inline void 00167 setOulierRatio (double outlier_ratio) 00168 { 00169 outlier_ratio_ = outlier_ratio; 00170 } 00171 00172 /** \brief Get the registration alignment probability. 00173 * \return transformation probability 00174 */ 00175 inline double 00176 getTransformationProbability () const 00177 { 00178 return (trans_probability_); 00179 } 00180 00181 /** \brief Get the number of iterations required to calculate alignment. 00182 * \return final number of iterations 00183 */ 00184 inline int 00185 getFinalNumIteration () const 00186 { 00187 return (nr_iterations_); 00188 } 00189 00190 /** \brief Convert 6 element transformation vector to affine transformation. 00191 * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] 00192 * \param[out] trans affine transform corresponding to given transfomation vector 00193 */ 00194 static void 00195 convertTransform (const Eigen::Matrix<double, 6, 1> &x, Eigen::Affine3f &trans) 00196 { 00197 trans = Eigen::Translation<float, 3>(float (x (0)), float (x (1)), float (x (2))) * 00198 Eigen::AngleAxis<float>(float (x (3)), Eigen::Vector3f::UnitX ()) * 00199 Eigen::AngleAxis<float>(float (x (4)), Eigen::Vector3f::UnitY ()) * 00200 Eigen::AngleAxis<float>(float (x (5)), Eigen::Vector3f::UnitZ ()); 00201 } 00202 00203 /** \brief Convert 6 element transformation vector to transformation matrix. 00204 * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw] 00205 * \param[out] trans 4x4 transformation matrix corresponding to given transfomation vector 00206 */ 00207 static void 00208 convertTransform (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix4f &trans) 00209 { 00210 Eigen::Affine3f _affine; 00211 convertTransform (x, _affine); 00212 trans = _affine.matrix (); 00213 } 00214 00215 protected: 00216 00217 using Registration<PointSource, PointTarget>::reg_name_; 00218 using Registration<PointSource, PointTarget>::getClassName; 00219 using Registration<PointSource, PointTarget>::input_; 00220 using Registration<PointSource, PointTarget>::indices_; 00221 using Registration<PointSource, PointTarget>::target_; 00222 using Registration<PointSource, PointTarget>::nr_iterations_; 00223 using Registration<PointSource, PointTarget>::max_iterations_; 00224 using Registration<PointSource, PointTarget>::previous_transformation_; 00225 using Registration<PointSource, PointTarget>::final_transformation_; 00226 using Registration<PointSource, PointTarget>::transformation_; 00227 using Registration<PointSource, PointTarget>::transformation_epsilon_; 00228 using Registration<PointSource, PointTarget>::converged_; 00229 using Registration<PointSource, PointTarget>::corr_dist_threshold_; 00230 using Registration<PointSource, PointTarget>::inlier_threshold_; 00231 00232 using Registration<PointSource, PointTarget>::update_visualizer_; 00233 00234 /** \brief Estimate the transformation and returns the transformed source (input) as output. 00235 * \param[out] output the resultant input transfomed point cloud dataset 00236 */ 00237 virtual void 00238 computeTransformation (PointCloudSource &output) 00239 { 00240 computeTransformation (output, Eigen::Matrix4f::Identity ()); 00241 } 00242 00243 /** \brief Estimate the transformation and returns the transformed source (input) as output. 00244 * \param[out] output the resultant input transfomed point cloud dataset 00245 * \param[in] guess the initial gross estimation of the transformation 00246 */ 00247 virtual void 00248 computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess); 00249 00250 /** \brief Initiate covariance voxel structure. */ 00251 void inline 00252 init () 00253 { 00254 target_cells_.setLeafSize (resolution_, resolution_, resolution_); 00255 target_cells_.setInputCloud ( target_ ); 00256 // Initiate voxel structure. 00257 target_cells_.filter (true); 00258 } 00259 00260 /** \brief Compute derivatives of probability function w.r.t. the transformation vector. 00261 * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. 00262 * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector 00263 * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 00264 * \param[in] trans_cloud transformed point cloud 00265 * \param[in] p the current transform vector 00266 * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. 00267 */ 00268 double 00269 computeDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient, 00270 Eigen::Matrix<double, 6, 6> &hessian, 00271 PointCloudSource &trans_cloud, 00272 Eigen::Matrix<double, 6, 1> &p, 00273 bool compute_hessian = true); 00274 00275 /** \brief Compute individual point contirbutions to derivatives of probability function w.r.t. the transformation vector. 00276 * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009]. 00277 * \param[in,out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector 00278 * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 00279 * \param[in] x_trans transformed point minus mean of occupied covariance voxel 00280 * \param[in] c_inv covariance of occupied covariance voxel 00281 * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. 00282 */ 00283 double 00284 updateDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient, 00285 Eigen::Matrix<double, 6, 6> &hessian, 00286 Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv, 00287 bool compute_hessian = true); 00288 00289 /** \brief Precompute anglular components of derivatives. 00290 * \note Equation 6.19 and 6.21 [Magnusson 2009]. 00291 * \param[in] p the current transform vector 00292 * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. 00293 */ 00294 void 00295 computeAngleDerivatives (Eigen::Matrix<double, 6, 1> &p, bool compute_hessian = true); 00296 00297 /** \brief Compute point derivatives. 00298 * \note Equation 6.18-21 [Magnusson 2009]. 00299 * \param[in] x point from the input cloud 00300 * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation. 00301 */ 00302 void 00303 computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian = true); 00304 00305 /** \brief Compute hessian of probability function w.r.t. the transformation vector. 00306 * \note Equation 6.13 [Magnusson 2009]. 00307 * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 00308 * \param[in] trans_cloud transformed point cloud 00309 * \param[in] p the current transform vector 00310 */ 00311 void 00312 computeHessian (Eigen::Matrix<double, 6, 6> &hessian, 00313 PointCloudSource &trans_cloud, 00314 Eigen::Matrix<double, 6, 1> &p); 00315 00316 /** \brief Compute individual point contirbutions to hessian of probability function w.r.t. the transformation vector. 00317 * \note Equation 6.13 [Magnusson 2009]. 00318 * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector 00319 * \param[in] x_trans transformed point minus mean of occupied covariance voxel 00320 * \param[in] c_inv covariance of occupied covariance voxel 00321 */ 00322 void 00323 updateHessian (Eigen::Matrix<double, 6, 6> &hessian, 00324 Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv); 00325 00326 /** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method. 00327 * \note Search Algorithm [More, Thuente 1994] 00328 * \param[in] x initial transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009] 00329 * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009] 00330 * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009] 00331 * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994) 00332 * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994) 00333 * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009] 00334 * \param[in,out] score_gradient gradient of score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009] 00335 * \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009] 00336 * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009] 00337 * \return final step length 00338 */ 00339 double 00340 computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, 00341 Eigen::Matrix<double, 6, 1> &step_dir, 00342 double step_init, 00343 double step_max, double step_min, 00344 double &score, 00345 Eigen::Matrix<double, 6, 1> &score_gradient, 00346 Eigen::Matrix<double, 6, 6> &hessian, 00347 PointCloudSource &trans_cloud); 00348 00349 /** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994) 00350 * \note Updating Algorithm until some value satifies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ 00351 * and Modified Updating Algorithm from then on [More, Thuente 1994]. 00352 * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) 00353 * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm 00354 * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified Update Algorithm 00355 * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) 00356 * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm 00357 * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified Update Algorithm 00358 * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) 00359 * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified Update Algorithm 00360 * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified Update Algorithm 00361 * \return if interval converges 00362 */ 00363 bool 00364 updateIntervalMT (double &a_l, double &f_l, double &g_l, 00365 double &a_u, double &f_u, double &g_u, 00366 double a_t, double f_t, double g_t); 00367 00368 /** \brief Select new trial value for More-Thuente method. 00369 * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$ 00370 * until some value satifies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ 00371 * then \f$ \phi(\alpha_k) \f$ is used from then on. 00372 * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).<\b> 00373 * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994) 00374 * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994) 00375 * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994) 00376 * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994) 00377 * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994) 00378 * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994) 00379 * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994) 00380 * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994) 00381 * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994) 00382 * \return new trial value 00383 */ 00384 double 00385 trialValueSelectionMT (double a_l, double f_l, double g_l, 00386 double a_u, double f_u, double g_u, 00387 double a_t, double f_t, double g_t); 00388 00389 /** \brief Auxilary function used to determin endpoints of More-Thuente interval. 00390 * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994) 00391 * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994) 00392 * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994) 00393 * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994) 00394 * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) 00395 * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] 00396 * \return sufficent decrease value 00397 */ 00398 inline double 00399 auxilaryFunction_PsiMT (double a, double f_a, double f_0, double g_0, double mu = 1.e-4) 00400 { 00401 return (f_a - f_0 - mu * g_0 * a); 00402 } 00403 00404 /** \brief Auxilary function derivative used to determin endpoints of More-Thuente interval. 00405 * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994) 00406 * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994) 00407 * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994) 00408 * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994] 00409 * \return sufficent decrease derivative 00410 */ 00411 inline double 00412 auxilaryFunction_dPsiMT (double g_a, double g_0, double mu = 1.e-4) 00413 { 00414 return (g_a - mu * g_0); 00415 } 00416 00417 /** \brief The voxel grid generated from target cloud containing point means and covariances. */ 00418 TargetGrid target_cells_; 00419 00420 //double fitness_epsilon_; 00421 00422 /** \brief The side length of voxels. */ 00423 float resolution_; 00424 00425 /** \brief The maximum step length. */ 00426 double step_size_; 00427 00428 /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */ 00429 double outlier_ratio_; 00430 00431 /** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. */ 00432 double gauss_d1_, gauss_d2_; 00433 00434 /** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. */ 00435 double trans_probability_; 00436 00437 /** \brief Precomputed Angular Gradient 00438 * 00439 * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009]. 00440 */ 00441 Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_; 00442 00443 /** \brief Precomputed Angular Hessian 00444 * 00445 * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009]. 00446 */ 00447 Eigen::Vector3d h_ang_a2_, h_ang_a3_, 00448 h_ang_b2_, h_ang_b3_, 00449 h_ang_c2_, h_ang_c3_, 00450 h_ang_d1_, h_ang_d2_, h_ang_d3_, 00451 h_ang_e1_, h_ang_e2_, h_ang_e3_, 00452 h_ang_f1_, h_ang_f2_, h_ang_f3_; 00453 00454 /** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */ 00455 Eigen::Matrix<double, 3, 6> point_gradient_; 00456 00457 /** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */ 00458 Eigen::Matrix<double, 18, 6> point_hessian_; 00459 00460 public: 00461 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00462 00463 }; 00464 00465 } 00466 00467 #include <pcl/registration/impl/ndt.hpp> 00468 00469 #endif // PCL_REGISTRATION_NDT_H_