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) 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 { 00051 /** \brief Class to store vector value and first and second derivatives 00052 * (grad vector and hessian matrix), so they can be returned easily from 00053 * functions 00054 */ 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 00084 /** \brief A normal distribution estimation class. 00085 * 00086 * First the indices of of the points from a point cloud that should be 00087 * modelled by the distribution are added with addIdx (...). 00088 * 00089 * Then estimateParams (...) uses the stored point indices to estimate the 00090 * parameters of a normal distribution, and discards the stored indices. 00091 * 00092 * Finally the distriubution, and its derivatives, may be evaluated at any 00093 * point using test (...). 00094 */ 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 00106 /** \brief Store a point index to use later for estimating distribution parameters. 00107 * \param[in] i Point index to store 00108 */ 00109 void 00110 addIdx (size_t i) 00111 { 00112 pt_indices_.push_back (i); 00113 } 00114 00115 /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared. 00116 * \param[in] cloud Point cloud corresponding to indices passed to addIdx. 00117 * \param[in] min_covar_eigvalue_mult Set the smallest eigenvalue to this times the largest. 00118 */ 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 00157 /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. 00158 * \param[in] transformed_pt Location to evaluate at. 00159 * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation 00160 * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation 00161 * estimateParams must have been called after at least three points were provided, or this will return zero. 00162 * 00163 */ 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 00213 /** \brief Build a set of normal distributions modelling a 2D point cloud, 00214 * and provide the value and derivatives of the model at any point via the 00215 * test (...) function. 00216 */ 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 00253 /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. 00254 * \param[in] transformed_pt Location to evaluate at. 00255 * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation 00256 * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation 00257 */ 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: 00271 /** \brief Return the normal distribution covering the location of point p 00272 * \param[in] p a point 00273 */ 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 00298 /** \brief Build a Normal Distributions Transform of a 2D point cloud. This 00299 * consists of the sum of four overlapping models of the original points 00300 * with normal distributions. 00301 * The value and derivatives of the model at any point can be evaluated 00302 * with the test (...) function. 00303 */ 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: 00312 /** \brief 00313 * \param[in] cloud the input point cloud 00314 * \param[in] about Centre of the grid for normal distributions model 00315 * \param[in] extent Extent of grid for normal distributions model 00316 * \param[in] step Size of region that each normal distribution will model 00317 */ 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 00331 /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution. 00332 * \param[in] transformed_pt Location to evaluate at. 00333 * \param[in] cos_theta sin(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation 00334 * \param[in] sin_theta cos(theta) of the current rotation angle of rigid transformation: to avoid repeated evaluation 00335 */ 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 00374 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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