一、原理概述
1.点云配准流程图
2.快速点特征直方图FPFH
快速点特征直方图（Fast Point Feature Histogram，FPFH）是一种用于点云数据描述和匹配的特征表示方法。它是基于点云中每个点的局部几何信息来计算的。
FPFH的计算过程包括以下几个步骤：
- 首先需要确定每个点最近邻点集合。可以使用kd树等数据结构来加速最近邻搜索过程。
- 对于每个点，计算其与最近邻点之间的相对位置和法线信息。这可以通过计算点与其最近邻点之间的距离、角度等来实现。同时，为了保持旋转不变性，可以使用点云曲率法线估计方法来计算点的法线向量。
- 其次，建立一个直方图来描述每个点与最近邻点之间的关系。直方图的每个条目表示一种关系，比如距离、角度等。直方图的bin数量和范围可以根据具体需求进行选择。
- 然后，将每个点的直方图与最近邻点的直方图进行组合。这可以通过将两个直方图的值进行加权平均来实现，其中权重可以根据点之间的距离、角度等进行选择。
- 最后，将每个点的组合直方图作为其特征表示。这个特征向量可以用于点云的描述、匹配和识别等任务。
FPFH具有以下特点：
局部性：FPFH是一种局部特征表示方法，它主要描述了点与其最近邻点之间的几何关系。
鲁棒性：FPFH对于点云的旋转、尺度和噪声具有一定的鲁棒性。
可扩展性：FPFH可以与其他点云特征描述方法结合使用，例如法线特征、曲率特征等，以提高点云的表达能力和鉴别能力。
3.采样一致性SAC_IA粗配准
采样一致性（Sample Consensus，SAC）是一种常用的点云配准方法，它通过采样一致性评估模型和数据之间的一致性来估计最佳的刚体变换参数。SAC-IA（Sample Consensus Initial Alignment）是SAC的一种变种，用于进行粗配准（coarse registration）。
SAC_IA其算法步骤如下：
1）对采样点进行选取。在待配准点云M中选取ｎ个采样点，并且要满足采样点两两之间的距离大于预先设定的最小距离阈值ｄ，用以保证各采样点的FPFH特征均不相同。
2）对采样点的对应点进行查找。依据采样点 的FPFH特征，在目标点云N中通过近邻搜索找到与采样点中每个点FPFH相差最小的点，即为 特征相似的点，将其作为目标点云N 中与待配准 点 云M中的 采样点相对应的点，并通过RANSAC算法剔除错误的对应点对，从而保证相似特征点对的正确匹配。
3）求解变换关系。首先求出对应点之间的变换矩阵，然后依据该变换计算对应点变换后的距离误差和函数，并将此函数作为评判配准性能的指标。
SAC-IA粗配准方法的优点在于它能够处理初始对齐差距较大的点云数据，并且具有较好的鲁棒性。然而，由于采样的随机性，SAC-IA可能存在局部最优解的问题，因此在需要更高精度的配准任务中，可能需要结合其他的配准方法进行进一步的优化。
4.正态分布变换NDT精配准
在SAC-IA初始配准中，通过优化刚体变换参数，使得待配准的两个点云位置尽可能地靠近，从而缩小点云之间的旋转和平移误差，实现较好的初始位置对齐。然后可以使用N-Dimensional Transformation (NDT) 配准方法进一步提高配准的精度。
NDT（N-Dimensional Transformation）是一种基于概率的点云配准方法，通过将三维点云分割为大小均匀的立方体，并将每个立方体中的点云数据转换成概率密度函数来进行配准。相对于ICP（Iterative Closest Point）算法，NDT方法不需要每次迭代都重新匹配对应点对，因此具有较高的配准效率。
下面是NDT算法的主要步骤：
a. 把点云模型划分为均匀大小的立方体
b. 求出各个立方体中点的均值向量q和协方差矩阵C:
式中，n为立方体中点的总数，Xk为立方体中的一点。
c. 对点Xk进行正态分布建模Ｎ(ｑ，Ｃ)，其中Xk的概率密度函数表示为：
d. 根据变换矩阵T对待配准点云的每个点进行变换，然后依据变换后的点T(Xi)所在的立方体的概率密度分布函数计算相应的概率分布函数。
e. 把各个待配准点的相应概率分布相乘，并将相乘的结果作为变换矩阵T的分数值s§：
f. 依据 Hessian矩阵法计算s§的最优解，从而得出最佳变换矩阵。
二、实验代码
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h> // 体素下采样滤波
#include <pcl/features/normal_3d_omp.h>// 使用OMP需要添加的头文件
#include <pcl/features/fpfh_omp.h> // fpfh加速计算的omp(多核并行计算)
#include <pcl/registration/ia_ransac.h>// sac_ia算法
#include <pcl/registration/ndt.h> // NDT配准算法
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
using namespace std;
typedef pcl::PointCloud<pcl::PointXYZ> pointcloud;
typedef pcl::PointCloud<pcl::Normal> pointnormal;
typedef pcl::PointCloud<pcl::FPFHSignature33> fpfhFeature;
#pragma region// 下采样滤波
pointcloud::Ptr voxel_grid_fiter(pointcloud::Ptr& inCloud)
{
pcl::VoxelGrid<pcl::PointXYZ> vs;
vs.setLeafSize(0.005, 0.005, 0.005);
//vs.setLeafSize(10.005, 10.005, 10.005);
vs.setInputCloud(inCloud);
pointcloud::Ptr outCloud(new pointcloud);
vs.filter(*outCloud);
return outCloud;
}
// 计算FPFH特征
fpfhFeature::Ptr compute_fpfh_feature(pointcloud::Ptr inCloud)
{
// 法向量估计
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
pointnormal::Ptr normals(new pointnormal);
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
n.setInputCloud(inCloud);
n.setNumberOfThreads(8);//设置openMP的线程数
n.setSearchMethod(tree);
n.setKSearch(10);
n.compute(*normals);
//------------------FPFH估计-------------------------
fpfhFeature::Ptr fpfh(new fpfhFeature);
pcl::FPFHEstimationOMP<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> f;
f.setNumberOfThreads(8); //指定8核计算
f.setInputCloud(inCloud);
f.setInputNormals(normals);
f.setSearchMethod(tree);
f.setKSearch(10);
f.compute(*fpfh);
return fpfh;
}
// 可视化
void visualize_registration(pcl::PointCloud<pcl::PointXYZ>::Ptr& source, pcl::PointCloud<pcl::PointXYZ>::Ptr& target, pcl::PointCloud<pcl::PointXYZ>::Ptr& regist)
{
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Registration"));
int v1 = 0;
int v2 = 1;
viewer->setWindowName("SAC-IA+NDT配准结果");
viewer->createViewPort(0, 0, 0.5, 1, v1);
viewer->createViewPort(0.5, 0, 1, 1, v2);
viewer->setBackgroundColor(0, 0, 0, v1);
viewer->setBackgroundColor(0.05, 0, 0, v2);
viewer->addText("Raw point clouds", 10, 10, "v1_text", v1);
viewer->addText("Registed point clouds", 10, 10, "v2_text", v2);
//原始点云绿色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source, 0, 255, 0);
//目标点云蓝色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target, 0, 0, 255);
//转换后的源点云红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> transe(regist, 255, 0, 0);
viewer->setBackgroundColor(255, 255, 255);
viewer->addPointCloud(source, src_h, "source cloud", v1);
viewer->addPointCloud(target, tgt_h, "target cloud", v1);
viewer->addPointCloud(target, tgt_h, "target cloud1", v2);
viewer->addPointCloud(regist, transe, "pcs cloud", v2);
//添加坐标系
//viewer->addCoordinateSystem(0.1);
//viewer->initCameraParameters();
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(10000));
}
}
#pragma endregion
int main(int argc, char** argv)
{
// 加载点云数据
pointcloud::Ptr source_cloud(new pointcloud);
pointcloud::Ptr target_cloud(new pointcloud);
/* pcl::io::loadPCDFile<pcl::PointXYZ>("E:\\******.pcd", *source_cloud);
pcl::io::loadPCDFile<pcl::PointXYZ>("E:\\******0.pcd", *target_cloud);*/
pcl::io::loadPCDFile<pcl::PointXYZ>("E:\\*****\\ArmadilloOnHeadMultiple_0.pcd", *source_cloud);
pcl::io::loadPCDFile<pcl::PointXYZ>("E:\\*****\\ArmadilloOnHeadMultiple_30.pcd", *target_cloud);
if (source_cloud->empty() || target_cloud->empty())
{
cout << "请确认点云文件名称是否正确" << endl;
return -1;
}
else
{
cout << "从目标点云读取 " << source_cloud->size() << " 个点" << endl;
cout << "从源点云中读取 " << target_cloud->size() << " 个点" << endl;
}
pointcloud::Ptr source(new pointcloud);
pointcloud::Ptr target(new pointcloud);
source = voxel_grid_fiter(source_cloud);
cout << "source提取的特征点个数:" << source->points.size() << endl;
target = voxel_grid_fiter(target_cloud);
cout << "target提取的特征点个数:" << target->points.size() << endl;
// 1、计算源点云和目标点云的FPFH
fpfhFeature::Ptr source_fpfh = compute_fpfh_feature(source);
fpfhFeature::Ptr target_fpfh = compute_fpfh_feature(target);
// 2、采样一致性SAC_IA初始配准
clock_t start, end;
start = clock();
pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;
sac_ia.setInputSource(source);
sac_ia.setSourceFeatures(source_fpfh);
sac_ia.setInputTarget(target);
sac_ia.setTargetFeatures(target_fpfh);
sac_ia.setMinSampleDistance(0.007); // 设置样本之间的最小距离
sac_ia.setMaxCorrespondenceDistance(0.1);// 设置对应点对之间的最大距离
sac_ia.setNumberOfSamples(100); // 设置每次迭代计算中使用的样本数量（可省）,可节省时间
sac_ia.setCorrespondenceRandomness(6); // 设置在6个最近特征对应中随机选取一个
pointcloud::Ptr align(new pointcloud);
sac_ia.align(*align);
Eigen::Matrix4f initial_RT = Eigen::Matrix4f::Identity();// 定义初始变换矩阵
initial_RT = sac_ia.getFinalTransformation();
cout << "\nSAC_IA has converged," << sac_ia.hasConverged() << " score is " << sac_ia.getFitnessScore() << endl;
cout << "SAC_IA初始配准变换矩阵：\n" << initial_RT << endl;
// 3、正态分布变换（NDT）
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setInputSource(source); // 设置要配准的点云
ndt.setInputTarget(target); // 设置点云配准目标
ndt.setStepSize(4); // 为More-Thuente线搜索设置最大步长
ndt.setResolution(0.01); // 设置NDT网格结构的分辨率（VoxelGridCovariance）
ndt.setMaximumIterations(35); // 设置匹配迭代的最大次数
ndt.setTransformationEpsilon(0.01); // 为终止条件设置最小转换差异
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
ndt.align(*output_cloud, initial_RT); // align()函数有第二个参数，输入的是初始变换的估计参数
end = clock();
cout << "NDT has converged:" << ndt.hasConverged()
<< " score: " << ndt.getFitnessScore() << endl;
cout << "正态分布变换变换矩阵：\n" << ndt.getFinalTransformation() << endl;
cout << "运行时间: " << float(end - start) / CLOCKS_PER_SEC << "s" << endl;
// 4、使用变换矩阵对未进行滤波的原始源点云进行变换
pcl::transformPointCloud(*source_cloud, *output_cloud, ndt.getFinalTransformation());
// 5、可视化
visualize_registration(source_cloud, target_cloud, output_cloud);
return 0;
}
三、实验结果
下图，绿色为源点云，蓝色为目标点云，红色为配准后的点云
四、总结
数据采用的斯坦福大学的Armadil，从配准后的效果来看，基本上能正确配准，但在其手部、脚部等一些区域，没有正确配准，最后的配准精度在4.8693e-05。因为NDT是基于概率的方法计算，精确性不高且稳定性也不高。若感兴趣，可以继续调整一些参数或者加以改进来提高精度。
五、参考
[1]荆路,武斌,李先帅.基于SAC-IA和NDT融合的点云配准方法[J].大地测量与地球动力学,2021,41(04):378-381.
浅谈FPFH算法实现原理及其在点云配准中的应用
基于SAC_IA和NDT融合的点云配准方法
416
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
