preface
icp.h has announced two categories: iterative losestpoint and iterative losestpointwithnormals, both of which are subcategories of Registration.
This article only focuses on the IterativeClosestPoint category.
using
template <typename PointSource, typename PointTarget, typename Scalar = float>
class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar> {
public:
Use to define the name for subsequent use.
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;
using Ptr = shared_ptr<IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
using ConstPtr =
shared_ptr<const IterativeClosestPoint<PointSource, PointTarget, Scalar>>;
Follow the definition in Registration, and import the protected member of Registration into the public permission block of IterativeClosestPoint.
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>::min_number_correspondences_;
using Registration<PointSource, PointTarget, Scalar>::update_visualizer_;
using Registration<PointSource, PointTarget, Scalar>::euclidean_fitness_epsilon_;
using Registration<PointSource, PointTarget, Scalar>::correspondences_;
using Registration<PointSource, PointTarget, Scalar>::transformation_estimation_;
using Registration<PointSource, PointTarget, Scalar>::correspondence_estimation_;
using Registration<PointSource, PointTarget, Scalar>::correspondence_rejectors_;
Define a public member variable conversion_ criteria_， Indicates the convergence of the current algorithm:
typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
convergence_criteria_;
Follow the category name defined in Registration Matrix4:
// Why not write using registration < pointsource, pointtarget, scalar >:: matrix4;? using Matrix4 = typename Registration<PointSource, PointTarget, Scalar>::Matrix4;
public member function
constructor and destructor
/** \brief Empty constructor. */
IterativeClosestPoint()
: x_idx_offset_(0)
, y_idx_offset_(0)
, z_idx_offset_(0)
, nx_idx_offset_(0)
, ny_idx_offset_(0)
, nz_idx_offset_(0)
, use_reciprocal_correspondence_(false)
, source_has_normals_(false)
, target_has_normals_(false)
{
reg_name_ = "IterativeClosestPoint";
transformation_estimation_.reset(
new pcl::registration::
TransformationEstimationSVD<PointSource, PointTarget, Scalar>());
correspondence_estimation_.reset(
new pcl::registration::
CorrespondenceEstimation<PointSource, PointTarget, Scalar>);
convergence_criteria_.reset(
new pcl::registration::DefaultConvergenceCriteria<Scalar>(
nr_iterations_, transformation_, *correspondences_));
};
transformation_estimation_， correspondence_estimation_ And convergence_criteria_ All shared_ptr, call std::shared_ptr::reset to replace the objects they manage.
Note: shared_ptr related knowledge can be obtained through C++ std::shared_ptr usage and examples Learn
copy constructor
Currently, the copy constructor of IterativeClosestPoint is disabled:
/** * \brief Due to `convergence_criteria_` holding references to the class members, * it is tricky to correctly implement its copy and move operations correctly. This * can result in subtle bugs and to prevent them, these operations for ICP have * been disabled. * * \todo: remove deleted ctors and assignments operations after resolving the issue */ // Copying and moving iterative losestpoint will cause problems, so it is prohibited to use it at present IterativeClosestPoint(const IterativeClosestPoint&) = delete; IterativeClosestPoint(IterativeClosestPoint&&) = delete; IterativeClosestPoint& operator=(const IterativeClosestPoint&) = delete; IterativeClosestPoint& operator=(IterativeClosestPoint&&) = delete;
destructor
/** \brief Empty destructor */
~IterativeClosestPoint() {}
getConvergeCriteria
public member variable conversion_ criteria_ getter for:
//Notes?
/** \brief Returns a pointer to the DefaultConvergenceCriteria used by the
* IterativeClosestPoint class. This allows to check the convergence state after the
* align() method as well as to configure DefaultConvergenceCriteria's parameters not
* available through the ICP API before the align() method is called. Please note that
* the align method sets max_iterations_, euclidean_fitness_epsilon_ and
* transformation_epsilon_ and therefore overrides the default / set values of the
* DefaultConvergenceCriteria instance. \return Pointer to the IterativeClosestPoint's
* DefaultConvergenceCriteria.
*/
inline typename pcl::registration::DefaultConvergenceCriteria<Scalar>::Ptr
getConvergeCriteria()
{
return convergence_criteria_;
}
setInputSource
Set the source point cloud itself and the XYZ coordinate offset x_idx_offset_, y_idx_offset_,z_idx_offset_， Normal vector XYZ coordinate offset nx_idx_offset_, ny_idx_offset_, nz_idx_offset_ And source_has_normals_. These variables are used in the transformCloud function.
/**
* Set the source point cloud and x_idx_offset_,y_idx_offset_,z_idx_offset_,
* nx_idx_offset_,ny_idx_offset_,nz_idx_offset_And source_has_normals_
**/
// This function is virtual in registration.h
/** \brief Provide a pointer to the input source
* (e.g., the point cloud that we want to align to the target)
*
* \param[in] cloud the input point cloud source
*/
void
setInputSource(const PointCloudSourceConstPtr& cloud) override
{
Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
const auto fields = pcl::getFields<PointSource>();
source_has_normals_ = false;
for (const auto& field : fields) {
if (field.name == "x")
x_idx_offset_ = field.offset;
else if (field.name == "y")
y_idx_offset_ = field.offset;
else if (field.name == "z")
z_idx_offset_ = field.offset;
else if (field.name == "normal_x") {
source_has_normals_ = true;
nx_idx_offset_ = field.offset;
}
else if (field.name == "normal_y") {
source_has_normals_ = true;
ny_idx_offset_ = field.offset;
}
else if (field.name == "normal_z") {
source_has_normals_ = true;
nz_idx_offset_ = field.offset;
}
}
}
setInputTarget
Set the target point cloud itself and target_has_normals_. target_has_normals_ It will be used in the transformCloud function.
//Set the target point cloud and target_has_normals_
//NO x_idx_offset_,nx_idx_offset_ Or something?
// This function is virtual in registration.h
/** \brief Provide a pointer to the input target
* (e.g., the point cloud that we want to align to the target)
*
* \param[in] cloud the input point cloud target
*/
void
setInputTarget(const PointCloudTargetConstPtr& cloud) override
{
Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
const auto fields = pcl::getFields<PointSource>();
target_has_normals_ = false;
for (const auto& field : fields) {
if (field.name == "normal_x" || field.name == "normal_y" ||
field.name == "normal_z") {
target_has_normals_ = true;
break;
}
}
}
use_reciprocal_correspondence_ Setters and getter s for
/** \brief Set whether to use reciprocal correspondence or not
*
* \param[in] use_reciprocal_correspondence whether to use reciprocal correspondence
* or not
*/
inline void
setUseReciprocalCorrespondences(bool use_reciprocal_correspondence)
{
use_reciprocal_correspondence_ = use_reciprocal_correspondence;
}
/** \brief Obtain whether reciprocal correspondence are used or not */
inline bool
getUseReciprocalCorrespondences() const
{
return (use_reciprocal_correspondence_);
}
protected member function
The transformCloud function is used to perform rigid body transformation on the XYZ coordinates and normal vectors of the input point cloud:
//Perform rigid body transformation on the XYZ coordinates and normal vectors of the input point cloud
/** \brief Apply a rigid transform to a given dataset. Here we check whether
* the dataset has surface normals in addition to XYZ, and rotate normals as well.
* \param[in] input the input point cloud
* \param[out] output the resultant output point cloud
* \param[in] transform a 4x4 rigid transformation
* \note Can be used with cloud_in equal to cloud_out
*/
virtual void
transformCloud(const PointCloudSource& input,
PointCloudSource& output,
const Matrix4& transform);
The computeTransformation function is the core of the ICP algorithm:
//Calculate the conversion from input to output point cloud, and then set output as the converted point cloud /** \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;
determineRequiredBlobData is an auxiliary function used to determine whether the input pcl::PointCloud needs to be converted to pcl::PCLPointCloud2:
//? /** \brief Looks at the Estimators and Rejectors and determines whether their * blob-setter methods need to be called */ virtual void determineRequiredBlobData();
protected member variable
In the correction algorithm, the input and output point cloud types may be different, and the underlying data structure may be different. Therefore, when actually processing the data inside the point cloud (such as the transformCloud function), it is necessary to know where the XYZ coordinates are located at a point respectively. In addition, if the point contains a normal vector, you also need to know the offset of the XYZ coordinate of the point normal vector relative to the starting position of the point.
The following six member variables are offset s used to represent XYZ and the normal vector XYZ. They will be initialized in the setInputSource function:
//Belongs to source point cloud /** \brief XYZ fields offset. */ std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_; //Belongs to source point cloud /** \brief Normal fields offset. */ std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_;
The source point cloud and the target point cloud may or may not have a normal vector. These two member variables record whether the two point cloud has a normal vector. It mainly plays a role in the transformCloud function. Where source_has_normals_ Initialized in setInputSource function, target_has_normals_ Initialized in setInputTarget function:
/** \brief Internal check whether source dataset has normals or not. */ bool source_has_normals_; /** \brief Internal check whether target dataset has normals or not. */ bool target_has_normals_;
The point cloud input by the correction algorithm is of pcl::PointCloud type, but the pcl::registration::CorrespondenceEstimationBase::setSourceNormals, PCL:: Registration:: correspondenceejector:: setsourcepoints and PCL:: Registration:: correspondenceejector:: setsourcenormals functions used internally (the corresponding target version is pcl::registration::CorrespondenceEstimationBase::setTargetNormals, PCL:: Registration:: correspondenceejector:: settargetpoints and PCL:: Registration:: correspondenceejector:: settargetnormals functions) it accepts point clouds of type PCL:: pclpointcloud 2.
In the determineRequiredBlobData member function, it will be determined whether the above functions are needed. If necessary, need_source_blob_or / and need_target_blob_will be set to true. Then, in the computeTransformation function, it will be determined whether to execute pcl::toPCLPointCloud2 according to these two flag s.
//? /** \brief Checks for whether estimators and rejectors need various data */ bool need_source_blob_, need_target_blob_;
The first step of ICP algorithm is to find point pairs. According to the judgment criteria, it can be divided into the following two types:
- Point B in the target point cloud is the nearest neighbor of point A in the original point cloud in the target point cloud
- Point B in the target point cloud is the nearest neighbor of point A in the original point cloud in the target point cloud, and A is also the nearest neighbor of B in the original point cloud
You can see that the second standard is relatively strict. This flag is used to select which of the above standards should be adopted:
/** \brief The correspondence type used for correspondence estimation. */ bool use_reciprocal_correspondence_;