ICP的一点学习笔记

1. ICP算法相关
2.GICP算法笔记,参考http://www.roboticsproceedings.org/rss05/p21.pdf
Abstract:在本文中结合ICP和点云面ICP算法到一个概率框架。接下来要使用这个框架进行局部平面结构而不是通常的点面方法。这个可以认为是面到面方法。与标准的ICP和点到面方法相比，这种方法在误匹配方法有更强的鲁棒性，因此更容易调节最大匹配参数。此外在性能改进方面，这种方法允许在ICP框架中表达更多的概率模型。维持ICP的速度和简单化条件下，GICP也可以允许增加异常项、测量噪声和其他的概率方法来增强鲁棒性。由于遮挡和有限的传感器范围，点云和range图应用中的大多数需要精确的将多个范围的图像合并成一个单一的模型方法。
ScanMatching:在90s早期应用于扫描匹配的ICP算法有了很多变种。当时发表的三篇文章到现在仍然是最先进的扫描匹配解决方案。
Besl and McKay的文章直接阐述了几何形状和点云的匹配。Chen and Medioni考虑了对象建模中调整范围数据的更具体问题，他们的方法利用了点数据考虑为平面的趋势，并引入了点面ICP变种。Zhang几乎同时提出了ICP，但在算法的对应选择阶段增加了一种鲁棒的离群抑制方法。现在两种流行的是Iterative Dual Correspondence  和 Metric-BaseICP.IDC通过维护两组通信来改进点匹配过程。MbICP的设计目的是通过明确地将旋转误差作为距离度量的一部分来改善具有较大初始方向误差的收敛性。大多数基于ICP的方法的主要优点是简单和相对快速的性能，当用kd-tree实现近点查找时。缺点包括隐含的假设完全重叠的形状匹配和理论要求，点是从已知的几何表面，而不是测量。第一个假设被部分重叠的扫描(从不同的位置)打破。第二种方法会产生问题，因为不同的物理表面离散化使得即使在收敛之后也不可能得到各个点的精确重叠。点对面，如[7]中所建议的，通过不惩罚沿表面的偏移量来解决离散化问题。完全重叠假设通常通过在匹配中设置最大距离阈值来处理。除了点对面外，大多数ICP变体使用封闭形式的解决方案来迭代计算对应的对齐。这通常使用[10]或基于两个数据集的相互关系的类似技术来完成。最近，有兴趣使用一般的非线性优化技术，而不是更具体的封闭形式接近[9]。这些技术的优势在于，它们允许更一般的最小化函数，而不是欧氏距离的和。[9]使用非线性优化与稳健的统计，以显示更广泛的收敛区域。在这些技术中，概率技术是最有动力的，因为已经有大量的理论工作在支持它们。[2]应用了一个概率模型，它假设第二次扫描是通过一个随机过程从第一次扫描生成的。[4]应用射线跟踪技术来最大化对齐的概率。[8]构建一组兼容的对应，然后在这个分布上最大化对齐的概率。[17]引入了一个完整的概率框架，该框架考虑了运动模型，并允许估计注册的不确定性。该方法的一个有趣之处在于，它使用了广义霍夫变换的采样模拟来计算不需要显式对应的对齐，同时考虑了二维数据集的两个表面法线。还有大量的文献致力于解决多重扫描的全局对齐问题([18]和许多其他的)。许多实现此目的的方法(尤其是[18])使用成对匹配算法作为基本组件。这使得成对匹配的改进也适用于全局对齐问题。
我们的方法处在标准ICP和全部几率模型之间，这是使用最大似然估计作为非线性优化，使用kd树计算相关匹配。这是与从不同的，因为它给出对称性，加入了7中的结构化假设。由于最近邻查找是使用欧式距离计算，所以kd树可以用来在大的点云中提高性能。与8相比，我们认为数据应该被假设到面上，因为很多环境下的采样数据都是分段平滑的。通过对最小化过程进行概率解释，我们可以很容易地扩展该技术，使其包含来自两次扫描的结构信息，而不是像“点对面”ICP那样只包含一次扫描。我们证明了引入这种对称性可以提高精度并减少对参数的依赖。GICP 处理大型3D点云。
A.ICP:标准ICP算法的关键步骤主要有两步，1.计算两次扫描间的匹配，2，计算使匹配点距离最小的转换矩阵。
重复迭代这两步直到转换收敛。由于不使用全部重叠假设，必须添加一个最大匹配阈值dmax.这个阈值使一部分点参加与第二部分的匹配。在大部分的ICP实现中，dmax值的选择是收敛和精确度的折衷。太小会导致不好的收敛，太大会导致误匹配。

基本的ICP算法可分为以下几个关键步骤：
(1)选取：在输入点集中采用某种采样策略，选择一定的点作为下一步计算的点集对象
(2)匹配：利用某种匹配算法，实现两个所选点集中点的对应
(3)权重：利用评价函数，计算所得匹配点的权重
(4)删除：按照某种丢弃算法，将某些误差过大的匹配点对剔除
(5)计算点集距离误差：按照特定的几何方法，计算匹配点对在旋转平移后总的误差值
(6)最优化：利用最优化算法，不断迭代，最终得到使得两个点对集之间距离均方差最小的旋转平移矩阵  

B.点面ICP:点面ICP通过使用面法线信息提高了性能，最小化沿着表面向量的误差。这个将上式改成：， 其中是mi上的表面法线。
泛化ICP(GICP):
A.Derivation:GICP是使用概率模型来最小化算法中的优化项，其他算法保持不变。其中匹配仍是计算标准欧式距离而不是几率测量。这使得可用 kd树的最近邻查找方法，因此来保证ICP的优势而不是全几率方法。 (2)。为了简化，我们假定最近邻的已经查询，两个点云A={ai}i=1,...N,B={bi}i=1,..根据他们的对应关系建立的索引序列。其中也假定移除||mi-T ·bi|| > dmax的数据。在几率模型中，我们假定存在潜在点集A={ai},B={bi}根据ai ∼ N(aˆi, CiA)，bi ∼ N(bˆi, CiB)产生A ,B.



B.面到面：为了提高点面ICP性能和这个模型的对称性，GICP是考虑两次扫描的面信息。最直观的方法是考虑点面中第二次扫描的面信息，但在数学上不可行，因为所涉及的矩阵是奇异的。相反我们用点面来产生几率模型。点面算法使点云比任意空间点有了更多结构（实际上是测距传感器测的表面模型的集合 ），这意味着着我们处理空间中扫描的二维流形。由于真实世界至少是分段可微的，我们假定数据是局部平面的。由于是从两个角度获取的流形，因此一般情况下获取的不是同一个点。本质上，每个测量点在它的面上只提供一个约束。为了构造这种结构，我们考虑了每个采样点在其局部平面上具有高协方差分布，在其表面法线方向上具有低协方差分布。表面法线为e1的点的协方差矩阵是（e 1 1,0 1 0,0 0 1）, 其中e是沿着法线方向的小常数。这表明沿着法线方向有很高的可信度,但在表面方向上不确定。我们采样的点ai,bi就是这种分布。给定了ai,bi点的法向量ui,vi，CiA,CiB是通过旋转上述协方差矩阵计算得到的，因此e代表沿着表面法线的不确定性。 Rx代表基向量e1到x方向的转换。变换矩阵是通过公式（2）计算得来的。

图给出算法在极端情况下的解释。在这种情况下，绿色扫描的垂直部分上的所有点都与红色扫描中的一个点不正确相关。由于表面取向是不一致的，平面与平面将自动对这些匹配进行折衷：每个对应的最终求和协方差矩阵将是各向同性的，也是稀疏和明确定义的对应协方差矩阵，它对目标函数的贡献非常小。这种行为从另一角度来看是对每一个匹配都是一个软约束。不一致的匹配允许红色扫描点沿着x轴移动，而绿色扫描点可以沿着y轴自由移动。这种不正确匹配对整体配准的约束是很弱且无信息的。计算表面协方差矩阵需要两次扫描所有点的表面法线。从点云恢复表面法线的技术很多，法线的精度自然地在算法的性能中起着重要的作用。本文中，我们使用每个扫描点的最近20个点的协方差矩阵进行主成分分析PCA。在这种情况下，本征矢量与表面法线的最小特征值有关。这种方法用于计算点到面和GICP的法线。对于GICP,构造旋转矩阵，使方差的分量与表面法线一致。
Result:我们比较了三种算法来测试所提技术的性能。虽然标准ICP中存在有效的T封闭解，但为了简化比较，我们利用共轭梯度实现了最小化。在两次扫描之间引入一个已知的偏移量后，根据收敛到正确的解决方案来分析性能。我们将标准ICP的测试次数限制在250次以内，并且另外两种算法的迭代次数为50次，因为在此之前通常会达到收敛。
模拟(图3)和真实(图4)的数据均用于演示理论和实际性能。模拟数据集还允许在更大范围的环境中进行测试，并且具有绝对已知的地面真实度。室外模拟环境与收集到的数据的主要区别在于遮挡的数量，以及地面更多的丘陵特征。真实世界的室外测试也展示了更详细的特性和更有代表性的测量噪声的性能。
模拟数据由安装在旋转接头上的病态扫描仪进行射线追踪生成。创建了两个3D环境，分别在室内(图2(a))和室外(图2(b))场景中测试性能。室内环境以办公室走廊为基础，而室外环境则反映了建筑周围的典型景观。在这两种情况下，我们模拟了一个装备了激光扫描仪的机器人沿着轨迹移动，并在轨迹上的定点进行测量。为了使测试更加真实，加入了高斯噪声。还对来自一辆装有仪器的汽车的日志的真实数据进行了测试。这些日志包括车顶安装的Velodyne测距仪记录的数据，当时这辆车正在郊区环境中绕行，并使用GPS和IMU数据进行了注释。这使得应用基于成对约束的SLAM技术来生成地面真实位置成为可能。
尽管标准ICP本身已经被用在成对匹配上来产生真实数据，SLAM方法扫描间隔是一个小的数量级。相反，为了增加挑战的难度性，测试用的扫描对之间的空间更大15-20m。为了测量性能，所有的算法都在三组数据集中成对scan。对每个扫描对， 初始偏移量设置为真偏移量，并添加统一生成的误差项。  通过对特定算法的所有扫描对平均定位误差来衡量性能。在所有测试中，忽略旋转误差。之前提到过，dmax的选取在ICP中起到很大的作用。
设置一个较低的值可以减少收敛的机会，但是可以提高精度。设置一个过高的值会增加收敛半径，但会降低精度，因为会产生更多不正确的对应。本文提出的算法通过对不正确对应的影响进行折现，大大降低了选择较大dmax值的代价。这使它更容易在广泛的环境中获得良好的性能，而不必手动为每个环境选择一个dmax值。除了提高了精度外，新算法在计算转换时对两种扫描都给予了同等的考虑。图6和图7显示了使用这两种扫描结构去除点对面存在的局部极小值的两种情况。这些图像代表了从上到下的维洛戴恩扫描图像，它们之间的距离大约为30米，并且是对齐的。图8显示了相同扫描对的一些附加视图，以更好地说明场景的结构。从传感器到户外环境的扫描范围为70-100米，就像从一辆正在行驶的汽车上看到的那样。
由于这种最小化仍然在ICP框架内执行，因此该方法将标准算法的速度和简单性与EM等全概率技术的一些优点结合在一起。理论框架还允许标准鲁棒性技术被纳入。例如，可以将高斯核函数与均匀分布相混合来建立离群点模型。高斯函数
RVs也可以被一个分布所代替，该分布考虑到匹配中的一定松弛量，以显式地对不精确对应进行建模(通过指定diT是在0周围的某个区域的恒定密度).虽然我们考虑了其中的一些变化，但它们都没有一个明显的易于最小化的闭合形式。这使得它们过于复杂，无法包含在当前的工作中，但却是未来研究的一个好主题。
V.结论
在本文中我们考虑了GICP，它考虑概率模型中两次扫描的局部平面结构。为了保持速度和准确型，ICP框架的大部分都没有改变。所提出的泛化方法只涉及转换矩阵的迭代计算。我们假定所有测量点都是服从中心在真实点的高斯分布，这样会有完美匹配。接下来使用MLE迭代计算匹配扫描的转换矩阵。一些列的模拟和实验结果表明，GICP提高了精度。同时，两次匹配的结构信息减少了误匹配的影响。因此选择最大匹配距离参数对性能的影响越小。这些改变保持了ICP的速度和简便性，同时提高性能消除参数选取带来的影响。

********************************************************************************************************************************
3.PCL中的icp, 参考PCL学习笔记二：Registration (ICP算法)_pcl::registration<pointt, pointt>::ptr registratio-优快云博客
PCL的icp里的transformation estimation是基于SVD的， SVD是奇异值分解Singular Value Decomposition;使用举例如下：

void PairwiseICP(const pcl::PointCloud<PointT>::Ptr &cloud_target, const pcl::PointCloud<PointT>::Ptr &cloud_source, pcl::PointCloud<PointT>::Ptr &output )

{
	PointCloud<PointT>::Ptr src(new PointCloud<PointT>);
	PointCloud<PointT>::Ptr tgt(new PointCloud<PointT>);

	tgt = cloud_target;
	src = cloud_source;

	pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setMaxCorrespondenceDistance(0.1);
	icp.setTransformationEpsilon(1e-10);
	icp.setEuclideanFitnessEpsilon(0.01);
	icp.setMaximumIterations (100);

	icp.setInputSource (src);
	icp.setInputTarget (tgt);
	icp.align (*output);//执行icp
//	std::cout << "has converged:" << icp.hasConverged() << " score: " <<icp.getFitnessScore() << std::endl;
		
	output->resize(tgt->size()+output->size());
	for (int i=0;i<tgt->size();i++)
	{ output->push_back(tgt->points[i]);	}
	cout<<"After registration using ICP:"<<output->size()<<endl;
}

 PCL中gicp的迭代使用的是BFGS的拟牛顿算法。它的参数如下：
setTransformationEpsilon();
setMaxCorrespondenceDistance();
setMaximumIterations();
setRANSACIterations();
setInputSource();   
setInputTarget();
align();
getFinalTransformation();
 在文件pcl/registration/impl/registration.hpp中。