Loading...
Searching...
No Matches
gicp.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010, 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/registration/bfgs.h>
#include <pcl/registration/icp.h>
namespace pcl {
/** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the
* generalized iterative closest point algorithm as described by Alex Segal et al. in
* http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf
* The approach is based on using anisotropic cost functions to optimize the alignment
* after closest point assignments have been made.
* The original code uses GSL and ANN while in ours we use FLANN and Newton's method
* for optimization (call `useBFGS` to switch to BFGS optimizer, however Newton
* is usually faster and more accurate).
* \author Nizar Sallem
* \ingroup registration
*/
template <typename PointSource, typename PointTarget, typename Scalar = float>
class GeneralizedIterativeClosestPoint
: public IterativeClosestPoint<PointSource, PointTarget, Scalar> {
public:
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::reg_name_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::getClassName;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::indices_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::target_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::input_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::tree_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::tree_reciprocal_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::nr_iterations_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::max_iterations_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
previous_transformation_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::final_transformation_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformation_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
transformation_epsilon_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::converged_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::inlier_threshold_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::
min_number_correspondences_;
using IterativeClosestPoint<PointSource, PointTarget, Scalar>::update_visualizer_;
using PointCloudSource = pcl::PointCloud<PointSource>;
using PointCloudSourcePtr = typename PointCloudSource::Ptr;
using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
using PointCloudTarget = pcl::PointCloud<PointTarget>;
using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
using PointIndicesPtr = PointIndices::Ptr;
using PointIndicesConstPtr = PointIndices::ConstPtr;
using MatricesVector =
std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>;
using MatricesVectorPtr = shared_ptr<MatricesVector>;
using MatricesVectorConstPtr = shared_ptr<const MatricesVector>;
using InputKdTree = typename Registration<PointSource, PointTarget, Scalar>::KdTree;
using InputKdTreePtr =
typename Registration<PointSource, PointTarget, Scalar>::KdTreePtr;
using Ptr =
shared_ptr<GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
using ConstPtr = shared_ptr<
const GeneralizedIterativeClosestPoint<PointSource, PointTarget, Scalar>>;
using Vector3 = typename Eigen::Matrix<Scalar, 3, 1>;
using Vector4 = typename Eigen::Matrix<Scalar, 4, 1>;
using Vector6d = Eigen::Matrix<double, 6, 1>;
using Matrix3 = typename Eigen::Matrix<Scalar, 3, 3>;
using Matrix4 =
typename IterativeClosestPoint<PointSource, PointTarget, Scalar>::Matrix4;
using Matrix6d = Eigen::Matrix<double, 6, 6>;
using AngleAxis = typename Eigen::AngleAxis<Scalar>;
PCL_MAKE_ALIGNED_OPERATOR_NEW
/** \brief Empty constructor. */
GeneralizedIterativeClosestPoint() : mahalanobis_(0)
{
min_number_correspondences_ = 4;
reg_name_ = "GeneralizedIterativeClosestPoint";
max_iterations_ = 200;
transformation_epsilon_ = 5e-4;
corr_dist_threshold_ = 5.;
rigid_transformation_estimation_ = [this](const PointCloudSource& cloud_src,
const pcl::Indices& indices_src,
const PointCloudTarget& cloud_tgt,
const pcl::Indices& indices_tgt,
Matrix4& transformation_matrix) {
estimateRigidTransformationNewton(
cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
};
}
/** \brief Provide a pointer to the input dataset
* \param cloud the const boost shared pointer to a PointCloud message
*/
inline void
setInputSource(const PointCloudSourceConstPtr& cloud) override
{
if (cloud->points.empty()) {
PCL_ERROR(
"[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
getClassName().c_str());
return;
}
PointCloudSource input = *cloud;
// Set all the point.data[3] values to 1 to aid the rigid transformation
for (std::size_t i = 0; i < input.size(); ++i)
input[i].data[3] = 1.0;
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputSource(cloud);
input_covariances_.reset();
}
/** \brief Provide a pointer to the covariances of the input source (if computed
* externally!). If not set, GeneralizedIterativeClosestPoint will compute the
* covariances itself. Make sure to set the covariances AFTER setting the input source
* point cloud (setting the input source point cloud will reset the covariances).
* \param[in] covariances the input source covariances
*/
inline void
setSourceCovariances(const MatricesVectorPtr& covariances)
{
input_covariances_ = covariances;
}
/** \brief Provide a pointer to the input target (e.g., the point cloud that we want
* to align the input source to) \param[in] target the input point cloud target
*/
inline void
setInputTarget(const PointCloudTargetConstPtr& target) override
{
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::setInputTarget(
target);
target_covariances_.reset();
}
/** \brief Provide a pointer to the covariances of the input target (if computed
* externally!). If not set, GeneralizedIterativeClosestPoint will compute the
* covariances itself. Make sure to set the covariances AFTER setting the input source
* point cloud (setting the input source point cloud will reset the covariances).
* \param[in] covariances the input target covariances
*/
inline void
setTargetCovariances(const MatricesVectorPtr& covariances)
{
target_covariances_ = covariances;
}
/** \brief Estimate a rigid rotation transformation between a source and a target
* point cloud using an iterative non-linear BFGS approach.
* \param[in] cloud_src the source point cloud dataset
* \param[in] indices_src the vector of indices describing
* the points of interest in \a cloud_src
* \param[in] cloud_tgt the target point cloud dataset
* \param[in] indices_tgt the vector of indices describing
* the correspondences of the interest points from \a indices_src
* \param[in,out] transformation_matrix the resultant transformation matrix
*/
void
estimateRigidTransformationBFGS(const PointCloudSource& cloud_src,
const pcl::Indices& indices_src,
const PointCloudTarget& cloud_tgt,
const pcl::Indices& indices_tgt,
Matrix4& transformation_matrix);
/** \brief Estimate a rigid rotation transformation between a source and a target
* point cloud using an iterative non-linear Newton approach.
* \param[in] cloud_src the source point cloud dataset
* \param[in] indices_src the vector of indices describing
* the points of interest in \a cloud_src
* \param[in] cloud_tgt the target point cloud dataset
* \param[in] indices_tgt the vector of indices describing
* the correspondences of the interest points from \a indices_src
* \param[in,out] transformation_matrix the resultant transformation matrix
*/
void
estimateRigidTransformationNewton(const PointCloudSource& cloud_src,
const pcl::Indices& indices_src,
const PointCloudTarget& cloud_tgt,
const pcl::Indices& indices_tgt,
Matrix4& transformation_matrix);
/** \brief \return Mahalanobis distance matrix for the given point index */
inline const Eigen::Matrix3d&
mahalanobis(std::size_t index) const
{
assert(index < mahalanobis_.size());
return mahalanobis_[index];
}
/** \brief Computes the derivative of the cost function w.r.t rotation angles.
* rotation matrix is obtainded from rotation angles x[3], x[4] and x[5]
* \return d/d_Phi, d/d_Theta, d/d_Psi respectively in g[3], g[4] and g[5]
* \param[in] x array representing 3D transformation
* \param[in] dCost_dR_T the transpose of the derivative of the cost function w.r.t
* rotation matrix
* \param[out] g gradient vector
*/
void
computeRDerivative(const Vector6d& x,
const Eigen::Matrix3d& dCost_dR_T,
Vector6d& g) const;
/** \brief Set the rotation epsilon (maximum allowable difference between two
* consecutive rotations) in order for an optimization to be considered as having
* converged to the final solution.
* \param epsilon the rotation epsilon
*/
inline void
setRotationEpsilon(double epsilon)
{
rotation_epsilon_ = epsilon;
}
/** \brief Get the rotation epsilon (maximum allowable difference between two
* consecutive rotations) as set by the user.
*/
inline double
getRotationEpsilon() const
{
return rotation_epsilon_;
}
/** \brief Set the number of neighbors used when selecting a point neighbourhood
* to compute covariances.
* A higher value will bring more accurate covariance matrix but will make
* covariances computation slower.
* \param k the number of neighbors to use when computing covariances
*/
void
setCorrespondenceRandomness(int k)
{
k_correspondences_ = k;
}
/** \brief Get the number of neighbors used when computing covariances as set by
* the user
*/
int
getCorrespondenceRandomness() const
{
return k_correspondences_;
}
/** \brief Use BFGS optimizer instead of default Newton optimizer
*/
void
useBFGS()
{
rigid_transformation_estimation_ = [this](const PointCloudSource& cloud_src,
const pcl::Indices& indices_src,
const PointCloudTarget& cloud_tgt,
const pcl::Indices& indices_tgt,
Matrix4& transformation_matrix) {
estimateRigidTransformationBFGS(
cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
};
}
/** \brief Set maximum number of iterations at the optimization step
* \param[in] max maximum number of iterations for the optimizer
*/
void
setMaximumOptimizerIterations(int max)
{
max_inner_iterations_ = max;
}
/** \brief Return maximum number of iterations at the optimization step
*/
int
getMaximumOptimizerIterations() const
{
return max_inner_iterations_;
}
/** \brief Set the minimal translation gradient threshold for early optimization stop
* \param[in] tolerance translation gradient threshold in meters
*/
void
setTranslationGradientTolerance(double tolerance)
{
translation_gradient_tolerance_ = tolerance;
}
/** \brief Return the minimal translation gradient threshold for early optimization
* stop
*/
double
getTranslationGradientTolerance() const
{
return translation_gradient_tolerance_;
}
/** \brief Set the minimal rotation gradient threshold for early optimization stop
* \param[in] tolerance rotation gradient threshold in radians
*/
void
setRotationGradientTolerance(double tolerance)
{
rotation_gradient_tolerance_ = tolerance;
}
/** \brief Return the minimal rotation gradient threshold for early optimization stop
*/
double
getRotationGradientTolerance() const
{
return rotation_gradient_tolerance_;
}
protected:
/** \brief The number of neighbors used for covariances computation.
* default: 20
*/
int k_correspondences_{20};
/** \brief The epsilon constant for gicp paper; this is NOT the convergence
* tolerance
* default: 0.001
*/
double gicp_epsilon_{0.001};
/** The epsilon constant for rotation error. (In GICP the transformation epsilon
* is split in rotation part and translation part).
* default: 2e-3
*/
double rotation_epsilon_{2e-3};
/** \brief base transformation */
Matrix4 base_transformation_;
/** \brief Temporary pointer to the source dataset. */
const PointCloudSource* tmp_src_;
/** \brief Temporary pointer to the target dataset. */
const PointCloudTarget* tmp_tgt_;
/** \brief Temporary pointer to the source dataset indices. */
const pcl::Indices* tmp_idx_src_;
/** \brief Temporary pointer to the target dataset indices. */
const pcl::Indices* tmp_idx_tgt_;
/** \brief Input cloud points covariances. */
MatricesVectorPtr input_covariances_;
/** \brief Target cloud points covariances. */
MatricesVectorPtr target_covariances_;
/** \brief Mahalanobis matrices holder. */
std::vector<Eigen::Matrix3d> mahalanobis_;
/** \brief maximum number of optimizations */
int max_inner_iterations_{20};
/** \brief minimal translation gradient for early optimization stop */
double translation_gradient_tolerance_{1e-2};
/** \brief minimal rotation gradient for early optimization stop */
double rotation_gradient_tolerance_{1e-2};
/** \brief compute points covariances matrices according to the K nearest
* neighbors. K is set via setCorrespondenceRandomness() method.
* \param[in] cloud pointer to point cloud
* \param[in] tree KD tree performer for nearest neighbors search
* \param[out] cloud_covariances covariances matrices for each point in the cloud
*/
template <typename PointT>
void
computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
const typename pcl::search::KdTree<PointT>::Ptr tree,
MatricesVector& cloud_covariances);
/** \return trace of mat1 . mat2
* \param mat1 matrix of dimension nxm
* \param mat2 matrix of dimension mxp
*/
inline double
matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
{
if (mat1.cols() != mat2.rows()) {
PCL_THROW_EXCEPTION(PCLException,
"The two matrices' shapes don't match. "
"They are ("
<< mat1.rows() << ", " << mat1.cols() << ") and ("
<< mat2.rows() << ", " << mat2.cols() << ")");
}
return (mat1 * mat2).trace();
}
/** \brief Rigid transformation computation method with initial guess.
* \param output the transformed input point cloud dataset using the rigid
* transformation found \param guess the initial guess of the transformation to
* compute
*/
void
computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
/** \brief Search for the closest nearest neighbor of a given point.
* \param query the point to search a nearest neighbour for
* \param index vector of size 1 to store the index of the nearest neighbour found
* \param distance vector of size 1 to store the distance to nearest neighbour found
*/
inline bool
searchForNeighbors(const PointSource& query,
pcl::Indices& index,
std::vector<float>& distance)
{
int k = tree_->nearestKSearch(query, 1, index, distance);
if (k == 0)
return (false);
return (true);
}
/// \brief compute transformation matrix from transformation matrix
void
applyState(Matrix4& t, const Vector6d& x) const;
/// \brief optimization functor structure
struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double, 6> {
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint* gicp)
: BFGSDummyFunctor<double, 6>(), gicp_(gicp)
{}
double
operator()(const Vector6d& x) override;
void
df(const Vector6d& x, Vector6d& df) override;
void
fdf(const Vector6d& x, double& f, Vector6d& df) override;
void
dfddf(const Vector6d& x, Vector6d& df, Matrix6d& ddf);
BFGSSpace::Status
checkGradient(const Vector6d& g) override;
const GeneralizedIterativeClosestPoint* gicp_;
};
std::function<void(const pcl::PointCloud<PointSource>& cloud_src,
const pcl::Indices& src_indices,
const pcl::PointCloud<PointTarget>& cloud_tgt,
const pcl::Indices& tgt_indices,
Matrix4& transformation_matrix)>
rigid_transformation_estimation_;
private:
void
getRDerivatives(double phi,
double theta,
double psi,
Eigen::Matrix3d& dR_dPhi,
Eigen::Matrix3d& dR_dTheta,
Eigen::Matrix3d& dR_dPsi) const;
void
getR2ndDerivatives(double phi,
double theta,
double psi,
Eigen::Matrix3d& ddR_dPhi_dPhi,
Eigen::Matrix3d& ddR_dPhi_dTheta,
Eigen::Matrix3d& ddR_dPhi_dPsi,
Eigen::Matrix3d& ddR_dTheta_dTheta,
Eigen::Matrix3d& ddR_dTheta_dPsi,
Eigen::Matrix3d& ddR_dPsi_dPsi) const;
};
} // namespace pcl
#include <pcl/registration/impl/gicp.hpp>
pcl::GeneralizedIterativeClosestPoint
GeneralizedIterativeClosestPoint is an ICP variant that implements the generalized iterative closest ...
Definition gicp.h:60
pcl::GeneralizedIterativeClosestPoint::rotation_epsilon_
double rotation_epsilon_
The epsilon constant for rotation error.
Definition gicp.h:374
pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS
void estimateRigidTransformationBFGS(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &indices_tgt, Matrix4 &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
Definition gicp.hpp:274
pcl::GeneralizedIterativeClosestPoint::setTranslationGradientTolerance
void setTranslationGradientTolerance(double tolerance)
Set the minimal translation gradient threshold for early optimization stop.
Definition gicp.h:327
pcl::GeneralizedIterativeClosestPoint::getCorrespondenceRandomness
int getCorrespondenceRandomness() const
Get the number of neighbors used when computing covariances as set by the user.
Definition gicp.h:286
pcl::GeneralizedIterativeClosestPoint::setCorrespondenceRandomness
void setCorrespondenceRandomness(int k)
Set the number of neighbors used when selecting a point neighbourhood to compute covariances.
Definition gicp.h:277
pcl::GeneralizedIterativeClosestPoint::InputKdTree
typename Registration< PointSource, PointTarget, Scalar >::KdTree InputKdTree
Definition gicp.h:100
pcl::GeneralizedIterativeClosestPoint::Matrix4
typename IterativeClosestPoint< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition gicp.h:114
pcl::GeneralizedIterativeClosestPoint::applyState
void applyState(Matrix4 &t, const Vector6d &x) const
compute transformation matrix from transformation matrix
Definition gicp.hpp:903
pcl::GeneralizedIterativeClosestPoint::PointCloudTarget
pcl::PointCloud< PointTarget > PointCloudTarget
Definition gicp.h:88
pcl::GeneralizedIterativeClosestPoint::tmp_idx_tgt_
const pcl::Indices * tmp_idx_tgt_
Temporary pointer to the target dataset indices.
Definition gicp.h:389
pcl::GeneralizedIterativeClosestPoint::PointCloudSource
pcl::PointCloud< PointSource > PointCloudSource
Definition gicp.h:84
pcl::GeneralizedIterativeClosestPoint::base_transformation_
Matrix4 base_transformation_
base transformation
Definition gicp.h:377
pcl::GeneralizedIterativeClosestPoint::PointIndicesConstPtr
PointIndices::ConstPtr PointIndicesConstPtr
Definition gicp.h:93
pcl::GeneralizedIterativeClosestPoint::Ptr
shared_ptr< GeneralizedIterativeClosestPoint< PointSource, PointTarget, Scalar > > Ptr
Definition gicp.h:105
pcl::GeneralizedIterativeClosestPoint::translation_gradient_tolerance_
double translation_gradient_tolerance_
minimal translation gradient for early optimization stop
Definition gicp.h:404
pcl::GeneralizedIterativeClosestPoint::Matrix6d
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition gicp.h:115
pcl::GeneralizedIterativeClosestPoint::tmp_tgt_
const PointCloudTarget * tmp_tgt_
Temporary pointer to the target dataset.
Definition gicp.h:383
pcl::GeneralizedIterativeClosestPoint::mahalanobis
const Eigen::Matrix3d & mahalanobis(std::size_t index) const
Definition gicp.h:231
pcl::GeneralizedIterativeClosestPoint::getMaximumOptimizerIterations
int getMaximumOptimizerIterations() const
Return maximum number of iterations at the optimization step.
Definition gicp.h:318
pcl::GeneralizedIterativeClosestPoint::input_covariances_
MatricesVectorPtr input_covariances_
Input cloud points covariances.
Definition gicp.h:392
pcl::GeneralizedIterativeClosestPoint::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition gicp.h:86
pcl::GeneralizedIterativeClosestPoint::Vector4
typename Eigen::Matrix< Scalar, 4, 1 > Vector4
Definition gicp.h:110
pcl::GeneralizedIterativeClosestPoint::Vector3
typename Eigen::Matrix< Scalar, 3, 1 > Vector3
Definition gicp.h:109
pcl::GeneralizedIterativeClosestPoint::setInputSource
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition gicp.h:142
pcl::GeneralizedIterativeClosestPoint::setTargetCovariances
void setTargetCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input target (if computed externally!).
Definition gicp.h:190
pcl::GeneralizedIterativeClosestPoint::MatricesVector
std::vector< Eigen::Matrix3d, Eigen::aligned_allocator< Eigen::Matrix3d > > MatricesVector
Definition gicp.h:96
pcl::GeneralizedIterativeClosestPoint::setSourceCovariances
void setSourceCovariances(const MatricesVectorPtr &covariances)
Provide a pointer to the covariances of the input source (if computed externally!).
Definition gicp.h:167
pcl::GeneralizedIterativeClosestPoint::MatricesVectorConstPtr
shared_ptr< const MatricesVector > MatricesVectorConstPtr
Definition gicp.h:98
pcl::GeneralizedIterativeClosestPoint::InputKdTreePtr
typename Registration< PointSource, PointTarget, Scalar >::KdTreePtr InputKdTreePtr
Definition gicp.h:102
pcl::GeneralizedIterativeClosestPoint::computeCovariances
void computeCovariances(typename pcl::PointCloud< PointT >::ConstPtr cloud, const typename pcl::search::KdTree< PointT >::Ptr tree, MatricesVector &cloud_covariances)
compute points covariances matrices according to the K nearest neighbors.
Definition gicp.hpp:51
pcl::GeneralizedIterativeClosestPoint::estimateRigidTransformationNewton
void estimateRigidTransformationNewton(const PointCloudSource &cloud_src, const pcl::Indices &indices_src, const PointCloudTarget &cloud_tgt, const pcl::Indices &indices_tgt, Matrix4 &transformation_matrix)
Estimate a rigid rotation transformation between a source and a target point cloud using an iterative...
Definition gicp.hpp:350
pcl::GeneralizedIterativeClosestPoint::useBFGS
void useBFGS()
Use BFGS optimizer instead of default Newton optimizer.
Definition gicp.h:294
pcl::GeneralizedIterativeClosestPoint::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition gicp.h:85
pcl::GeneralizedIterativeClosestPoint::PointCloudTargetPtr
typename PointCloudTarget::Ptr PointCloudTargetPtr
Definition gicp.h:89
pcl::GeneralizedIterativeClosestPoint::tmp_src_
const PointCloudSource * tmp_src_
Temporary pointer to the source dataset.
Definition gicp.h:380
pcl::GeneralizedIterativeClosestPoint::max_inner_iterations_
int max_inner_iterations_
maximum number of optimizations
Definition gicp.h:401
pcl::GeneralizedIterativeClosestPoint::gicp_epsilon_
double gicp_epsilon_
The epsilon constant for gicp paper; this is NOT the convergence tolerance default: 0....
Definition gicp.h:368
pcl::GeneralizedIterativeClosestPoint::getRotationEpsilon
double getRotationEpsilon() const
Get the rotation epsilon (maximum allowable difference between two consecutive rotations) as set by t...
Definition gicp.h:265
pcl::GeneralizedIterativeClosestPoint::ConstPtr
shared_ptr< const GeneralizedIterativeClosestPoint< PointSource, PointTarget, Scalar > > ConstPtr
Definition gicp.h:107
pcl::GeneralizedIterativeClosestPoint::setRotationEpsilon
void setRotationEpsilon(double epsilon)
Set the rotation epsilon (maximum allowable difference between two consecutive rotations) in order fo...
Definition gicp.h:256
pcl::GeneralizedIterativeClosestPoint::matricesInnerProd
double matricesInnerProd(const Eigen::MatrixXd &mat1, const Eigen::MatrixXd &mat2) const
Definition gicp.h:426
pcl::GeneralizedIterativeClosestPoint::GeneralizedIterativeClosestPoint
PCL_MAKE_ALIGNED_OPERATOR_NEW GeneralizedIterativeClosestPoint()
Empty constructor.
Definition gicp.h:121
pcl::GeneralizedIterativeClosestPoint::PointIndicesPtr
PointIndices::Ptr PointIndicesPtr
Definition gicp.h:92
pcl::GeneralizedIterativeClosestPoint::getRotationGradientTolerance
double getRotationGradientTolerance() const
Return the minimal rotation gradient threshold for early optimization stop.
Definition gicp.h:353
pcl::GeneralizedIterativeClosestPoint::k_correspondences_
int k_correspondences_
The number of neighbors used for covariances computation.
Definition gicp.h:362
pcl::GeneralizedIterativeClosestPoint::mahalanobis_
std::vector< Eigen::Matrix3d > mahalanobis_
Mahalanobis matrices holder.
Definition gicp.h:398
pcl::GeneralizedIterativeClosestPoint::setMaximumOptimizerIterations
void setMaximumOptimizerIterations(int max)
Set maximum number of iterations at the optimization step.
Definition gicp.h:310
pcl::GeneralizedIterativeClosestPoint::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition gicp.h:90
pcl::GeneralizedIterativeClosestPoint::target_covariances_
MatricesVectorPtr target_covariances_
Target cloud points covariances.
Definition gicp.h:395
pcl::GeneralizedIterativeClosestPoint::computeRDerivative
void computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &dCost_dR_T, Vector6d &g) const
Computes the derivative of the cost function w.r.t rotation angles.
Definition gicp.hpp:182
pcl::GeneralizedIterativeClosestPoint::AngleAxis
typename Eigen::AngleAxis< Scalar > AngleAxis
Definition gicp.h:116
pcl::GeneralizedIterativeClosestPoint::computeTransformation
void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override
Rigid transformation computation method with initial guess.
Definition gicp.hpp:747
pcl::GeneralizedIterativeClosestPoint::getTranslationGradientTolerance
double getTranslationGradientTolerance() const
Return the minimal translation gradient threshold for early optimization stop.
Definition gicp.h:336
pcl::GeneralizedIterativeClosestPoint::setInputTarget
void setInputTarget(const PointCloudTargetConstPtr &target) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Definition gicp.h:176
pcl::GeneralizedIterativeClosestPoint::Matrix3
typename Eigen::Matrix< Scalar, 3, 3 > Matrix3
Definition gicp.h:112
pcl::GeneralizedIterativeClosestPoint::MatricesVectorPtr
shared_ptr< MatricesVector > MatricesVectorPtr
Definition gicp.h:97
pcl::GeneralizedIterativeClosestPoint::rotation_gradient_tolerance_
double rotation_gradient_tolerance_
minimal rotation gradient for early optimization stop
Definition gicp.h:407
pcl::GeneralizedIterativeClosestPoint::rigid_transformation_estimation_
std::function< void(const pcl::PointCloud< PointSource > &cloud_src, const pcl::Indices &src_indices, const pcl::PointCloud< PointTarget > &cloud_tgt, const pcl::Indices &tgt_indices, Matrix4 &transformation_matrix)> rigid_transformation_estimation_
Definition gicp.h:490
pcl::GeneralizedIterativeClosestPoint::setRotationGradientTolerance
void setRotationGradientTolerance(double tolerance)
Set the minimal rotation gradient threshold for early optimization stop.
Definition gicp.h:345
pcl::GeneralizedIterativeClosestPoint::searchForNeighbors
bool searchForNeighbors(const PointSource &query, pcl::Indices &index, std::vector< float > &distance)
Search for the closest nearest neighbor of a given point.
Definition gicp.h:452
pcl::GeneralizedIterativeClosestPoint::Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition gicp.h:111
pcl::GeneralizedIterativeClosestPoint::tmp_idx_src_
const pcl::Indices * tmp_idx_src_
Temporary pointer to the source dataset indices.
Definition gicp.h:386
pcl::IterativeClosestPoint
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition icp.h:98
pcl::IterativeClosestPoint::Matrix4
typename Registration< PointSource, PointTarget, Scalar >::Matrix4 Matrix4
Definition icp.h:143
pcl::IterativeClosestPoint::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 icp.h:232
pcl::IterativeClosestPoint::setInputSource
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition icp.h:199
pcl::PCLBase< PointSource >::input_
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
pcl::PCLBase< PointSource >::indices_
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
pcl::PCLException
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition exceptions.h:64
pcl::PointCloud< PointSource >
pcl::PointCloud::size
std::size_t size() const
Definition point_cloud.h:443
pcl::PointCloud< PointSource >::Ptr
shared_ptr< PointCloud< PointSource > > Ptr
Definition point_cloud.h:413
pcl::PointCloud< PointSource >::ConstPtr
shared_ptr< const PointCloud< PointSource > > ConstPtr
Definition point_cloud.h:414
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::tree_reciprocal_
KdTreeReciprocalPtr tree_reciprocal_
A pointer to the spatial search object of the source.
Definition registration.h:554
pcl::Registration::KdTreePtr
typename KdTree::Ptr KdTreePtr
Definition registration.h:71
pcl::Registration::nr_iterations_
int nr_iterations_
The number of iterations the internal optimization ran for (used internally).
Definition registration.h:558
pcl::Registration::tree_
KdTreePtr tree_
A pointer to the spatial search object.
Definition registration.h:551
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::max_iterations_
int max_iterations_
The maximum number of iterations the internal optimization should run for.
Definition registration.h:563
pcl::Registration::min_number_correspondences_
unsigned int min_number_correspondences_
The minimum number of correspondences that the algorithm needs before attempting to estimate the tran...
Definition registration.h:618
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::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
pcl::search::KdTree::Ptr
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition kdtree.h:75
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
BFGSSpace::Status
Status
Definition bfgs.h:70
pcl
Definition convolution.h:46
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
BFGSDummyFunctor
Definition bfgs.h:80
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices
optimization functor structure
Definition gicp.h:467
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::dfddf
void dfddf(const Vector6d &x, Vector6d &df, Matrix6d &ddf)
Definition gicp.hpp:578
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::OptimizationFunctorWithIndices
OptimizationFunctorWithIndices(const GeneralizedIterativeClosestPoint *gicp)
Definition gicp.h:468
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::operator()
double operator()(const Vector6d &x) override
Definition gicp.hpp:460
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::df
void df(const Vector6d &x, Vector6d &df) override
Definition gicp.hpp:491
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::gicp_
const GeneralizedIterativeClosestPoint * gicp_
Definition gicp.h:482
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::checkGradient
BFGSSpace::Status checkGradient(const Vector6d &g) override
Definition gicp.hpp:724
pcl::GeneralizedIterativeClosestPoint::OptimizationFunctorWithIndices::fdf
void fdf(const Vector6d &x, double &f, Vector6d &df) override
Definition gicp.hpp:534
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