ndt.h
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-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 {
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 
00079       typedef VoxelGridCovariance<PointTarget> TargetGrid;
00081       typedef TargetGrid* TargetGridPtr;
00083       typedef const TargetGrid* TargetGridConstPtr;
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 
00097       NormalDistributionsTransform ();
00098       
00100       virtual ~NormalDistributionsTransform () {}
00101 
00105       inline void
00106       setInputTarget (const PointCloudTargetConstPtr &cloud)
00107       {
00108         Registration<PointSource, PointTarget>::setInputTarget (cloud);
00109         init ();
00110       }
00111 
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 
00130       inline float
00131       getResolution () const
00132       {
00133         return (resolution_);
00134       }
00135 
00139       inline double
00140       getStepSize () const
00141       {
00142         return (step_size_);
00143       }
00144 
00148       inline void
00149       setStepSize (double step_size)
00150       {
00151         step_size_ = step_size;
00152       }
00153 
00157       inline double
00158       getOulierRatio () const
00159       {
00160         return (outlier_ratio_);
00161       }
00162 
00166       inline void
00167       setOulierRatio (double outlier_ratio)
00168       {
00169         outlier_ratio_ = outlier_ratio;
00170       }
00171 
00175       inline double
00176       getTransformationProbability () const
00177       {
00178         return (trans_probability_);
00179       }
00180 
00184       inline int
00185       getFinalNumIteration () const
00186       {
00187         return (nr_iterations_);
00188       }
00189 
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 
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 
00237       virtual void
00238       computeTransformation (PointCloudSource &output)
00239       {
00240         computeTransformation (output, Eigen::Matrix4f::Identity ());
00241       }
00242 
00247       virtual void
00248       computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess);
00249 
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 
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 
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 
00294       void
00295       computeAngleDerivatives (Eigen::Matrix<double, 6, 1> &p, bool compute_hessian = true);
00296 
00302       void
00303       computePointDerivatives (Eigen::Vector3d &x, bool compute_hessian = true);
00304 
00311       void
00312       computeHessian (Eigen::Matrix<double, 6, 6> &hessian,
00313                       PointCloudSource &trans_cloud,
00314                       Eigen::Matrix<double, 6, 1> &p);
00315 
00322       void
00323       updateHessian (Eigen::Matrix<double, 6, 6> &hessian,
00324                      Eigen::Vector3d &x_trans, Eigen::Matrix3d &c_inv);
00325 
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 
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 
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 
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 
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 
00418       TargetGrid target_cells_;
00419 
00420       //double fitness_epsilon_;
00421 
00423       float resolution_;
00424 
00426       double step_size_;
00427 
00429       double outlier_ratio_;
00430 
00432       double gauss_d1_, gauss_d2_;
00433 
00435       double trans_probability_;
00436 
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 
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 
00455       Eigen::Matrix<double, 3, 6> point_gradient_;
00456 
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_


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