6D姿态估计常见技术总结

PnP：Perspective-n-Point（PnP）：

PnP问题：根据图像中特征点的二维像素坐标及其对应的三维空间坐标，来估计相机在参考坐标系中位姿的一类算法
思想：常见的是 根据n个3D-2D匹配点对，利用最小化重投影误差来求解相机外参的算法。

RANSAC：随机抽样一致算法

作用：在一个包含异常数据（外点）的数据集中选取出最大一致集（选取最大有效样本数据集）的算法，同时，一些情况下，也可以得到想要的模型参数

思想： 1.随机选取一组最小样本集合作为内点集，根据内点集评估模型，得到初始化模型参数。2.由得到的模型去评估所有的数据，将满足条件（误差小于一定阈值）的点作为内点，再用最小二乘法评估得到新模型。3.如果新模型达到要求，则将满足条件的点集作为最终的最大一致集

ICP：Iterative Closest Point

作用：利用两集合的N对对应点，计算空间变换得到R和T配准两个集合，可以说是解决PnP算法的一种
思想：
1.目标函数是投影误差平方和的平均，
2.寻找对应点，可是，我们现在并不知道有哪些对应点，因此，我们在有初值的情况下，假设用初始的旋转平移矩阵对source cloud进行变换，得到的一个变换后的点云。然后将这个变换后的点云与target cloud进行比较，只要两个点云中存在距离小于一定阈值（这就是题主所说的ICP中的一个参数），我们就认为这两个点就是对应点。
3. R、T优化。有了对应点之后，我们就可以用对应点对旋转R与平移T进行估计。这里R和T中只有6个自由度，而我们的对应点数量是庞大的（存在多余观测值）。因此，我们可以采用最小二乘等方法求解最优的旋转平移矩阵。
4. 迭代。我们优化得到了一个新的R与T，导致了一些点转换后的位置发生变化，一些最邻近点对也相应的发生了变化。因此，我们又回到了步骤2)中的寻找最邻近点方法。2)3)步骤不停迭代进行，直到满足一些迭代终止条件

FPS ：farthest point sampling

作用：均匀对物体模型的表面点进行采样，采样点会均匀的覆盖模型表面
思路：选取一个点（本文选取目标的中心）添加到集合中进行初始化，然后，选取离当前集合最远的(文中采取的是目标表面)点加入到集合中，反复进行直到选满K个。（点与集合的距离定义为点与集合中所有点的距离的最小值）上