PCL点云库的传统ICP代码分析
ICP算法的介绍
ICP(Iterative Closest Point),即最近点迭代算法,是最为经典的数据配准算法。其特征在于,通过求取源点云和目标点云之间的对应点对,基于对应点对构造旋转平移矩阵,并利用所求矩阵,将源点云变换到目标点云的座标系下,估计变换后源点云与目标点云的误差函数,若误差函数值大于阀值,则迭代进行上述运算直到满足给定的误差要求.
ICP算法采用最小二乘估计计算变换矩阵,原理简单且具有较好的精度,但是由于采用了迭代计算,导致算法计算速度较慢,而且采用ICP进行配准计算时,其对待配准点云的初始位置有一定要求,若所选初始位置不合理,则会导致算法陷入局部最优。。
PCL里面的源码分析
我接下来对pcl里面的源码了解了下,大体有些地方做了备注,但是未必万全正确。
- 首先要介绍的是主体的ICP启动程序:
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the CloudIn data
cloud_in->width = 5;
cloud_in->height = 1;
cloud_in->is_dense = false;
cloud_in->points.resize (cloud_in->width * cloud_in->height);
for (size_t i = 0; i < cloud_in->points.size (); ++i)
{
cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
}
std::cout << "Saved " << cloud_in->points.size () << " data points to input:"
<< std::endl;
for (size_t i = 0; i < cloud_in->points.size (); ++i) std::cout << " " <<
cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
cloud_in->points[i].z << std::endl;
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->points.size() << std::endl;
for (size_t i = 0; i < cloud_in->points.size (); ++i)
cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
std::cout << "Transformed " << cloud_in->points.size () << " data points:"
<< std::endl;
for (size_t i = 0; i < cloud_out->points.size (); ++i)
std::cout << " " << cloud_out->points[i].x << " " <<
cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
return (0);
}
其中主要的功能就是在align()这个函数中实现的。这个函数的大体位置是
registration/include/pcl/registraion/impl/registration.hpp这里。代码接下如下
- align()函数
//函数里调用这个真正的函数
template <typename PointSource, typename PointTarget, typename Scalar> inline void
pcl::Registration<PointSource, PointTarget, Scalar>::align (PointCloudSource &output, const Matrix4& guess)
{
//分配input点云的下标,函数在common/include/pcl/impl/pcl_base.hpp
if (!initCompute ())
return;
// Resize the output dataset
//如果output的下标数量和input不一样,那就成为一样的
if (output.points.size () != indices_->size ())
output.points.resize (indices_->size ());
// Copy the header
output.header = input_->header;
// Check if the output will be computed for all points or only a subset
//这里没搞懂,感觉肯定是相等的呀?
if (indices_->size () != input_->points.size ())
{
output.width = static_cast<uint32_t> (indices_->size ());
output.height = 1;
}
else
{
output.width = static_cast<uint32_t> (input_->width);
output.height = input_->height;
}
output.is_dense = input_->is_dense;
// Copy the point data to output
//这里的output就是final,也就是最后由input转化过来的点云,不是匹配的目标点云
//因为没有被初试化的,所以直接拷贝点云
for (size_t i = 0; i < indices_->size (); ++i)
output.points[i] = input_->points[(*indices_)[i]];
// Set the internal point representation of choice unless otherwise noted
if (point_representation_ && !force_no_recompute_)
tree_->setPointRepresentation (point_representation_);
// Perform the actual transformation computation
converged_ = false;
final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
// Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid
// transformation
//其实因为座标是齐次座标,所以第四个元素是1,前面三个元素是x,y,z
for (size_t i = 0; i < indices_->size (); ++i)
output.points[i].data[3] = 1.0;
//实现的icp.hpp里面,这个函数是重载函数,所以要找对
//变种icp的更改基本都在这里,改动h,hpp文件,以及改动主要的computeTransformation函数,前面的都是预备工作,关系不大
computeTransformation (output, guess);
//这个函数仅仅是返回一个布尔值true
deinitCompute ();
}
在这个align里面的最重要的函数就是computeTransformation (output, guess)。而这个函数就在registration/include/pcl/registraion/icp.hpp这里。
- computeTransformation()函数
#ifndef PCL_REGISTRATION_IMPL_ICP_HPP_
#define PCL_REGISTRATION_IMPL_ICP_HPP_
#include <pcl/registration/boost.h>
#include <pcl/correspondence.h>
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::transformCloud (
const PointCloudSource &input,
PointCloudSource &output,
const Matrix4 &transform)
{
//这里的input和output在第一次的时候还是相同的值
//但是在第二次迭代的时候才是正常的步骤
Eigen::Vector4f pt (0.0f, 0.0f, 0.0f, 1.0f), pt_t;
Eigen::Matrix4f tr = transform.template cast<float> ();
// XYZ is ALWAYS present due to the templatization, so we only have to check for normals
if (source_has_normals_)
{
Eigen::Vector3f nt, nt_t;
Eigen::Matrix3f rot = tr.block<3, 3> (0, 0);
for (size_t i = 0; i < input.size (); ++i)
{
将input的数据填充到pt里
const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);
uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);
memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));
if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2]))
continue;
//这里就是转换的公式,是齐次的转换
pt_t = tr * pt;
//把pt_t的值给data_out
memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
memcpy (&nt[0], data_in + nx_idx_offset_, sizeof (float));
memcpy (&nt[1], data_in + ny_idx_offset_, sizeof (float));
memcpy (&nt[2], data_in + nz_idx_offset_, sizeof (float));
if (!pcl_isfinite (nt[0]) || !pcl_isfinite (nt[1]) || !pcl_isfinite (nt[2]))
continue;
//这里是非齐次的转换
nt_t = rot * nt;
//把转换后的nt_t给data_out
memcpy (data_out + nx_idx_offset_, &nt_t[0], sizeof (float));
memcpy (data_out + ny_idx_offset_, &nt_t[1], sizeof (float));
memcpy (data_out + nz_idx_offset_, &nt_t[2], sizeof (float));
}
}
else
{
for (size_t i = 0; i < input.size (); ++i)
{
const uint8_t* data_in = reinterpret_cast<const uint8_t*> (&input[i]);
uint8_t* data_out = reinterpret_cast<uint8_t*> (&output[i]);
memcpy (&pt[0], data_in + x_idx_offset_, sizeof (float));
memcpy (&pt[1], data_in + y_idx_offset_, sizeof (float));
memcpy (&pt[2], data_in + z_idx_offset_, sizeof (float));
if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2]))
continue;
//这里是齐次的转换
pt_t = tr * pt;
memcpy (data_out + x_idx_offset_, &pt_t[0], sizeof (float));
memcpy (data_out + y_idx_offset_, &pt_t[1], sizeof (float));
memcpy (data_out + z_idx_offset_, &pt_t[2], sizeof (float));
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation (
PointCloudSource &output, const Matrix4 &guess)
{
// Point cloud containing the correspondences of each point in <input, indices>
//input_transformed是input被转换一次之后的点云
PointCloudSourcePtr input_transformed (new PointCloudSource);
nr_iterations_ = 0;
converged_ = false;
// Initialise final transformation to the guessed one
//都变成单位矩阵
final_transformation_ = guess;
// If the guessed transformation is non identity
if (guess != Matrix4::Identity ())
{
input_transformed->resize (input_->size ());
// Apply guessed transformation prior to search for neighbours
//在icp.hpp里48行
transformCloud (*input_, *input_transformed, guess);
}
else
//否则就是直接复制,其实这里还没有开始转换,因为input_transformed还是原来的input
*input_transformed = *input_;
transformation_ = Matrix4::Identity ();
// Make blobs if necessary
//我也不知道这个步骤的含义,要制造异常点吗?
determineRequiredBlobData ();
PCLPointCloud2::Ptr target_blob (new PCLPointCloud2);
if (need_target_blob_)
//转换成二进制的点云
pcl::toPCLPointCloud2 (*target_, *target_blob);
// Pass in the default target for the Correspondence Estimation/Rejection code
correspondence_estimation_->setInputTarget (target_);
if (correspondence_estimation_->requiresTargetNormals ())
correspondence_estimation_->setTargetNormals (target_blob);
// Correspondence Rejectors need a binary blob
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
if (rej->requiresTargetPoints ())
rej->setTargetPoints (target_blob);
if (rej->requiresTargetNormals () && target_has_normals_)
rej->setTargetNormals (target_blob);
}
//MSE是均方误差,这里是设置迭代的相关参数
convergence_criteria_->setMaximumIterations (max_iterations_);
convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
if (transformation_rotation_epsilon_ > 0)
convergence_criteria_->setRotationThreshold (transformation_rotation_epsilon_);
else
convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
// Repeat until convergence
//该方法的主体是一个do-while循环,查找最近点,剔除错误的对应点,收敛原则都在这里
//correspondence_estimation_ 、correspondence_rejectors_ 和 convergence_criteria_
//这三个变量的作用分别代表查找最近点,剔除错误的对应点,收敛准则
//因为是do-while循环,因此收敛准则的作用是跳出循环
//transformation_estimation_是求解ICP的具体算法
do
{
// Get blob data if needed
PCLPointCloud2::Ptr input_transformed_blob;
if (need_source_blob_)
{
input_transformed_blob.reset (new PCLPointCloud2);
toPCLPointCloud2 (*input_transformed, *input_transformed_blob);
}
// Save the previously estimated transformation
//第一步迭代之前,到这个步骤之前一直是单位矩阵
previous_transformation_ = transformation_;
// Set the source each iteration, to ensure the dirty flag is updated
correspondence_estimation_->setInputSource (input_transformed);
if (correspondence_estimation_->requiresSourceNormals ())
correspondence_estimation_->setSourceNormals (input_transformed_blob);
// Estimate correspondences
//寻找迭代点云的对应点
//use_reciprocal_correspondence_是相反的对应关系
if (use_reciprocal_correspondence_)
//determineReciprocalCorrespondences()在correspondence_estimation.hpp文件里,corr_dist_threshold_是最大距离
correspondence_estimation_->determineReciprocalCorrespondences (*correspondences_, corr_dist_threshold_);
else
correspondence_estimation_->determineCorrespondences (*correspondences_, corr_dist_threshold_);
//if (correspondence_rejectors_.empty ())
//把已经有对应关系的correspondences_初始化temp_correspondences,当然这是一个动态的暂时内存
CorrespondencesPtr temp_correspondences (new Correspondences (*correspondences_));
for (size_t i = 0; i < correspondence_rejectors_.size (); ++i)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
PCL_DEBUG ("Applying a correspondence rejector method: %s.\n", rej->getClassName ().c_str ());
if (rej->requiresSourcePoints ())
rej->setSourcePoints (input_transformed_blob);
if (rej->requiresSourceNormals () && source_has_normals_)
rej->setSourceNormals (input_transformed_blob);
rej->setInputCorrespondences (temp_correspondences);
rej->getCorrespondences (*correspondences_);
// Modify input for the next iteration
if (i < correspondence_rejectors_.size () - 1)
*temp_correspondences = *correspondences_;
}
size_t cnt = correspondences_->size ();
// Check whether we have enough correspondences
if (static_cast<int> (cnt) < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
convergence_criteria_->setConvergenceState(pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NO_CORRESPONDENCES);
converged_ = false;
break;
}
//在前面的寻找一致性估计后(寻找对应点后),接下来的步骤又是主要的函数步骤,transformation_estimation_是求解ICP的具体算法
// Estimate the transform
//查看transformation_estimation_svd.hpp中的TransformationEstimationSVD类的estimateRigidTransformation 方法
//这里就是target_是最终的目标点云,在迭代过程中不变,但是input_transformed总是会不停的更新,直到和目标重合
transformation_estimation_->estimateRigidTransformation (*input_transformed, *target_, *correspondences_, transformation_);
// Tranform the data
transformCloud (*input_transformed, *input_transformed, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
++nr_iterations_;
// Update the vizualization of icp convergence
//if (update_visualizer_ != 0)
// update_visualizer_(output, source_indices_good, *target_, target_indices_good );
converged_ = static_cast<bool> ((*convergence_criteria_));
}
while (!converged_);
// Transform the input cloud using the final transformation
PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",
final_transformation_ (0, 0), final_transformation_ (0, 1), final_transformation_ (0, 2), final_transformation_ (0, 3),
final_transformation_ (1, 0), final_transformation_ (1, 1), final_transformation_ (1, 2), final_transformation_ (1, 3),
final_transformation_ (2, 0), final_transformation_ (2, 1), final_transformation_ (2, 2), final_transformation_ (2, 3),
final_transformation_ (3, 0), final_transformation_ (3, 1), final_transformation_ (3, 2), final_transformation_ (3, 3));
// Copy all the values
output = *input_;
// Transform the XYZ + normals
//先把input_复制过去,然后在将转换后的点云叠加上去,至此,算法完成
transformCloud (*input_, output, final_transformation_);
}
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::determineRequiredBlobData ()
{
need_source_blob_ = false;
need_target_blob_ = false;
// Check estimator
//仅仅是检查而已,requiresSourceNormals函数返回true
// |=是复合的逻辑或
need_source_blob_ |= correspondence_estimation_->requiresSourceNormals ();
need_target_blob_ |= correspondence_estimation_->requiresTargetNormals ();
// Add warnings if necessary
if (correspondence_estimation_->requiresSourceNormals () && !source_has_normals_)
{
PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects source normals, but we can't provide them.\n", getClassName ().c_str ());
}
if (correspondence_estimation_->requiresTargetNormals () && !target_has_normals_)
{
PCL_WARN("[pcl::%s::determineRequiredBlobData] Estimator expects target normals, but we can't provide them.\n", getClassName ().c_str ());
}
// Check rejectors
for (size_t i = 0; i < correspondence_rejectors_.size (); i++)
{
registration::CorrespondenceRejector::Ptr& rej = correspondence_rejectors_[i];
need_source_blob_ |= rej->requiresSourcePoints ();
need_source_blob_ |= rej->requiresSourceNormals ();
need_target_blob_ |= rej->requiresTargetPoints ();
need_target_blob_ |= rej->requiresTargetNormals ();
if (rej->requiresSourceNormals () && !source_has_normals_)
{
PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects source normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
}
if (rej->requiresTargetNormals () && !target_has_normals_)
{
PCL_WARN("[pcl::%s::determineRequiredBlobData] Rejector %s expects target normals, but we can't provide them.\n", getClassName ().c_str (), rej->getClassName ().c_str ());
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::IterativeClosestPointWithNormals<PointSource, PointTarget, Scalar>::transformCloud (
const PointCloudSource &input,
PointCloudSource &output,
const Matrix4 &transform)
{
pcl::transformPointCloudWithNormals (input, output, transform);
}
#endif /* PCL_REGISTRATION_IMPL_ICP_HPP_ */
当初的代码注释,但是也是依照个人理解初次写,如果有问题,还请见谅!