Point Cloud Library (PCL) 1.13.0
ndt_2d.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2011-2012, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#ifndef PCL_NDT_2D_IMPL_H_
42#define PCL_NDT_2D_IMPL_H_
43
44#include <boost/core/noncopyable.hpp> // for boost::noncopyable
45
46#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver, EigenSolver
47
48#include <cmath>
49#include <memory>
50
51namespace pcl {
52
53namespace ndt2d {
54/** \brief Class to store vector value and first and second derivatives
55 * (grad vector and hessian matrix), so they can be returned easily from
56 * functions
57 */
58template <unsigned N = 3, typename T = double>
61
62 Eigen::Matrix<T, N, N> hessian;
63 Eigen::Matrix<T, N, 1> grad;
65
68 {
70 r.hessian = Eigen::Matrix<T, N, N>::Zero();
71 r.grad = Eigen::Matrix<T, N, 1>::Zero();
72 r.value = 0;
73 return r;
74 }
75
78 {
79 hessian += r.hessian;
80 grad += r.grad;
81 value += r.value;
82 return *this;
83 }
84};
85
86/** \brief A normal distribution estimation class.
87 *
88 * First the indices of of the points from a point cloud that should be
89 * modelled by the distribution are added with addIdx (...).
90 *
91 * Then estimateParams (...) uses the stored point indices to estimate the
92 * parameters of a normal distribution, and discards the stored indices.
93 *
94 * Finally the distriubution, and its derivatives, may be evaluated at any
95 * point using test (...).
96 */
97template <typename PointT>
100
101public:
102 NormalDist() : min_n_(3), n_(0) {}
103
104 /** \brief Store a point index to use later for estimating distribution parameters.
105 * \param[in] i Point index to store
106 */
107 void
108 addIdx(std::size_t i)
109 {
110 pt_indices_.push_back(i);
111 }
112
113 /** \brief Estimate the normal distribution parameters given the point indices
114 * provided. Memory of point indices is cleared. \param[in] cloud Point cloud
115 * corresponding to indices passed to addIdx. \param[in] min_covar_eigvalue_mult Set
116 * the smallest eigenvalue to this times the largest.
117 */
118 void
119 estimateParams(const PointCloud& cloud, double min_covar_eigvalue_mult = 0.001)
120 {
121 Eigen::Vector2d sx = Eigen::Vector2d::Zero();
122 Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero();
123
124 for (auto i = pt_indices_.cbegin(); i != pt_indices_.cend(); i++) {
125 Eigen::Vector2d p(cloud[*i].x, cloud[*i].y);
126 sx += p;
127 sxx += p * p.transpose();
128 }
129
130 n_ = pt_indices_.size();
131
132 if (n_ >= min_n_) {
133 mean_ = sx / static_cast<double>(n_);
134 // Using maximum likelihood estimation as in the original paper
135 Eigen::Matrix2d covar =
136 (sxx - 2 * (sx * mean_.transpose())) / static_cast<double>(n_) +
137 mean_ * mean_.transpose();
138
139 Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver(covar);
140 if (solver.eigenvalues()[0] < min_covar_eigvalue_mult * solver.eigenvalues()[1]) {
141 PCL_DEBUG("[pcl::NormalDist::estimateParams] NDT normal fit: adjusting "
142 "eigenvalue %f\n",
143 solver.eigenvalues()[0]);
144 Eigen::Matrix2d l = solver.eigenvalues().asDiagonal();
145 Eigen::Matrix2d q = solver.eigenvectors();
146 // set minimum smallest eigenvalue:
147 l(0, 0) = l(1, 1) * min_covar_eigvalue_mult;
148 covar = q * l * q.transpose();
149 }
150 covar_inv_ = covar.inverse();
151 }
152
153 pt_indices_.clear();
154 }
155
156 /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of
157 * the point p given this distribution. \param[in] transformed_pt Location to
158 * evaluate at. \param[in] cos_theta sin(theta) of the current rotation angle
159 * of rigid transformation: to avoid repeated evaluation \param[in] sin_theta
160 * cos(theta) of the current rotation angle of rigid transformation: to avoid repeated
161 * evaluation estimateParams must have been called after at least three points were
162 * provided, or this will return zero.
163 *
164 */
166 test(const PointT& transformed_pt,
167 const double& cos_theta,
168 const double& sin_theta) const
169 {
170 if (n_ < min_n_)
172
174 const double x = transformed_pt.x;
175 const double y = transformed_pt.y;
176 const Eigen::Vector2d p_xy(transformed_pt.x, transformed_pt.y);
177 const Eigen::Vector2d q = p_xy - mean_;
178 const Eigen::RowVector2d qt_cvi(q.transpose() * covar_inv_);
179 const double exp_qt_cvi_q = std::exp(-0.5 * double(qt_cvi * q));
180 r.value = -exp_qt_cvi_q;
181
182 Eigen::Matrix<double, 2, 3> jacobian;
183 jacobian << 1, 0, -(x * sin_theta + y * cos_theta), 0, 1,
184 x * cos_theta - y * sin_theta;
185
186 for (std::size_t i = 0; i < 3; i++)
187 r.grad[i] = double(qt_cvi * jacobian.col(i)) * exp_qt_cvi_q;
188
189 // second derivative only for i == j == 2:
190 const Eigen::Vector2d d2q_didj(y * sin_theta - x * cos_theta,
191 -(x * sin_theta + y * cos_theta));
192
193 for (std::size_t i = 0; i < 3; i++)
194 for (std::size_t j = 0; j < 3; j++)
195 r.hessian(i, j) =
196 -exp_qt_cvi_q *
197 (double(-qt_cvi * jacobian.col(i)) * double(-qt_cvi * jacobian.col(j)) +
198 (-qt_cvi * ((i == 2 && j == 2) ? d2q_didj : Eigen::Vector2d::Zero())) +
199 (-jacobian.col(j).transpose() * covar_inv_ * jacobian.col(i)));
200
201 return r;
202 }
203
204protected:
205 const std::size_t min_n_;
206
207 std::size_t n_;
208 std::vector<std::size_t> pt_indices_;
209 Eigen::Vector2d mean_;
210 Eigen::Matrix2d covar_inv_;
211};
212
213/** \brief Build a set of normal distributions modelling a 2D point cloud,
214 * and provide the value and derivatives of the model at any point via the
215 * test (...) function.
216 */
217template <typename PointT>
218class NDTSingleGrid : public boost::noncopyable {
220 using PointCloudConstPtr = typename PointCloud::ConstPtr;
222
223public:
224 NDTSingleGrid(PointCloudConstPtr cloud,
225 const Eigen::Vector2f& about,
226 const Eigen::Vector2f& extent,
227 const Eigen::Vector2f& step)
228 : min_(about - extent)
229 , max_(min_ + 2 * extent)
230 , step_(step)
231 , cells_((max_[0] - min_[0]) / step_[0], (max_[1] - min_[1]) / step_[1])
233 {
234 // sort through all points, assigning them to distributions:
235 std::size_t used_points = 0;
236 for (std::size_t i = 0; i < cloud->size(); i++)
237 if (NormalDist* n = normalDistForPoint(cloud->at(i))) {
238 n->addIdx(i);
239 used_points++;
240 }
241
242 PCL_DEBUG("[pcl::NDTSingleGrid] NDT single grid %dx%d using %d/%d points\n",
243 cells_[0],
244 cells_[1],
245 used_points,
246 cloud->size());
247
248 // then bake the distributions such that they approximate the
249 // points (and throw away memory of the points)
250 for (int x = 0; x < cells_[0]; x++)
251 for (int y = 0; y < cells_[1]; y++)
252 normal_distributions_.coeffRef(x, y).estimateParams(*cloud);
253 }
254
255 /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of
256 * the point p given this distribution. \param[in] transformed_pt Location to
257 * evaluate at. \param[in] cos_theta sin(theta) of the current rotation angle
258 * of rigid transformation: to avoid repeated evaluation \param[in] sin_theta
259 * cos(theta) of the current rotation angle of rigid transformation: to avoid repeated
260 * evaluation
261 */
263 test(const PointT& transformed_pt,
264 const double& cos_theta,
265 const double& sin_theta) const
266 {
267 const NormalDist* n = normalDistForPoint(transformed_pt);
268 // index is in grid, return score from the normal distribution from
269 // the correct part of the grid:
270 if (n)
271 return n->test(transformed_pt, cos_theta, sin_theta);
273 }
274
275protected:
276 /** \brief Return the normal distribution covering the location of point p
277 * \param[in] p a point
278 */
281 {
282 // this would be neater in 3d...
283 Eigen::Vector2f idxf;
284 for (std::size_t i = 0; i < 2; i++)
285 idxf[i] = (p.getVector3fMap()[i] - min_[i]) / step_[i];
286 Eigen::Vector2i idxi = idxf.cast<int>();
287 for (std::size_t i = 0; i < 2; i++)
288 if (idxi[i] >= cells_[i] || idxi[i] < 0)
289 return nullptr;
290 // const cast to avoid duplicating this function in const and
291 // non-const variants...
292 return const_cast<NormalDist*>(&normal_distributions_.coeffRef(idxi[0], idxi[1]));
293 }
294
295 Eigen::Vector2f min_;
296 Eigen::Vector2f max_;
297 Eigen::Vector2f step_;
298 Eigen::Vector2i cells_;
299
300 Eigen::Matrix<NormalDist, Eigen::Dynamic, Eigen::Dynamic> normal_distributions_;
301};
302
303/** \brief Build a Normal Distributions Transform of a 2D point cloud. This
304 * consists of the sum of four overlapping models of the original points
305 * with normal distributions.
306 * The value and derivatives of the model at any point can be evaluated
307 * with the test (...) function.
308 */
309template <typename PointT>
310class NDT2D : public boost::noncopyable {
312 using PointCloudConstPtr = typename PointCloud::ConstPtr;
314
315public:
316 /** \brief
317 * \param[in] cloud the input point cloud
318 * \param[in] about Centre of the grid for normal distributions model
319 * \param[in] extent Extent of grid for normal distributions model
320 * \param[in] step Size of region that each normal distribution will model
321 */
322 NDT2D(PointCloudConstPtr cloud,
323 const Eigen::Vector2f& about,
324 const Eigen::Vector2f& extent,
325 const Eigen::Vector2f& step)
326 {
327 Eigen::Vector2f dx(step[0] / 2, 0);
328 Eigen::Vector2f dy(0, step[1] / 2);
329 single_grids_[0].reset(new SingleGrid(cloud, about, extent, step));
330 single_grids_[1].reset(new SingleGrid(cloud, about + dx, extent, step));
331 single_grids_[2].reset(new SingleGrid(cloud, about + dy, extent, step));
332 single_grids_[3].reset(new SingleGrid(cloud, about + dx + dy, extent, step));
333 }
334
335 /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of
336 * the point p given this distribution. \param[in] transformed_pt Location to
337 * evaluate at. \param[in] cos_theta sin(theta) of the current rotation angle
338 * of rigid transformation: to avoid repeated evaluation \param[in] sin_theta
339 * cos(theta) of the current rotation angle of rigid transformation: to avoid repeated
340 * evaluation
341 */
343 test(const PointT& transformed_pt,
344 const double& cos_theta,
345 const double& sin_theta) const
346 {
348 for (const auto& single_grid : single_grids_)
349 r += single_grid->test(transformed_pt, cos_theta, sin_theta);
350 return r;
351 }
352
353protected:
354 std::shared_ptr<SingleGrid> single_grids_[4];
355};
356
357} // namespace ndt2d
358} // namespace pcl
359
360namespace Eigen {
361
362/* This NumTraits specialisation is necessary because NormalDist is used as
363 * the element type of an Eigen Matrix.
364 */
365template <typename PointT>
366struct NumTraits<pcl::ndt2d::NormalDist<PointT>> {
367 using Real = double;
368 using Literal = double;
369 static Real
371 {
372 return 1.0;
373 }
374 enum {
375 IsComplex = 0,
376 IsInteger = 0,
377 IsSigned = 0,
378 RequireInitialization = 1,
379 ReadCost = 1,
380 AddCost = 1,
381 MulCost = 1
382 };
383};
384
385} // namespace Eigen
386
387namespace pcl {
388
389template <typename PointSource, typename PointTarget>
390void
392 PointCloudSource& output, const Eigen::Matrix4f& guess)
393{
394 PointCloudSource intm_cloud = output;
395
396 nr_iterations_ = 0;
397 converged_ = false;
398
399 if (guess != Eigen::Matrix4f::Identity()) {
400 transformation_ = guess;
401 transformPointCloud(output, intm_cloud, transformation_);
402 }
403
404 // build Normal Distribution Transform of target cloud:
405 ndt2d::NDT2D<PointTarget> target_ndt(target_, grid_centre_, grid_extent_, grid_step_);
406
407 // can't seem to use .block<> () member function on transformation_
408 // directly... gcc bug?
409 Eigen::Matrix4f& transformation = transformation_;
410
411 // work with x translation, y translation and z rotation: extending to 3D
412 // would be some tricky maths, but not impossible.
413 const Eigen::Matrix3f initial_rot(transformation.block<3, 3>(0, 0));
414 const Eigen::Vector3f rot_x(initial_rot * Eigen::Vector3f::UnitX());
415 const double z_rotation = std::atan2(rot_x[1], rot_x[0]);
416
417 Eigen::Vector3d xytheta_transformation(
418 transformation(0, 3), transformation(1, 3), z_rotation);
419
420 while (!converged_) {
421 const double cos_theta = std::cos(xytheta_transformation[2]);
422 const double sin_theta = std::sin(xytheta_transformation[2]);
423 previous_transformation_ = transformation;
424
427 for (std::size_t i = 0; i < intm_cloud.size(); i++)
428 score += target_ndt.test(intm_cloud[i], cos_theta, sin_theta);
429
430 PCL_DEBUG("[pcl::NormalDistributionsTransform2D::computeTransformation] NDT score "
431 "%f (x=%f,y=%f,r=%f)\n",
432 float(score.value),
433 xytheta_transformation[0],
434 xytheta_transformation[1],
435 xytheta_transformation[2]);
436
437 if (score.value != 0) {
438 // test for positive definiteness, and adjust to ensure it if necessary:
439 Eigen::EigenSolver<Eigen::Matrix3d> solver;
440 solver.compute(score.hessian, false);
441 double min_eigenvalue = 0;
442 for (int i = 0; i < 3; i++)
443 if (solver.eigenvalues()[i].real() < min_eigenvalue)
444 min_eigenvalue = solver.eigenvalues()[i].real();
445
446 // ensure "safe" positive definiteness: this is a detail missing
447 // from the original paper
448 if (min_eigenvalue < 0) {
449 double lambda = 1.1 * min_eigenvalue - 1;
450 score.hessian += Eigen::Vector3d(-lambda, -lambda, -lambda).asDiagonal();
451 solver.compute(score.hessian, false);
452 PCL_DEBUG("[pcl::NormalDistributionsTransform2D::computeTransformation] adjust "
453 "hessian: %f: new eigenvalues:%f %f %f\n",
454 float(lambda),
455 solver.eigenvalues()[0].real(),
456 solver.eigenvalues()[1].real(),
457 solver.eigenvalues()[2].real());
458 }
459 assert(solver.eigenvalues()[0].real() >= 0 &&
460 solver.eigenvalues()[1].real() >= 0 &&
461 solver.eigenvalues()[2].real() >= 0);
462
463 Eigen::Vector3d delta_transformation(-score.hessian.inverse() * score.grad);
464 Eigen::Vector3d new_transformation =
465 xytheta_transformation + newton_lambda_.cwiseProduct(delta_transformation);
466
467 xytheta_transformation = new_transformation;
468
469 // update transformation matrix from x, y, theta:
470 transformation.block<3, 3>(0, 0).matrix() = Eigen::Matrix3f(Eigen::AngleAxisf(
471 static_cast<float>(xytheta_transformation[2]), Eigen::Vector3f::UnitZ()));
472 transformation.block<3, 1>(0, 3).matrix() =
473 Eigen::Vector3f(static_cast<float>(xytheta_transformation[0]),
474 static_cast<float>(xytheta_transformation[1]),
475 0.0f);
476
477 // std::cout << "new transformation:\n" << transformation << std::endl;
478 }
479 else {
480 PCL_ERROR("[pcl::NormalDistributionsTransform2D::computeTransformation] no "
481 "overlap: try increasing the size or reducing the step of the grid\n");
482 break;
483 }
484
485 transformPointCloud(output, intm_cloud, transformation);
486
487 nr_iterations_++;
488
489 if (update_visualizer_)
490 update_visualizer_(output, *indices_, *target_, *indices_);
491
492 // std::cout << "eps=" << std::abs ((transformation - previous_transformation_).sum
493 // ()) << std::endl;
494
495 Eigen::Matrix4f transformation_delta =
496 transformation.inverse() * previous_transformation_;
497 double cos_angle =
498 0.5 * (transformation_delta.coeff(0, 0) + transformation_delta.coeff(1, 1) +
499 transformation_delta.coeff(2, 2) - 1);
500 double translation_sqr =
501 transformation_delta.coeff(0, 3) * transformation_delta.coeff(0, 3) +
502 transformation_delta.coeff(1, 3) * transformation_delta.coeff(1, 3) +
503 transformation_delta.coeff(2, 3) * transformation_delta.coeff(2, 3);
504
505 if (nr_iterations_ >= max_iterations_ ||
506 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
507 (transformation_rotation_epsilon_ > 0 &&
508 cos_angle >= transformation_rotation_epsilon_)) ||
509 ((transformation_epsilon_ <= 0) &&
510 (transformation_rotation_epsilon_ > 0 &&
511 cos_angle >= transformation_rotation_epsilon_)) ||
512 ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) &&
513 (transformation_rotation_epsilon_ <= 0))) {
514 converged_ = true;
515 }
516 }
517 final_transformation_ = transformation;
518 output = intm_cloud;
519}
520
521} // namespace pcl
522
523#endif // PCL_NDT_2D_IMPL_H_
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method with initial guess.
Definition: ndt_2d.hpp:391
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
Build a Normal Distributions Transform of a 2D point cloud.
Definition: ndt_2d.hpp:310
ValueAndDerivatives< 3, double > test(const PointT &transformed_pt, const double &cos_theta, const double &sin_theta) const
Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distr...
Definition: ndt_2d.hpp:343
std::shared_ptr< SingleGrid > single_grids_[4]
Definition: ndt_2d.hpp:354
NDT2D(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Definition: ndt_2d.hpp:322
Build a set of normal distributions modelling a 2D point cloud, and provide the value and derivatives...
Definition: ndt_2d.hpp:218
Eigen::Vector2f min_
Definition: ndt_2d.hpp:295
Eigen::Vector2f max_
Definition: ndt_2d.hpp:296
NormalDist * normalDistForPoint(PointT const &p) const
Return the normal distribution covering the location of point p.
Definition: ndt_2d.hpp:280
NDTSingleGrid(PointCloudConstPtr cloud, const Eigen::Vector2f &about, const Eigen::Vector2f &extent, const Eigen::Vector2f &step)
Definition: ndt_2d.hpp:224
Eigen::Matrix< NormalDist, Eigen::Dynamic, Eigen::Dynamic > normal_distributions_
Definition: ndt_2d.hpp:300
Eigen::Vector2i cells_
Definition: ndt_2d.hpp:298
Eigen::Vector2f step_
Definition: ndt_2d.hpp:297
ValueAndDerivatives< 3, double > test(const PointT &transformed_pt, const double &cos_theta, const double &sin_theta) const
Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distr...
Definition: ndt_2d.hpp:263
A normal distribution estimation class.
Definition: ndt_2d.hpp:98
std::vector< std::size_t > pt_indices_
Definition: ndt_2d.hpp:208
const std::size_t min_n_
Definition: ndt_2d.hpp:205
void addIdx(std::size_t i)
Store a point index to use later for estimating distribution parameters.
Definition: ndt_2d.hpp:108
void estimateParams(const PointCloud &cloud, double min_covar_eigvalue_mult=0.001)
Estimate the normal distribution parameters given the point indices provided.
Definition: ndt_2d.hpp:119
ValueAndDerivatives< 3, double > test(const PointT &transformed_pt, const double &cos_theta, const double &sin_theta) const
Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distr...
Definition: ndt_2d.hpp:166
Eigen::Vector2d mean_
Definition: ndt_2d.hpp:209
Eigen::Matrix2d covar_inv_
Definition: ndt_2d.hpp:210
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Definition: transforms.hpp:221
Definition: bfgs.h:10
A point structure representing Euclidean xyz coordinates, and the RGB color.
Class to store vector value and first and second derivatives (grad vector and hessian matrix),...
Definition: ndt_2d.hpp:59
Eigen::Matrix< T, N, N > hessian
Definition: ndt_2d.hpp:62
static ValueAndDerivatives< N, T > Zero()
Definition: ndt_2d.hpp:67
ValueAndDerivatives< N, T > & operator+=(ValueAndDerivatives< N, T > const &r)
Definition: ndt_2d.hpp:77
Eigen::Matrix< T, N, 1 > grad
Definition: ndt_2d.hpp:63