Point Cloud Library (PCL)  1.7.0
/tmp/buildd/pcl-1.7-1.7.0/registration/include/pcl/registration/ndt.h
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_