PCL 实现 SAC_IA 算法原理源码解析

采样一致性算法（SAC_IA）用于点云粗配准中，配准效果还不错，PCL 中也实现了该算法，本文深入 PCL 源码，学习一下 SAC_IA 算法在 PCL 中是如何实现的。

主要函数

SAC_IA 算法实现在头文件 ia_ransac.hpp 中，computeTransformation 实现了旋转矩阵的计算并进行迭代，内部调用了其他的一些功能函数。下面看一下各个函数是如何实现的。

computeTransformation

computeTransformation函数内部实现了迭代过程，并最终求取了迭代过程中误差最小的旋转矩阵。

参数：

output：输出旋转后的点云
guess：初始旋转矩阵（猜想矩阵），默认为单位阵

具体流程解析看注释：

template <typename PointSource, typename PointTarget, typename FeatureT> void
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
{// nr_samples_ 为用户设定的采样点数量，其值 >= 3// 定义 随机采样点索引 的容器std::vector<int> sample_indices (nr_samples_);// 定义 随机采样点对应点索引 的容器std::vector<int> corresponding_indices (nr_samples_);PointCloudSource input_transformed;   // 定义每次旋转的点云，主要作为临时点云float error, lowest_error (0);      // 定义误差final_transformation_ = guess;  // 将 guess 赋值给 final_transformation_（最小误差对应的旋转矩阵）int i_iter = 0;     // 定义迭代次数converged_ = false;if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f)) // 如果 guess 不是单位矩阵{// 将初始点云 input_ 根据 guess 做一次旋转，储存至临时点云 input_transformedtransformPointCloud (*input_, input_transformed, final_transformation_);// 计算旋转后的误差lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));i_iter = 1;  // 迭代次数加 1}// 开始迭代for (; i_iter < max_iterations_; ++i_iter){// 选择随机点，将随机选出的点的索引存储到 sample_indices 中selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);// 在目标云中找到相应的特征，将选择出的对应点的索引存储到 corresponding_indices 中findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);// 估计从样本到其对应点的变换，计算旋转矩阵 transformation_transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);// 转换点云数据并计算误差transformPointCloud (*input_, input_transformed, transformation_);error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));// 如果新误差更小，则更新 lowest_error 和 final_transformation_if (i_iter == 0 || error < lowest_error){lowest_error = error;final_transformation_ = transformation_;converged_=true;}}// 使用 final_transformation_ 对点云进行旋转transformPointCloud (*input_, output, final_transformation_);
}

selectSamples

该函数用于选择随机采样点。

template <typename PointSource, typename PointTarget, typename FeatureT> void
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSamples (const PointCloudSource &cloud, int nr_samples, float min_sample_distance, std::vector<int> &sample_indices)
{// 如果采样点数量大于点云总数，错误返回if (nr_samples > static_cast<int> (cloud.points.size ())){PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%lu)!\n", nr_samples, cloud.points.size ());return;}// 反复抽取随机样本，直到达到 nr_samples 个// 定义找到一个有效点的查找次数int iterations_without_a_sample = 0;  // 定义找到一个有效点的查找次数的最大值int max_iterations_without_a_sample = static_cast<int> (3 * cloud.points.size ());sample_indices.clear ();while (static_cast<int> (sample_indices.size ()) < nr_samples){// 随机选择一个点int sample_index = getRandomIndex (static_cast<int> (cloud.points.size ()));// 检查该点是否唯一并且与其他点有一定的距离bool valid_sample = true;for (size_t i = 0; i < sample_indices.size (); ++i){float distance_between_samples = euclideanDistance (cloud.points[sample_index], cloud.points[sample_indices[i]]);if (sample_index == sample_indices[i] || distance_between_samples < min_sample_distance){valid_sample = false;break;}}// 如果该点为有效点，那么就加入 sample_indicesif (valid_sample){sample_indices.push_back (sample_index);iterations_without_a_sample = 0;}else++iterations_without_a_sample;     // 该点不是有效点，查找次数加 1// 如果找不到有效样本，则放宽样本间距离要求// 一个有效点的查找次数大于等于查找次数的最大值if (iterations_without_a_sample >= max_iterations_without_a_sample){PCL_WARN ("[pcl::%s::selectSamples] ", getClassName ().c_str ());PCL_WARN ("No valid sample found after %d iterations. Relaxing min_sample_distance_ to %f\n", iterations_without_a_sample, 0.5*min_sample_distance);min_sample_distance_ *= 0.5f;min_sample_distance = min_sample_distance_;iterations_without_a_sample = 0;}}
}

findSimilarFeatures

该函数用于查询相似特征。

input_features：输入为含有特征的点云
sample_indices：：待查询近邻特征的索引
corresponding_indices：保存查找到的对应点的索引

注意：

查找的方式为 kdtree
查找 K 个近邻特征匹配的点，然后从 K 个特征中随机选取一个点

template <typename PointSource, typename PointTarget, typename FeatureT> void
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::findSimilarFeatures (const FeatureCloud &input_features, const std::vector<int> &sample_indices, std::vector<int> &corresponding_indices)
{std::vector<int> nn_indices (k_correspondences_);std::vector<float> nn_distances (k_correspondences_);corresponding_indices.resize (sample_indices.size ());for (size_t i = 0; i < sample_indices.size (); ++i){// 找到最接近输入特征的 k 个特征feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);// 随机选择一个并将其添加到相应的索引中int random_correspondence = getRandomIndex (k_correspondences_);corresponding_indices[i] = nn_indices[random_correspondence];}
}

computeErrorMetric

该函数用于计算对应点之间的误差。

template <typename PointSource, typename PointTarget, typename FeatureT> float
pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric (const PointCloudSource &cloud, float)
{std::vector<int> nn_index (1);std::vector<float> nn_distance (1);const ErrorFunctor & compute_error = *error_functor_;float error = 0;// 循环遍历 source 中的每一个点for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i){// 在 target 点云中查找距离 cloud.points[i] 点的最近点tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance);// 误差累加error += compute_error (nn_distance[0]);}return (error);
}

总结

经过以上的分析，总结一下 SAC_IA 算法的主要流程：

在 source 点云中随机选择 n 个采样点，n 个采样点有距离限制
在 target 点云中查找采样点的对应点（特征最相近）

使用 kdtree 方法查找与特征点对应的 K 个特征，然后从其中随机选择一个特征作为对应点
根据选择的采样点与对应点求取旋转矩阵
根据旋转矩阵计算两幅点云的误差，对比每次迭代结果误差，更新最小误差与最佳变换矩阵
以上步骤不断迭代，直至迭代次数达到设定值

SAC_IA 算法也是基于 RANSAC 方法，SAC_IA 算法相对 RANSAC 方法而言，改进之处在于：

随机选择采样点：采样点数量更多，采样点之间的关系有距离限制
对应点查找：不是点与点距离的查找，而是特征与特征之间的匹配，特征描述符考虑了点的局部结构，相比计算点与点之间的距离，精度更加准确，但速度会降低

注：

以上代码中，由于直接截取了函数代码，有一些成员变量、函数看不到定义，理解起来有困难。

PCL 实现 SAC_IA 算法原理源码解析相关推荐

六、Dubbo协议模块原理源码解析
课程概要: RPC协议基本组成 RPC协议报文编码与实现详解 Dubbo中所支持RPC协议与使用 RPC协议基本组成 RPC 协议名词解释在一个典型RPC的使用场景中,包含了服务发现.负载.容错.网 ...
vue双向绑定原理源码解析
当我们学习angular或者vue的时候,其双向绑定为我们开发带来了诸多便捷,今天我们就来分析一下vue双向绑定的原理. 简易vue源码地址:https://github.com/maxlove123 ...
Spring注解配置工作原理源码解析
一.背景知识在[Spring实战]Spring容器初始化完成后执行初始化数据方法一文中说要分析其实现原理,于是就从源码中寻找答案,看源码容易跑偏,因此应当有个主线,或者带着问题.目标去看,这样才能最 ...
单例设计模式-Enum枚举单例、原理源码解析以及反编译实战
package com.learn.design.pattern.creational.singleton;/*** 这个类是Enum类型* 这个枚举非常简单* 枚举类是Object* 他在多线程的时 ...
java怎么让main方法不退出_JAVA线程池原理源码解析—为什么启动一个线程池，提交一个任务后，Main方法不会退出？...
public static void main(String[] args) { ExecutorService service = Executors.newFixedThreadPool(10); ...
python 聚类分析实战案例:K-means算法(原理源码)
K-means算法: [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-xvV44zrK-1573127992123)(https://img-blog.csdn.net/ ...
python聚类分析案例_python 聚类分析实战案例:K-means算法(原理源码)
K-means算法: 关于步骤:参考之前的博客关于代码与数据:暂时整理代码如下:后期会附上github地址,上传原始数据与代码完整版, 各种聚类算法的对比:参考连接 Kmeans算法的缺陷 1.聚类 ...
Spring之事务底层原理源码解析
文章目录一.`@EnableTransactionManagement`工作原理二.Spring事务基本执行原理三.Spring事务详细执行流程四.Spring事务传播机制五.Spring事 ...
深入解析SpringBoot核心运行原理和运作原理源码
SpringBoot核心运行原理 Spring Boot 最核心的功能就是自动配置,第 1 章中我们已经提到,功能的实现都是基于"约定优于配置"的原则.那么 Spring Boot ...

PCL 实现 SAC_IA 算法原理源码解析

PCL 实现 SAC_IA 算法原理源码解析

主要函数

computeTransformation

selectSamples

findSimilarFeatures

computeErrorMetric

总结

PCL 实现 SAC_IA 算法原理源码解析相关推荐

最新文章

热门文章