/* * 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 #include #include #include #include #include namespace pcl { /** \brief A 3D Normal Distribution Transform registration implementation for point * cloud data. \note For more information please see Magnusson, M. (2009). The * Three-Dimensional Normal-Distributions Transform — an Efficient Representation for * Registration, Surface Analysis, and Loop Detection. PhD thesis, Orebro University. * Orebro Studies in Technology 36., More, J., and Thuente, D. (1994). Line * Search Algorithm with Guaranteed Sufficient Decrease In ACM Transactions on * Mathematical Software. 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) */ template class NormalDistributionsTransform : public Registration { protected: using PointCloudSource = typename Registration::PointCloudSource; using PointCloudSourcePtr = typename PointCloudSource::Ptr; using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; using PointCloudTarget = typename Registration::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; /** \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>; using ConstPtr = shared_ptr>; /** \brief Constructor. * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ * to 1.0 */ NormalDistributionsTransform(); /** \brief Empty destructor */ ~NormalDistributionsTransform() {} /** \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::setInputTarget(cloud); init(); } /** \brief Set/change the voxel grid resolution. * \param[in] resolution side length of voxels */ inline void setResolution(float resolution) { // Prevents unnessary voxel initiations if (resolution_ != resolution) { resolution_ = resolution; if (input_) { init(); } } } /** \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 probability. * \return transformation probability */ inline double getTransformationProbability() const { return trans_probability_; } /** \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 transfomation vector */ static void convertTransform(const Eigen::Matrix& x, Eigen::Affine3f& trans) { trans = Eigen::Translation(x.head<3>().cast()) * Eigen::AngleAxis(float(x(3)), Eigen::Vector3f::UnitX()) * Eigen::AngleAxis(float(x(4)), Eigen::Vector3f::UnitY()) * Eigen::AngleAxis(float(x(5)), Eigen::Vector3f::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 transfomation * vector */ static void convertTransform(const Eigen::Matrix& x, Eigen::Matrix4f& trans) { Eigen::Affine3f _affine; convertTransform(x, _affine); trans = _affine.matrix(); } protected: using Registration::reg_name_; using Registration::getClassName; using Registration::input_; using Registration::indices_; using Registration::target_; using Registration::nr_iterations_; using Registration::max_iterations_; using Registration::previous_transformation_; using Registration::final_transformation_; using Registration::transformation_; using Registration::transformation_epsilon_; using Registration::transformation_rotation_epsilon_; using Registration::converged_; using Registration::corr_dist_threshold_; using Registration::inlier_threshold_; using Registration::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, Eigen::Matrix4f::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 Eigen::Matrix4f& 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 probability 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 probability function w.r.t. the * transformation vector \param[out] hessian the hessian matrix of the probability * 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, unnessissary for step calculation. */ double computeDerivatives(Eigen::Matrix& score_gradient, Eigen::Matrix& hessian, const PointCloudSource& trans_cloud, const Eigen::Matrix& transform, bool compute_hessian = true); /** \brief Compute individual point contirbutions to derivatives of probability * 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 * probability function w.r.t. the transformation vector \param[in,out] hessian the * hessian matrix of the probability 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, unnessissary for step * calculation. */ double updateDerivatives(Eigen::Matrix& score_gradient, Eigen::Matrix& hessian, const Eigen::Vector3d& x_trans, const Eigen::Matrix3d& c_inv, bool compute_hessian = true) const; /** \brief Precompute anglular 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, unnessissary for step * calculation. */ void computeAngleDerivatives(const Eigen::Matrix& 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, unnessissary for step * calculation. */ void computePointDerivatives(const Eigen::Vector3d& x, bool compute_hessian = true); /** \brief Compute hessian of probability function w.r.t. the transformation vector. * \note Equation 6.13 [Magnusson 2009]. * \param[out] hessian the hessian matrix of the probability function w.r.t. the * transformation vector \param[in] trans_cloud transformed point cloud */ void computeHessian(Eigen::Matrix& hessian, const PointCloudSource& trans_cloud); /** \brief Compute hessian of probability function w.r.t. the transformation vector. * \note Equation 6.13 [Magnusson 2009]. * \param[out] hessian the hessian matrix of the probability 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& hessian, const PointCloudSource& trans_cloud, const Eigen::Matrix& transform) { pcl::utils::ignore(transform); computeHessian(hessian, trans_cloud); } /** \brief Compute individual point contirbutions to hessian of probability function * w.r.t. the transformation vector. \note Equation 6.13 [Magnusson 2009]. * \param[in,out] hessian the hessian matrix of the probability 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& hessian, const Eigen::Vector3d& x_trans, const Eigen::Matrix3d& c_inv) const; /** \brief Compute line search step length and update transform and probability * 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 noramal 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& transform, Eigen::Matrix& step_dir, double step_init, double step_max, double step_min, double& score, Eigen::Matrix& score_gradient, Eigen::Matrix& 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_; /** \brief The maximum step length. */ double step_size_; /** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 * [Magnusson 2009]. */ double outlier_ratio_; /** \brief The normalization constants used fit the point distribution to a normal * distribution, Equation 6.8 [Magnusson 2009]. */ double gauss_d1_, gauss_d2_; /** \brief The probability score of the transform applied to the input cloud, * Equation 6.9 and 6.10 [Magnusson 2009]. */ double trans_probability_; /** \brief Precomputed Angular Gradient * * The precomputed angular derivatives for the jacobian of a transformation vector, * Equation 6.19 [Magnusson 2009]. */ Eigen::Matrix angular_jacobian_; /** \brief Precomputed Angular Hessian * * The precomputed angular derivatives for the hessian of a transformation vector, * Equation 6.19 [Magnusson 2009]. */ Eigen::Matrix 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 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 point_hessian_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace pcl #include