0. 简介
作为常用的配准方法,ICP和NDT两种匹配被广泛应用于激光雷达的点云配准方法中。我们知道IPC的匹配主要是描述了点到点的匹配方法,而无法胜任点到面以及面到面的匹配,而本博客主要就是将向读者分析《Generalized-ICP》这篇论文,GICP可以通过点到点的距离作为损失函数求解point-to-point的损失函数,点到局部目标点局部拟合的平面距离作为point-to-plane的损失函数,而文中主要提到的plane-to-plane损失函数则是假设点云具有平面特征,这意味着在3D空间处理采样2D流形。
1. GICP统一模型
GICP引入了概率信息(使用协方差阵),提出了ICP的统一模型。本文方法的核心思想是如何从概率的角度去看待和推导出ICP算法的目标函数。这里我们直接看原文就好,原文提到: 假设有两个匹配好的点集, , 且 和 是对应点(A为source,B为target)
再假设两个点云中的每个点,都是服从高斯分布的,其原因是由于测量等环节的误差,每个点的位置的测量值实际上是和真值 存在偏差
对于 有:
(注意有上标 )是理想中的correct rigid transform。代表了两个点云真实的转换关系,我们需要一个目标函数来寻找出最佳的 ,以下是目标函数的推导过程:
首先定义残差 , 代表了对原始点云使用 做转换后,第 个点对的有向距离。
它是由分布采样而来
其中的等号变换可以参考这篇文章。
因为 都被我们假设为独立的、服从高斯分布的随机变量,所以将上式中的 替换为 ,则可以变为:
接下来就是这篇文章的重点,
可以被看作
的概率分布中待估计的分布参数,借助最大似然估计(MLE)的思想,我们寻找一个是的当前样本
出现概率最大的
:
这一部分是执行了取log的操作,然后进一步化简
上面的式子是参考了Multivariate normal distribution的取对数以及代入的方法。 对于多元常态分布 ,其概率密度函数可以表示为 对上面的式子取log可以得到: 代入 ,得到: 这样也就得到了我们上面的输出结果。这里的结果如果发现正态分布的协方差矩阵的行列式为常数时,则只需要优化最后一项就可以了。最后一项的二次型又被称作马哈拉诺比斯距离(马氏距离),极大似然估计等价于最小化样本点与均值之间的马氏距离。更详细的内容可以参考 高翔《视觉SLAM14讲》6.1 状态估计问题 。
这一部分则是对上一步的进一步化简,在 的情況下
然后又因为三维刚体变换矩阵中的旋转矩阵行列式值为1,平移矩阵行列式值也为1。又因为 是旋转平移矩阵,可以拆成旋转矩阵和平移矩阵的乘积。且 ,所以有矩阵的行列式值 ,因此
照视觉十四讲所说,这里对 做优化。其中第一项为常数,则可以忽略,其中 可以参考这个推导。
然后舍去负号,则可以将上式化简为论文中的公式2:
到此为止我们学习了GICP中最主要的公式推导公式了。
2. ICP应用
这里我们直接参照keineahnung2345的文章。文中介绍了三种ICP的推导,这一节要借助上文的结论。
2.1 point-to-point
传统的点到点ICP可以用GICP的框架表示如下 验证: 可以看出其目标为最小化点对间距离的平方和,也就是点到点ICP更新公式
2.2 point-to-plane
首先定义一个为正交的投影矩阵 ,有以下性质 。 其中 会将向量投影到目标点云 中的第 点 法向量的局部平面上,因此 也就是转换后的 到 所在平面的距离。 验证:
和GICP比较我们就可以发现关系为
2.3 plane-to-plane
这里是GICP专门提出的一种方法,即相对于点到点和点到面加入概率模型(协方差阵)
平面到平面算法的做法是,假设点云具有平面特征,这意味着在3D空间处理采样2D流形。 由于现实世界的曲面至少是分段可微的,我们可以假设我们的数据集是局部平面的。此外,由于我们从两个不同的角度对流形进行采样,因此通常不会对完全相同的点进行采样(即,对应关系永远不会是精确的)。 从而导致采样点在局部拟合的平面方向上的不确定性较大,但是在法向量方向上不确定性较小。
为此,每个测量点仅提供沿其曲面法线的约束。为了对这种结构进行建模,我们考虑每个采样点沿其局部平面以高协方差分布,而在曲面法线方向(垂直于平面方向)以极低协方差分布(即点云法线方向不在局部平面上)。假设局部拟合平面上某一点的法向量 是沿X轴的,则该点协方差矩阵变为:
为沿着法线方向极小的常数。
因为实际上法向量并不一定是沿 轴方向,所以需要进行坐标转换。假设 对应的法向量分别为 ,则它们对应的协方差阵为:
为
到
旋转矩阵。
上述协方差计算过程可以表述如下图:
确定协方差阵后利用
得到最大似然估计推导。 这里其实就是怎么确定协方差阵。
当然也可以通过PCA计算协方差阵(代替上述求解过程):
PCA求解时要注意公式(4)对协方差有个求逆过程,需要注意当协方差阵奇异时,用微小量替代0值(类似NDT中处理方式)。
3. PCL中GICP代码
// 利用控制台计算时间
using namespace std;
int
main()
{
pcl::console::TicToc time;
// -----------------加载点云数据---------------------------
pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("30m1A.pcd", *target);
pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("30m2A.pcd", *source);
cout << "读取源点云个数:" << source->points.size() << endl;
cout << "读取目标点云个数:" << target->points.size() << endl;
time.tic();
//-----------------初始化GICP对象-------------------------
pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp;
//-----------------KD树加速搜索---------------------------
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>);
tree1->setInputCloud(source);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>);
tree2->setInputCloud(target);
gicp.setSearchMethodSource(tree1);
gicp.setSearchMethodTarget(tree2);
//-----------------设置GICP相关参数-----------------------
gicp.setInputSource(source); //源点云
gicp.setInputTarget(target); //目标点云
gicp.setMaxCorrespondenceDistance(100); //设置对应点对之间的最大距离
gicp.setTransformationEpsilon(1e-10); //为终止条件设置最小转换差异
/* gicp.setSourceCovariances(source_covariances);
gicp.setTargetCovariances(target_covariances);*/
gicp.setEuclideanFitnessEpsilon(0.001); //设置收敛条件是均方误差和小于阈值, 停止迭代
gicp.setMaximumIterations(35); //最大迭代次数
//gicp.setUseReciprocalCorrespondences(true);//使用相互对应关系
// 计算需要的刚体变换以便将输入的源点云匹配到目标点云
pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
gicp.align(*icp_cloud);
//---------------输出必要信息到显示--------------------
cout << "Applied " << 35 << " GICP iterations in " << time.toc()/1000 << " s" << endl;
cout << "\nGICP has converged, score is " << gicp.getFitnessScore() << endl;
cout << "变换矩阵:\n" << gicp.getFinalTransformation() << endl;
// 使用变换矩阵对为输入点云进行变换
pcl::transformPointCloud(*source, *icp_cloud, gicp.getFinalTransformation());
//pcl::io::savePCDFileASCII (".pcd", *icp_cloud);
// -----------------点云可视化--------------------------
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final(new pcl::visualization::PCLVisualizer("配准结果"));
viewer_final->setBackgroundColor(0, 0, 0); //设置背景颜色为黑色
// 对目标点云着色可视化 (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color(target, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
// 对源点云着色可视化 (green).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
input_color(source, 0, 255, 0);
viewer_final->addPointCloud<pcl::PointXYZ>(source, input_color, "input cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "input cloud");
// 对转换后的源点云着色 (blue)可视化.
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color(icp_cloud, 0, 0, 255);
viewer_final->addPointCloud<pcl::PointXYZ>(icp_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
while (!viewer_final->wasStopped())
{
viewer_final->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return (0);
}
参考链接
https://blog.csdn.net/xinxiangwangzhi_/article/details/125236953
https://blog.csdn.net/pingjun5579/article/details/119029370
https://blog.csdn.net/keineahnung2345/article/details/122836492
https://www.freesion.com/article/24001018167/
评论(0)
您还未登录,请登录后发表或查看评论