Loading...
Searching...
No Matches
ndt.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#pragma once
#include <pcl/common/utils.h>
#include <pcl/filters/voxel_grid_covariance.h>
#include <pcl/registration/registration.h>
#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <unsupported/Eigen/NonLinearOptimization>
namespace pcl {
/** \brief A 3D Normal Distribution Transform registration implementation for
* point cloud data.
* \note For more information please see <b>Magnusson, M. (2009). The
* Three-Dimensional Normal-Distributions Transform — an Efﬁcient Representation
* for Registration, Surface Analysis, and Loop Detection. PhD thesis, Orebro
* University. Orebro Studies in Technology 36.</b>, <b>More, J., and Thuente,
* D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease In ACM
* Transactions on Mathematical Software.</b> and Sun, W. and Yuan, Y, (2006)
* Optimization Theory and Methods: Nonlinear Programming. 89-100
* \note Math refactored by Todor Stoyanov.
* \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
* \ingroup registration
*/
template <typename PointSource, typename PointTarget, typename Scalar = float>
class NormalDistributionsTransform
: public Registration<PointSource, PointTarget, Scalar> {
protected:
using PointCloudSource =
typename Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
using PointCloudTarget =
typename Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
using PointIndicesPtr = PointIndices::Ptr;
using PointIndicesConstPtr = PointIndices::ConstPtr;
/** \brief Typename of searchable voxel grid containing mean and
* covariance. */
using TargetGrid = VoxelGridCovariance<PointTarget>;
/** \brief Typename of pointer to searchable voxel grid. */
using TargetGridPtr = TargetGrid*;
/** \brief Typename of const pointer to searchable voxel grid. */
using TargetGridConstPtr = const TargetGrid*;
/** \brief Typename of const pointer to searchable voxel grid leaf. */
using TargetGridLeafConstPtr = typename TargetGrid::LeafConstPtr;
public:
using Ptr =
shared_ptr<NormalDistributionsTransform<PointSource, PointTarget, Scalar>>;
using ConstPtr =
shared_ptr<const NormalDistributionsTransform<PointSource, PointTarget, Scalar>>;
using Vector3 = typename Eigen::Matrix<Scalar, 3, 1>;
using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;
using Affine3 = typename Eigen::Transform<Scalar, 3, Eigen::Affine>;
/** \brief Constructor. Sets \ref outlier_ratio_ to 0.55, \ref step_size_ to
* 0.1 and \ref resolution_ to 1.0
*/
NormalDistributionsTransform();
/** \brief Empty destructor */
~NormalDistributionsTransform() override = default;
/** \brief Provide a pointer to the input target (e.g., the point cloud that
* we want to align the input source to).
* \param[in] cloud the input point cloud target
*/
inline void
setInputTarget(const PointCloudTargetConstPtr& cloud) override
{
Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
init();
}
/** \brief Set/change the voxel grid resolution.
* \param[in] resolution side length of voxels
*/
inline void
setResolution(float resolution)
{
// Prevents unnecessary voxel initiations
if (resolution_ != resolution) {
resolution_ = resolution;
if (input_) {
init();
}
}
}
/** \brief Set the minimum number of points required for a cell to be used (must be 3
* or greater for covariance calculation). Calls the function of the underlying
* VoxelGridCovariance. This function must be called before `setInputTarget` and
* `setResolution`. \param[in] min_points_per_voxel the minimum number of points
* required for a voxel to be used
*/
inline void
setMinPointPerVoxel(unsigned int min_points_per_voxel)
{
target_cells_.setMinPointPerVoxel(min_points_per_voxel);
}
/** \brief Get voxel grid resolution.
* \return side length of voxels
*/
inline float
getResolution() const
{
return resolution_;
}
/** \brief Get the newton line search maximum step length.
* \return maximum step length
*/
inline double
getStepSize() const
{
return step_size_;
}
/** \brief Set/change the newton line search maximum step length.
* \param[in] step_size maximum step length
*/
inline void
setStepSize(double step_size)
{
step_size_ = step_size;
}
/** \brief Get the point cloud outlier ratio.
* \return outlier ratio
*/
inline double
getOulierRatio() const
{
return outlier_ratio_;
}
/** \brief Set/change the point cloud outlier ratio.
* \param[in] outlier_ratio outlier ratio
*/
inline void
setOulierRatio(double outlier_ratio)
{
outlier_ratio_ = outlier_ratio;
}
/** \brief Get the registration alignment likelihood.
* \return transformation likelihood
*/
inline double
getTransformationLikelihood() const
{
return trans_likelihood_;
}
/** \brief Get the registration alignment probability.
* \return transformation probability
*/
PCL_DEPRECATED(1,
16,
"The method `getTransformationProbability` has been renamed to "
"`getTransformationLikelihood`.")
inline double
getTransformationProbability() const
{
return trans_likelihood_;
}
/** \brief Get the number of iterations required to calculate alignment.
* \return final number of iterations
*/
inline int
getFinalNumIteration() const
{
return nr_iterations_;
}
/** \brief Convert 6 element transformation vector to affine transformation.
* \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
* \param[out] trans affine transform corresponding to given transformation
* vector
*/
static void
convertTransform(const Eigen::Matrix<double, 6, 1>& x, Affine3& trans)
{
trans = Eigen::Translation<Scalar, 3>(x.head<3>().cast<Scalar>()) *
Eigen::AngleAxis<Scalar>(static_cast<Scalar>(x(3)), Vector3::UnitX()) *
Eigen::AngleAxis<Scalar>(static_cast<Scalar>(x(4)), Vector3::UnitY()) *
Eigen::AngleAxis<Scalar>(static_cast<Scalar>(x(5)), Vector3::UnitZ());
}
/** \brief Convert 6 element transformation vector to transformation matrix.
* \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
* \param[out] trans 4x4 transformation matrix corresponding to given
* transformation vector
*/
static void
convertTransform(const Eigen::Matrix<double, 6, 1>& x, Matrix4& trans)
{
Affine3 _affine;
convertTransform(x, _affine);
trans = _affine.matrix();
}
protected:
using Registration<PointSource, PointTarget, Scalar>::reg_name_;
using Registration<PointSource, PointTarget, Scalar>::getClassName;
using Registration<PointSource, PointTarget, Scalar>::input_;
using Registration<PointSource, PointTarget, Scalar>::indices_;
using Registration<PointSource, PointTarget, Scalar>::target_;
using Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
using Registration<PointSource, PointTarget, Scalar>::max_iterations_;
using Registration<PointSource, PointTarget, Scalar>::previous_transformation_;
using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
using Registration<PointSource, PointTarget, Scalar>::transformation_;
using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
using Registration<PointSource, PointTarget, Scalar>::
transformation_rotation_epsilon_;
using Registration<PointSource, PointTarget, Scalar>::converged_;
using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;
using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
/** \brief Estimate the transformation and returns the transformed source
* (input) as output.
* \param[out] output the resultant input transformed point cloud dataset
*/
virtual void
computeTransformation(PointCloudSource& output)
{
computeTransformation(output, Matrix4::Identity());
}
/** \brief Estimate the transformation and returns the transformed source
* (input) as output.
* \param[out] output the resultant input transformed point cloud dataset
* \param[in] guess the initial gross estimation of the transformation
*/
void
computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
/** \brief Initiate covariance voxel structure. */
void inline init()
{
target_cells_.setLeafSize(resolution_, resolution_, resolution_);
target_cells_.setInputCloud(target_);
// Initiate voxel structure.
target_cells_.filter(true);
}
/** \brief Compute derivatives of likelihood function w.r.t. the
* transformation vector.
* \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
* \param[out] score_gradient the gradient vector of the likelihood function
* w.r.t. the transformation vector
* \param[out] hessian the hessian matrix of the likelihood function
* w.r.t. the transformation vector
* \param[in] trans_cloud transformed point cloud
* \param[in] transform the current transform vector
* \param[in] compute_hessian flag to calculate hessian, unnecessary for step
* calculation.
*/
double
computeDerivatives(Eigen::Matrix<double, 6, 1>& score_gradient,
Eigen::Matrix<double, 6, 6>& hessian,
const PointCloudSource& trans_cloud,
const Eigen::Matrix<double, 6, 1>& transform,
bool compute_hessian = true);
/** \brief Compute individual point contributions to derivatives of
* likelihood function w.r.t. the transformation vector.
* \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
* \param[in,out] score_gradient the gradient vector of the likelihood
* function w.r.t. the transformation vector
* \param[in,out] hessian the hessian matrix of the likelihood function
* w.r.t. the transformation vector
* \param[in] x_trans transformed point minus mean of occupied covariance
* voxel
* \param[in] c_inv covariance of occupied covariance voxel
* \param[in] compute_hessian flag to calculate hessian, unnecessary for step
* calculation.
*/
double
updateDerivatives(Eigen::Matrix<double, 6, 1>& score_gradient,
Eigen::Matrix<double, 6, 6>& hessian,
const Eigen::Vector3d& x_trans,
const Eigen::Matrix3d& c_inv,
bool compute_hessian = true) const;
/** \brief Precompute angular components of derivatives.
* \note Equation 6.19 and 6.21 [Magnusson 2009].
* \param[in] transform the current transform vector
* \param[in] compute_hessian flag to calculate hessian, unnecessary for step
* calculation.
*/
void
computeAngleDerivatives(const Eigen::Matrix<double, 6, 1>& transform,
bool compute_hessian = true);
/** \brief Compute point derivatives.
* \note Equation 6.18-21 [Magnusson 2009].
* \param[in] x point from the input cloud
* \param[in] compute_hessian flag to calculate hessian, unnecessary for step
* calculation.
*/
void
computePointDerivatives(const Eigen::Vector3d& x, bool compute_hessian = true);
/** \brief Compute hessian of likelihood function w.r.t. the transformation
* vector.
* \note Equation 6.13 [Magnusson 2009].
* \param[out] hessian the hessian matrix of the likelihood function
* w.r.t. the transformation vector
* \param[in] trans_cloud transformed point cloud
*/
void
computeHessian(Eigen::Matrix<double, 6, 6>& hessian,
const PointCloudSource& trans_cloud);
/** \brief Compute hessian of likelihood function w.r.t. the transformation
* vector.
* \note Equation 6.13 [Magnusson 2009].
* \param[out] hessian the hessian matrix of the likelihood function
* w.r.t. the transformation vector
* \param[in] trans_cloud transformed point cloud
* \param[in] transform the current transform vector
*/
PCL_DEPRECATED(1, 15, "Parameter `transform` is not required")
void
computeHessian(Eigen::Matrix<double, 6, 6>& hessian,
const PointCloudSource& trans_cloud,
const Eigen::Matrix<double, 6, 1>& transform)
{
pcl::utils::ignore(transform);
computeHessian(hessian, trans_cloud);
}
/** \brief Compute individual point contributions to hessian of likelihood
* function w.r.t. the transformation vector.
* \note Equation 6.13 [Magnusson 2009].
* \param[in,out] hessian the hessian matrix of the likelihood function
* w.r.t. the transformation vector
* \param[in] x_trans transformed point minus mean of occupied covariance
* voxel
* \param[in] c_inv covariance of occupied covariance voxel
*/
void
updateHessian(Eigen::Matrix<double, 6, 6>& hessian,
const Eigen::Vector3d& x_trans,
const Eigen::Matrix3d& c_inv) const;
/** \brief Compute line search step length and update transform and
* likelihood derivatives using More-Thuente method.
* \note Search Algorithm [More, Thuente 1994]
* \param[in] transform initial transformation vector, \f$ x \f$ in Equation
* 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson
* 2009]
* \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]
* \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in
* Moore-Thuente (1994) and the normal of \f$ \delta \vec{p} \f$ in Algorithm
* 2 [Magnusson 2009]
* \param[in] step_max maximum step length, \f$ \alpha_max \f$ in
* Moore-Thuente (1994)
* \param[in] step_min minimum step length, \f$ \alpha_min \f$ in
* Moore-Thuente (1994)
* \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]
* \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]
* \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]
* \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]
* \return final step length
*/
double
computeStepLengthMT(const Eigen::Matrix<double, 6, 1>& transform,
Eigen::Matrix<double, 6, 1>& step_dir,
double step_init,
double step_max,
double step_min,
double& score,
Eigen::Matrix<double, 6, 1>& score_gradient,
Eigen::Matrix<double, 6, 6>& hessian,
PointCloudSource& trans_cloud);
/** \brief Update interval of possible step lengths for More-Thuente method,
* \f$ I \f$ in More-Thuente (1994)
* \note Updating Algorithm until some value satisfies \f$ \psi(\alpha_k) \leq
* 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ and Modified Updating Algorithm
* from then on [More, Thuente 1994].
* \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$
* in Moore-Thuente (1994)
* \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
* \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
* \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$
* in Moore-Thuente (1994)
* \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
* \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
* \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
* \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
* \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
* \return if interval converges
*/
bool
updateIntervalMT(double& a_l,
double& f_l,
double& g_l,
double& a_u,
double& f_u,
double& g_u,
double a_t,
double f_t,
double g_t) const;
/** \brief Select new trial value for More-Thuente method.
* \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is
* used for \f$ f_k \f$ and \f$ g_k \f$ until some value satisfies the test
* \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$ then \f$
* \phi(\alpha_k) \f$ is used from then on.
* \note Interpolation Minimizer equations from Optimization Theory and
* Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).
* \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in
* Moore-Thuente (1994)
* \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994)
* \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente
* (1994)
* \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in
* Moore-Thuente (1994)
* \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente
* (1994)
* \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente
* (1994)
* \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente
* (1994)
* \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente
* (1994)
* \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in
* Moore-Thuente (1994)
* \return new trial value
*/
double
trialValueSelectionMT(double a_l,
double f_l,
double g_l,
double a_u,
double f_u,
double g_u,
double a_t,
double f_t,
double g_t) const;
/** \brief Auxiliary function used to determine endpoints of More-Thuente
* interval.
* \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
* \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
* \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in
* More-Thuente (1994)
* \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente
* (1994)
* \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in More-Thuente
* (1994)
* \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More,
* Thuente 1994]
* \return sufficient decrease value
*/
inline double
auxilaryFunction_PsiMT(
double a, double f_a, double f_0, double g_0, double mu = 1.e-4) const
{
return f_a - f_0 - mu * g_0 * a;
}
/** \brief Auxiliary function derivative used to determine endpoints of
* More-Thuente interval.
* \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente
* 1994)
* \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in
* More-Thuente (1994)
* \param[in] g_0 initial function gradient, \f$ \phi'(0) \f$ in More-Thuente
* (1994)
* \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More,
* Thuente 1994]
* \return sufficient decrease derivative
*/
inline double
auxilaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4) const
{
return g_a - mu * g_0;
}
/** \brief The voxel grid generated from target cloud containing point means
* and covariances. */
TargetGrid target_cells_;
/** \brief The side length of voxels. */
float resolution_{1.0f};
/** \brief The maximum step length. */
double step_size_{0.1};
/** \brief The ratio of outliers of points w.r.t. a normal distribution,
* Equation 6.7 [Magnusson 2009]. */
double outlier_ratio_{0.55};
/** \brief The normalization constants used fit the point distribution to a
* normal distribution, Equation 6.8 [Magnusson 2009]. */
double gauss_d1_{0.0}, gauss_d2_{0.0};
/** \brief The likelihood score of the transform applied to the input cloud,
* Equation 6.9 and 6.10 [Magnusson 2009]. */
union {
PCL_DEPRECATED(1,
16,
"`trans_probability_` has been renamed to `trans_likelihood_`.")
double trans_probability_;
double trans_likelihood_{0.0};
};
/** \brief Precomputed Angular Gradient
*
* The precomputed angular derivatives for the jacobian of a transformation
* vector, Equation 6.19 [Magnusson 2009].
*/
Eigen::Matrix<double, 8, 4> angular_jacobian_;
/** \brief Precomputed Angular Hessian
*
* The precomputed angular derivatives for the hessian of a transformation
* vector, Equation 6.19 [Magnusson 2009].
*/
Eigen::Matrix<double, 15, 4> angular_hessian_;
/** \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]. */
Eigen::Matrix<double, 3, 6> point_jacobian_;
/** \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]. */
Eigen::Matrix<double, 18, 6> point_hessian_;
public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
} // namespace pcl
#include <pcl/registration/impl/ndt.hpp>
pcl::NormalDistributionsTransform
A 3D Normal Distribution Transform registration implementation for point cloud data.
Definition ndt.h:67
pcl::NormalDistributionsTransform::computePointDerivatives
void computePointDerivatives(const Eigen::Vector3d &x, bool compute_hessian=true)
Compute point derivatives.
Definition ndt.hpp:321
pcl::NormalDistributionsTransform::getResolution
float getResolution() const
Get voxel grid resolution.
Definition ndt.h:151
pcl::NormalDistributionsTransform::ConstPtr
shared_ptr< const NormalDistributionsTransform< PointSource, PointTarget, Scalar > > ConstPtr
Definition ndt.h:96
pcl::NormalDistributionsTransform::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition ndt.h:76
pcl::NormalDistributionsTransform::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition ndt.h:80
pcl::NormalDistributionsTransform::getStepSize
double getStepSize() const
Get the newton line search maximum step length.
Definition ndt.h:160
pcl::NormalDistributionsTransform::trans_likelihood_
double trans_likelihood_
Definition ndt.h:580
pcl::NormalDistributionsTransform::gauss_d2_
double gauss_d2_
Definition ndt.h:571
pcl::NormalDistributionsTransform::point_hessian_
Eigen::Matrix< double, 18, 6 > point_hessian_
The second order derivative of the transformation of a point w.r.t.
Definition ndt.h:605
pcl::NormalDistributionsTransform::computeTransformation
virtual void computeTransformation(PointCloudSource &output)
Estimate the transformation and returns the transformed source (input) as output.
Definition ndt.h:275
pcl::NormalDistributionsTransform::TargetGridLeafConstPtr
typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr
Typename of const pointer to searchable voxel grid leaf.
Definition ndt.h:90
pcl::NormalDistributionsTransform::getFinalNumIteration
int getFinalNumIteration() const
Get the number of iterations required to calculate alignment.
Definition ndt.h:218
pcl::NormalDistributionsTransform::PointCloudTarget
typename Registration< PointSource, PointTarget, Scalar >::PointCloudTarget PointCloudTarget
Definition ndt.h:75
pcl::NormalDistributionsTransform::updateDerivatives
double updateDerivatives(Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv, bool compute_hessian=true) const
Compute individual point contributions to derivatives of likelihood function w.r.t.
Definition ndt.hpp:367
pcl::NormalDistributionsTransform::computeDerivatives
double computeDerivatives(Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud, const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Compute derivatives of likelihood function w.r.t.
Definition ndt.hpp:185
pcl::NormalDistributionsTransform::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition ndt.h:77
pcl::NormalDistributionsTransform::computeAngleDerivatives
void computeAngleDerivatives(const Eigen::Matrix< double, 6, 1 > &transform, bool compute_hessian=true)
Precompute angular components of derivatives.
Definition ndt.hpp:236
pcl::NormalDistributionsTransform::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition ndt.h:72
pcl::NormalDistributionsTransform::PointCloudSource
typename Registration< PointSource, PointTarget, Scalar >::PointCloudSource PointCloudSource
Definition ndt.h:70
pcl::NormalDistributionsTransform::NormalDistributionsTransform
NormalDistributionsTransform()
Constructor.
Definition ndt.hpp:48
pcl::NormalDistributionsTransform::updateIntervalMT
bool updateIntervalMT(double &a_l, double &f_l, double &g_l, double &a_u, double &f_u, double &g_u, double a_t, double f_t, double g_t) const
Update interval of possible step lengths for More-Thuente method, in More-Thuente (1994)
Definition ndt.hpp:492
pcl::NormalDistributionsTransform::computeHessian
void computeHessian(Eigen::Matrix< double, 6, 6 > &hessian, const PointCloudSource &trans_cloud)
Compute hessian of likelihood function w.r.t.
Definition ndt.hpp:415
pcl::NormalDistributionsTransform::init
void init()
Initiate covariance voxel structure.
Definition ndt.h:289
pcl::NormalDistributionsTransform::Vector3
typename Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition ndt.h:97
pcl::NormalDistributionsTransform::auxilaryFunction_dPsiMT
double auxilaryFunction_dPsiMT(double g_a, double g_0, double mu=1.e-4) const
Auxiliary function derivative used to determine endpoints of More-Thuente interval.
Definition ndt.h:550
pcl::NormalDistributionsTransform::resolution_
float resolution_
The side length of voxels.
Definition ndt.h:560
pcl::NormalDistributionsTransform::Matrix4
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition ndt.h:98
pcl::NormalDistributionsTransform::~NormalDistributionsTransform
~NormalDistributionsTransform() override=default
Empty destructor.
pcl::NormalDistributionsTransform::outlier_ratio_
double outlier_ratio_
The ratio of outliers of points w.r.t.
Definition ndt.h:567
pcl::NormalDistributionsTransform::updateHessian
void updateHessian(Eigen::Matrix< double, 6, 6 > &hessian, const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const
Compute individual point contributions to hessian of likelihood function w.r.t.
Definition ndt.hpp:457
pcl::NormalDistributionsTransform::setInputTarget
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Definition ndt.h:114
pcl::NormalDistributionsTransform::getTransformationProbability
double getTransformationProbability() const
Get the registration alignment probability.
Definition ndt.h:209
pcl::NormalDistributionsTransform::target_cells_
TargetGrid target_cells_
The voxel grid generated from target cloud containing point means and covariances.
Definition ndt.h:557
pcl::NormalDistributionsTransform::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition ndt.h:71
pcl::NormalDistributionsTransform::angular_jacobian_
Eigen::Matrix< double, 8, 4 > angular_jacobian_
Precomputed Angular Gradient.
Definition ndt.h:588
pcl::NormalDistributionsTransform::setMinPointPerVoxel
void setMinPointPerVoxel(unsigned int min_points_per_voxel)
Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance ...
Definition ndt.h:142
pcl::NormalDistributionsTransform::setOulierRatio
void setOulierRatio(double outlier_ratio)
Set/change the point cloud outlier ratio.
Definition ndt.h:187
pcl::NormalDistributionsTransform::getOulierRatio
double getOulierRatio() const
Get the point cloud outlier ratio.
Definition ndt.h:178
pcl::NormalDistributionsTransform::angular_hessian_
Eigen::Matrix< double, 15, 4 > angular_hessian_
Precomputed Angular Hessian.
Definition ndt.h:595
pcl::NormalDistributionsTransform::trans_probability_
double trans_probability_
Definition ndt.h:579
pcl::NormalDistributionsTransform::Ptr
shared_ptr< NormalDistributionsTransform< PointSource, PointTarget, Scalar > > Ptr
Definition ndt.h:94
pcl::NormalDistributionsTransform::convertTransform
static void convertTransform(const Eigen::Matrix< double, 6, 1 > &x, Matrix4 &trans)
Convert 6 element transformation vector to transformation matrix.
Definition ndt.h:243
pcl::NormalDistributionsTransform::setResolution
void setResolution(float resolution)
Set/change the voxel grid resolution.
Definition ndt.h:124
pcl::NormalDistributionsTransform::step_size_
double step_size_
The maximum step length.
Definition ndt.h:563
pcl::NormalDistributionsTransform::gauss_d1_
double gauss_d1_
The normalization constants used fit the point distribution to a normal distribution,...
Definition ndt.h:571
pcl::NormalDistributionsTransform::setStepSize
void setStepSize(double step_size)
Set/change the newton line search maximum step length.
Definition ndt.h:169
pcl::NormalDistributionsTransform::trialValueSelectionMT
double trialValueSelectionMT(double a_l, double f_l, double g_l, double a_u, double f_u, double g_u, double a_t, double f_t, double g_t) const
Select new trial value for More-Thuente method.
Definition ndt.hpp:537
pcl::NormalDistributionsTransform::PointIndicesPtr
PointIndices::Ptr PointIndicesPtr
Definition ndt.h:79
pcl::NormalDistributionsTransform::point_jacobian_
Eigen::Matrix< double, 3, 6 > point_jacobian_
The first order derivative of the transformation of a point w.r.t.
Definition ndt.h:600
pcl::NormalDistributionsTransform::auxilaryFunction_PsiMT
double auxilaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu=1.e-4) const
Auxiliary function used to determine endpoints of More-Thuente interval.
Definition ndt.h:531
pcl::NormalDistributionsTransform::computeStepLengthMT
double computeStepLengthMT(const Eigen::Matrix< double, 6, 1 > &transform, Eigen::Matrix< double, 6, 1 > &step_dir, double step_init, double step_max, double step_min, double &score, Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &hessian, PointCloudSource &trans_cloud)
Compute line search step length and update transform and likelihood derivatives using More-Thuente me...
Definition ndt.hpp:650
pcl::NormalDistributionsTransform::convertTransform
static void convertTransform(const Eigen::Matrix< double, 6, 1 > &x, Affine3 &trans)
Convert 6 element transformation vector to affine transformation.
Definition ndt.h:229
pcl::NormalDistributionsTransform::Affine3
typename Eigen::Transform< Scalar, 3, Eigen::Affine > Affine3
Definition ndt.h:99
pcl::NormalDistributionsTransform::getTransformationLikelihood
double getTransformationLikelihood() const
Get the registration alignment likelihood.
Definition ndt.h:196
pcl::PCLBase< PointSource >::input_
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
pcl::PCLBase::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
pcl::PCLBase< PointSource >::indices_
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
pcl::PointCloud< PointSource >
pcl::Registration
Registration represents the base registration class for general purpose, ICP-like methods.
Definition registration.h:57
pcl::Registration::final_transformation_
Matrix4 final_transformation_
The final transformation matrix estimated by the registration method after N iterations.
Definition registration.h:573
pcl::Registration::update_visualizer_
std::function< UpdateVisualizerCallbackSignature > update_visualizer_
Callback function to update intermediate source point cloud position during it's registration to the ...
Definition registration.h:653
pcl::Registration::corr_dist_threshold_
double corr_dist_threshold_
The maximum distance threshold between two correspondent points in source <-> target.
Definition registration.h:603
pcl::Registration::reg_name_
std::string reg_name_
The registration method name.
Definition registration.h:548
pcl::Registration::transformation_
Matrix4 transformation_
The transformation matrix estimated by the registration method.
Definition registration.h:576
pcl::Registration::setInputTarget
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Definition registration.hpp:61
pcl::Registration::nr_iterations_
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Definition registration.h:558
pcl::Registration::previous_transformation_
Matrix4 previous_transformation_
The previous transformation matrix estimated by the registration method (used internally).
Definition registration.h:580
pcl::Registration::converged_
bool converged_
Holds internal convergence state, given user parameters.
Definition registration.h:613
pcl::Registration::Matrix4
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition registration.h:59
pcl::Registration::max_iterations_
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition registration.h:563
pcl::Registration::transformation_rotation_epsilon_
double transformation_rotation_epsilon_
The maximum rotation difference between two consecutive transformations in order to consider converge...
Definition registration.h:590
pcl::Registration::inlier_threshold_
double inlier_threshold_
The inlier distance threshold for the internal RANSAC outlier rejection loop.
Definition registration.h:610
pcl::Registration::transformation_epsilon_
double transformation_epsilon_
The maximum difference between two consecutive transformations in order to consider convergence (user...
Definition registration.h:585
pcl::Registration::target_
PointCloudTargetConstPtr target_
The input point cloud dataset target.
Definition registration.h:569
pcl::Registration::getClassName
const std::string & getClassName() const
Abstract class get name method.
Definition registration.h:485
pcl::VoxelGridCovariance< PointTarget >
pcl::VoxelGridCovariance::setMinPointPerVoxel
void setMinPointPerVoxel(int min_points_per_voxel)
Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance ...
Definition voxel_grid_covariance.h:214
pcl::VoxelGridCovariance::filter
void filter(PointCloud &output, bool searchable=false)
Filter cloud and initializes voxel structure.
Definition voxel_grid_covariance.h:259
pcl::VoxelGridCovariance< PointTarget >::LeafConstPtr
const Leaf * LeafConstPtr
Const pointer to VoxelGridCovariance leaf structure.
Definition voxel_grid_covariance.h:190
pcl::VoxelGrid::setLeafSize
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
Definition voxel_grid.h:214
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
memory.h
Defines functions, macros and traits for allocating and using memory.
Eigen
Definition bfgs.h:10
pcl::utils::ignore
void ignore(const T &...)
Utility function to eliminate unused variable warnings.
Definition utils.h:62
pcl
Definition convolution.h:46
pcl_macros.h
Defines all the PCL and non-PCL macros used.
PCL_DEPRECATED
#define PCL_DEPRECATED(Major, Minor, Message)
macro for compatibility across compilers and help remove old deprecated items for the Major....
Definition pcl_macros.h:156
pcl::PointIndices::Ptr
shared_ptr< ::pcl::PointIndices > Ptr
Definition PointIndices.h:13
pcl::PointIndices::ConstPtr
shared_ptr< const ::pcl::PointIndices > ConstPtr
Definition PointIndices.h:14