ndt_2d.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) 2011-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 #ifndef PCL_NDT_2D_IMPL_H_
00041 #define PCL_NDT_2D_IMPL_H_
00042 #include <cmath>
00043 
00044 #include <pcl/registration/eigen.h>
00045 #include <pcl/registration/boost.h>
00046 
00047 namespace pcl
00048 {
00049   namespace ndt2d
00050   {
00055     template<unsigned N=3, typename T=double>
00056     struct ValueAndDerivatives
00057     {
00058       ValueAndDerivatives () : hessian (), grad (), value () {}
00059 
00060       Eigen::Matrix<T, N, N> hessian;
00061       Eigen::Matrix<T, N, 1>    grad;
00062       T value;
00063       
00064       static ValueAndDerivatives<N,T>
00065       Zero ()
00066       {
00067         ValueAndDerivatives<N,T> r;
00068         r.hessian = Eigen::Matrix<T, N, N>::Zero ();
00069         r.grad    = Eigen::Matrix<T, N, 1>::Zero ();
00070         r.value   = 0;
00071         return r;
00072       }
00073 
00074       ValueAndDerivatives<N,T>&
00075       operator+= (ValueAndDerivatives<N,T> const& r)
00076       {
00077         hessian += r.hessian;
00078         grad    += r.grad;
00079         value   += r.value;
00080         return *this;
00081       }
00082     };
00083 
00095     template <typename PointT>
00096     class NormalDist
00097     {
00098       typedef pcl::PointCloud<PointT> PointCloud;
00099 
00100       public:
00101         NormalDist ()
00102           : min_n_ (3), n_ (0), pt_indices_ (), mean_ (), covar_inv_ ()
00103         {
00104         }
00105         
00109         void
00110         addIdx (size_t i)
00111         {
00112           pt_indices_.push_back (i);
00113         }
00114         
00119         void
00120         estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001)
00121         {
00122           Eigen::Vector2d sx  = Eigen::Vector2d::Zero ();
00123           Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero ();
00124           
00125           std::vector<size_t>::const_iterator i;
00126           for (i = pt_indices_.begin (); i != pt_indices_.end (); i++)
00127           {
00128             Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y);
00129             sx  += p;
00130             sxx += p * p.transpose ();
00131           }
00132           
00133           n_ = pt_indices_.size ();
00134 
00135           if (n_ >= min_n_)
00136           {
00137             mean_ = sx / static_cast<double> (n_);
00138             // Using maximum likelihood estimation as in the original paper
00139             Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / static_cast<double> (n_) + mean_ * mean_.transpose ();
00140 
00141             Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver (covar);
00142             if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1])
00143             {
00144               PCL_DEBUG ("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]);
00145               Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal ();
00146               Eigen::Matrix2d q = solver.eigenvectors ();
00147               // set minimum smallest eigenvalue:
00148               l (0,0) = l (1,1) * min_covar_eigvalue_mult;
00149               covar = q * l * q.transpose ();
00150             }
00151             covar_inv_ = covar.inverse ();
00152           }
00153 
00154           pt_indices_.clear ();
00155         }
00156 
00164         ValueAndDerivatives<3,double>
00165         test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
00166         {
00167           if (n_ < min_n_)
00168             return ValueAndDerivatives<3,double>::Zero ();
00169           
00170           ValueAndDerivatives<3,double> r;
00171           const double x = transformed_pt.x;
00172           const double y = transformed_pt.y;
00173           const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y);
00174           const Eigen::Vector2d q = p_xy - mean_;
00175           const Eigen::RowVector2d qt_cvi (q.transpose () * covar_inv_);
00176           const double exp_qt_cvi_q = std::exp (-0.5 * double (qt_cvi * q));
00177           r.value = -exp_qt_cvi_q;
00178 
00179           Eigen::Matrix<double, 2, 3> jacobian;
00180           jacobian <<
00181             1, 0, -(x * sin_theta + y*cos_theta),
00182             0, 1,   x * cos_theta - y*sin_theta;
00183           
00184           for (size_t i = 0; i < 3; i++)
00185             r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q;
00186           
00187           // second derivative only for i == j == 2:
00188           const Eigen::Vector2d d2q_didj (
00189               y * sin_theta - x*cos_theta,
00190             -(x * sin_theta + y*cos_theta)
00191           );
00192 
00193           for (size_t i = 0; i < 3; i++)
00194             for (size_t j = 0; j < 3; j++)
00195               r.hessian (i,j) = -exp_qt_cvi_q * (
00196                 double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) +
00197                 (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) +
00198                 (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i))
00199               );
00200           
00201           return r;
00202         }
00203 
00204     protected:
00205         const size_t min_n_;
00206 
00207         size_t n_;
00208         std::vector<size_t> pt_indices_;
00209         Eigen::Vector2d mean_;
00210         Eigen::Matrix2d covar_inv_;
00211     };
00212     
00217     template <typename PointT> 
00218     class NDTSingleGrid: public boost::noncopyable
00219     {
00220       typedef typename pcl::PointCloud<PointT> PointCloud;
00221       typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
00222       typedef typename pcl::ndt2d::NormalDist<PointT> NormalDist;
00223 
00224       public:
00225         NDTSingleGrid (PointCloudConstPtr cloud,
00226                        const Eigen::Vector2f& about,
00227                        const Eigen::Vector2f& extent,
00228                        const Eigen::Vector2f& step)
00229             : min_ (about - extent), max_ (min_ + 2*extent), step_ (step),
00230               cells_ ((max_[0]-min_[0]) / step_[0],
00231                       (max_[1]-min_[1]) / step_[1]),
00232               normal_distributions_ (cells_[0], cells_[1])
00233         {
00234           // sort through all points, assigning them to distributions:
00235           NormalDist* n;
00236           size_t used_points = 0;
00237           for (size_t i = 0; i < cloud->size (); i++)
00238           if ((n = normalDistForPoint (cloud->at (i))))
00239           {
00240             n->addIdx (i);
00241             used_points++;
00242           }
00243 
00244           PCL_DEBUG ("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n", cells_[0], cells_[1], used_points, cloud->size ());
00245 
00246           // then bake the distributions such that they approximate the
00247           // points (and throw away memory of the points)
00248           for (int x = 0; x < cells_[0]; x++)
00249             for (int y = 0; y < cells_[1]; y++)
00250               normal_distributions_.coeffRef (x,y).estimateParams (*cloud);
00251         }
00252         
00258         ValueAndDerivatives<3,double>
00259         test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
00260         {
00261           const NormalDist* n = normalDistForPoint (transformed_pt);
00262           // index is in grid, return score from the normal distribution from
00263           // the correct part of the grid:
00264           if (n)
00265             return n->test (transformed_pt, cos_theta, sin_theta);
00266           else
00267             return ValueAndDerivatives<3,double>::Zero ();
00268         }
00269 
00270       protected:
00274         NormalDist* 
00275         normalDistForPoint (PointT const& p) const
00276         {
00277           // this would be neater in 3d...
00278           Eigen::Vector2f idxf;
00279           for (size_t i = 0; i < 2; i++)
00280             idxf[i] = (p.getVector3fMap ()[i] - min_[i]) / step_[i];
00281           Eigen::Vector2i idxi = idxf.cast<int> ();
00282           for (size_t i = 0; i < 2; i++)
00283             if (idxi[i] >= cells_[i] || idxi[i] < 0)
00284               return NULL;
00285           // const cast to avoid duplicating this function in const and
00286           // non-const variants...
00287           return const_cast<NormalDist*> (&normal_distributions_.coeffRef (idxi[0], idxi[1]));
00288         }
00289 
00290         Eigen::Vector2f min_;
00291         Eigen::Vector2f max_;
00292         Eigen::Vector2f step_;
00293         Eigen::Vector2i cells_;
00294 
00295         Eigen::Matrix<NormalDist, Eigen::Dynamic, Eigen::Dynamic> normal_distributions_;
00296     };
00297 
00304     template <typename PointT> 
00305     class NDT2D: public boost::noncopyable
00306     {
00307       typedef typename pcl::PointCloud<PointT> PointCloud;
00308       typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
00309       typedef NDTSingleGrid<PointT> SingleGrid;
00310 
00311       public:
00318         NDT2D (PointCloudConstPtr cloud,
00319              const Eigen::Vector2f& about,
00320              const Eigen::Vector2f& extent,
00321              const Eigen::Vector2f& step)
00322         {
00323           Eigen::Vector2f dx (step[0]/2, 0);
00324           Eigen::Vector2f dy (0, step[1]/2);
00325           single_grids_[0] = boost::make_shared<SingleGrid> (cloud, about,        extent, step);
00326           single_grids_[1] = boost::make_shared<SingleGrid> (cloud, about +dx,    extent, step);
00327           single_grids_[2] = boost::make_shared<SingleGrid> (cloud, about +dy,    extent, step);
00328           single_grids_[3] = boost::make_shared<SingleGrid> (cloud, about +dx+dy, extent, step);
00329         }
00330         
00336         ValueAndDerivatives<3,double>
00337         test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
00338         {
00339           ValueAndDerivatives<3,double> r = ValueAndDerivatives<3,double>::Zero ();
00340           for (size_t i = 0; i < 4; i++)
00341               r += single_grids_[i]->test (transformed_pt, cos_theta, sin_theta);
00342           return r;
00343         }
00344 
00345       protected:
00346         boost::shared_ptr<SingleGrid> single_grids_[4];
00347     };
00348 
00349   } // namespace ndt2d
00350 } // namespace pcl
00351 
00352 
00353 namespace Eigen
00354 {
00355   /* This NumTraits specialisation is necessary because NormalDist is used as
00356    * the element type of an Eigen Matrix.
00357    */
00358   template<typename PointT> struct NumTraits<pcl::ndt2d::NormalDist<PointT> >
00359   {
00360     typedef double Real;
00361     static Real dummy_precision () { return 1.0; }
00362     enum {
00363       IsComplex = 0,
00364       IsInteger = 0,
00365       IsSigned = 0,
00366       RequireInitialization = 1,
00367       ReadCost = 1,
00368       AddCost = 1,
00369       MulCost = 1
00370     };
00371   };
00372 }
00373 
00375 template <typename PointSource, typename PointTarget> void
00376 pcl::NormalDistributionsTransform2D<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
00377 {
00378   PointCloudSource intm_cloud = output;
00379 
00380   if (guess != Eigen::Matrix4f::Identity ())
00381   {
00382     transformation_ = guess;
00383     transformPointCloud (output, intm_cloud, transformation_);
00384   } 
00385 
00386   // build Normal Distribution Transform of target cloud:
00387   ndt2d::NDT2D<PointTarget> target_ndt (target_, grid_centre_, grid_extent_, grid_step_);
00388   
00389   // can't seem to use .block<> () member function on transformation_
00390   // directly... gcc bug? 
00391   Eigen::Matrix4f& transformation = transformation_;
00392 
00393 
00394   // work with x translation, y translation and z rotation: extending to 3D
00395   // would be some tricky maths, but not impossible.
00396   const Eigen::Matrix3f initial_rot (transformation.block<3,3> (0,0));
00397   const Eigen::Vector3f rot_x (initial_rot*Eigen::Vector3f::UnitX ());
00398   const double z_rotation = std::atan2 (rot_x[1], rot_x[0]);
00399 
00400   Eigen::Vector3d xytheta_transformation (
00401     transformation (0,3),
00402     transformation (1,3),
00403     z_rotation
00404   );
00405 
00406   while (!converged_)
00407   {
00408     const double cos_theta = std::cos (xytheta_transformation[2]);
00409     const double sin_theta = std::sin (xytheta_transformation[2]);
00410     previous_transformation_ = transformation;    
00411 
00412     ndt2d::ValueAndDerivatives<3, double> score = ndt2d::ValueAndDerivatives<3, double>::Zero ();
00413     for (size_t i = 0; i < intm_cloud.size (); i++)
00414       score += target_ndt.test (intm_cloud[i], cos_theta, sin_theta);
00415     
00416     PCL_DEBUG ("[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score %f (x=%f,y=%f,r=%f)\n",
00417       float (score.value), xytheta_transformation[0], xytheta_transformation[1], xytheta_transformation[2]
00418     );
00419 
00420     if (score.value != 0)
00421     {
00422       // test for positive definiteness, and adjust to ensure it if necessary:
00423       Eigen::EigenSolver<Eigen::Matrix3d> solver;
00424       solver.compute (score.hessian, false);
00425       double min_eigenvalue = 0;
00426       for (int i = 0; i <3; i++)
00427         if (solver.eigenvalues ()[i].real () < min_eigenvalue)
00428             min_eigenvalue = solver.eigenvalues ()[i].real ();
00429 
00430       // ensure "safe" positive definiteness: this is a detail missing
00431       // from the original paper
00432       if (min_eigenvalue < 0)
00433       {
00434         double lambda = 1.1 * min_eigenvalue - 1;
00435         score.hessian += Eigen::Vector3d (-lambda, -lambda, -lambda).asDiagonal ();
00436         solver.compute (score.hessian, false);
00437         PCL_DEBUG ("[pcl::NormalDistributionsTransform2D::computeTransformation] adjust hessian: %f: new eigenvalues:%f %f %f\n",
00438             float (lambda),
00439             solver.eigenvalues ()[0].real (),
00440             solver.eigenvalues ()[1].real (),
00441             solver.eigenvalues ()[2].real ()
00442         );
00443       }
00444       assert (solver.eigenvalues ()[0].real () >= 0 &&
00445               solver.eigenvalues ()[1].real () >= 0 &&
00446               solver.eigenvalues ()[2].real () >= 0);
00447       
00448       Eigen::Vector3d delta_transformation (-score.hessian.inverse () * score.grad);
00449       Eigen::Vector3d new_transformation = xytheta_transformation + newton_lambda_.cwiseProduct (delta_transformation);
00450 
00451       xytheta_transformation = new_transformation;
00452       
00453       // update transformation matrix from x, y, theta:
00454       transformation.block<3,3> (0,0).matrix () = Eigen::Matrix3f (Eigen::AngleAxisf (static_cast<float> (xytheta_transformation[2]), Eigen::Vector3f::UnitZ ()));
00455       transformation.block<3,1> (0,3).matrix () = Eigen::Vector3f (static_cast<float> (xytheta_transformation[0]), static_cast<float> (xytheta_transformation[1]), 0.0f);
00456 
00457       //std::cout << "new transformation:\n" << transformation << std::endl;
00458     }
00459     else
00460     {
00461       PCL_ERROR ("[pcl::NormalDistributionsTransform2D::computeTransformation] no overlap: try increasing the size or reducing the step of the grid\n");
00462       break;
00463     }
00464     
00465     transformPointCloud (output, intm_cloud, transformation);
00466 
00467     nr_iterations_++;
00468     
00469     if (update_visualizer_ != 0)
00470       update_visualizer_ (output, *indices_, *target_, *indices_);
00471 
00472     //std::cout << "eps=" << fabs ((transformation - previous_transformation_).sum ()) << std::endl;
00473 
00474     if (nr_iterations_ > max_iterations_ ||
00475        (transformation - previous_transformation_).array ().abs ().sum () < transformation_epsilon_)
00476       converged_ = true;
00477   }
00478   final_transformation_ = transformation;
00479   output = intm_cloud;
00480 }
00481 
00482 #endif    // PCL_NDT_2D_IMPL_H_
00483  


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