前言
icp算法是点云匹配的常用方法之一，这里将pcl源码关于icp部分做一下解析，以加深对原理和代码的理解。
一、应用实例
icp匹配应用实例在之前已经写了。
二、源码分析
icp算法在IterativeClosestPoint类中实现，而该类继承了Registration类
template <typename PointSource, typename PointTarget, typename Scalar = float>
class IterativeClosestPoint : public Registration<PointSource, PointTarget, Scalar>
{
...
}
在Registration类中，核心算法在align函数中实现
inline void
align (PointCloudSource &output, const Matrix4& guess);
1、align 函数
align 函数是实现主体，需要详细展开
Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output)
{
align(output, Matrix4::Identity());
}
template <typename PointSource, typename PointTarget, typename Scalar>
inline void
Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output,
const Matrix4& guess)
{
//初始化
if (!initCompute())
return;
// Resize the output dataset
//输出数据大小要和id大小一致，因为有可能输入的数据（和id）可能是一个子集，并非全部点云
output.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_->size()) {
output.width = indices_->size();
output.height = 1;
}
else {
output.width = static_cast<std::uint32_t>(input_->width);
output.height = input_->height;
}
//is_dense：bool 类型，若点云中的数据都是有限的（不包含 inf/NaN 这样的值），则为 true，否则为 false。
output.is_dense = input_->is_dense;
// Copy the point data to output
//将indices_里的id对应的数据拷贝给output
for (std::size_t i = 0; i < indices_->size(); ++i)
output[i] = (*input_)[(*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
//引入齐次坐标，方便后面变换矩阵和向量（点云）相乘
for (std::size_t i = 0; i < indices_->size(); ++i)
output[i].data[3] = 1.0;
//开始运算icp，输出转换后的点云、相对位姿（点云转换前到转换后的变换矩阵）
computeTransformation(output, guess);
deinitCompute();
}
2、initCompute函数
也即初始化函数
template <typename PointSource, typename PointTarget, typename Scalar>
bool
Registration<PointSource, PointTarget, Scalar>::initCompute()
{
//判断输入的目标点云是否为空
if (!target_) {
PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",
getClassName().c_str());
return (false);
}
// Only update target kd-tree if a new target cloud was set
//目标点云构建kdtree，且只在初始化时构建一次
if (target_cloud_updated_ && !force_no_recompute_) {
tree_->setInputCloud(target_);
target_cloud_updated_ = false;
}
// Update the correspondence estimation
//实际上就是更新两个bool值，目标点云获取成功且已经构建了kdtree，后面不要再做这个操作
if (correspondence_estimation_) {
correspondence_estimation_->setSearchMethodTarget(tree_, force_no_recompute_);
correspondence_estimation_->setSearchMethodSource(tree_reciprocal_,
force_no_recompute_reciprocal_);
}
// Note: we /cannot/ update the search method on all correspondence rejectors, because
// we know nothing about them. If they should be cached, they must be cached
// individually.
//这里用到了PCLBase里的初始化，继承是个好东西~~
return (PCLBase<PointSource>::initCompute());
}
3、computeTransformation函数
这里是icp算法的实现部分
template <typename PointSource, typename PointTarget, typename Scalar>
void
IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformation(
PointCloudSource& output, const Matrix4& guess)
{
// Point cloud containing the correspondences of each point in <input, indices>
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
//guess 单位化，这个位置如果没有单位化，直接进行单位化就好了，为什么搞这么复杂~~
if (guess != Matrix4::Identity()) {
input_transformed->resize(input_->size());
// Apply guessed transformation prior to search for neighbours
transformCloud(*input_, *input_transformed, guess);
}
else
*input_transformed = *input_;
transformation_ = Matrix4::Identity();
// Make blobs if necessary
//点云法向量参与运算时，将格式转换成PCLPointCloud2格式
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
//输入target点云，为后续迭代做准备，目标点云只在循环前做一次，这里得correspondence_estimation_以及
//correspondence_rejectors_怎么操作的，后续再写
correspondence_estimation_->setInputTarget(target_);
if (correspondence_estimation_->requiresTargetNormals())
correspondence_estimation_->setTargetNormals(target_blob);
// Correspondence Rejectors need a binary blob
for (std::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);
}
//为icp迭代器设置参数
//最大迭代次数
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_);
// Repeat until convergence
//开始循环，直至收敛
do {
// Get blob data if needed
//如果需要法向量参与运算，当前帧点云要转换成PCLPointCloud2格式
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
//出入source点云，每迭代一次，都必须做一次处理，和target不一样
correspondence_estimation_->setInputSource(input_transformed);
if (correspondence_estimation_->requiresSourceNormals())
correspondence_estimation_->setSourceNormals(input_transformed_blob);
// Estimate correspondences
if (use_reciprocal_correspondence_)
correspondence_estimation_->determineReciprocalCorrespondences(
*correspondences_, corr_dist_threshold_);
else
correspondence_estimation_->determineCorrespondences(*correspondences_,
corr_dist_threshold_);
// if (correspondence_rejectors_.empty ())
CorrespondencesPtr temp_correspondences(new Correspondences(*correspondences_));
for (std::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);
//这里获取getCorrespondences函数原理，后续再写，correspondences_这个参数也
//是target与source之间得关联，和各自得id相关
rej->getCorrespondences(*correspondences_);
// Modify input for the next iteration
if (i < correspondence_rejectors_.size() - 1)
*temp_correspondences = *correspondences_;
}
// Check whether we have enough correspondences
if (correspondences_->size() < 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;
}
// Estimate the transform
//获取当前迭代次数次下，target和source得相对位姿（变换矩阵），这也是icp算法得核心部分，后续再做详细分析
transformation_estimation_->estimateRigidTransformation(
*input_transformed, *target_, *correspondences_, transformation_);
// Transform the data
//将点云乘以变换矩阵，得到转换位姿后得点云姿态
transformCloud(*input_transformed, *input_transformed, transformation_);
// Obtain the final transformation
//将这次迭代获取的变换矩阵乘以上一次得变换矩阵，方便输出最终得变换矩阵
final_transformation_ = transformation_ * final_transformation_;
++nr_iterations_;
// Update the visualization of icp convergence
//上传收敛系数
if (update_visualizer_ != nullptr) {
pcl::Indices source_indices_good, target_indices_good;
for (const Correspondence& corr : *correspondences_) {
source_indices_good.emplace_back(corr.index_query);
target_indices_good.emplace_back(corr.index_match);
}
update_visualizer_(
*input_transformed, source_indices_good, *target_, target_indices_good);
}
converged_ = static_cast<bool>((*convergence_criteria_));
} while (convergence_criteria_->getConvergenceState() ==
pcl::registration::DefaultConvergenceCriteria<
Scalar>::CONVERGENCE_CRITERIA_NOT_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
//将source乘以最终变换矩阵，得到icp匹配后得source
transformCloud(*input_, output, final_transformation_);
}
总结
总结了一下pcl icp匹配得源码，本来打算一次性写完的，发现原理以及代码还是比较复杂的，需要分多次写完，不过收获还是颇多，未完待续~~
扫一扫
794
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
