基于点特征的各位姿求解算法对比（pose-estimation-compared）

基于点特征的位姿求解算法：

（1）非迭代：P3P 、 P4P 、 RPnP

（2）迭代：正交迭代（OI）算法 、 L-M 算法 、 SoftPOSIT 算法 、 Tsai算法（初始解不迭代，精确解迭代）

各常用算法稳定性、精度和实时性的对比结果:

（1）RPnP 算法求解 R、T 的误差均值和标准差都最小，精度最高；

（2）Tsai 算法求 解 R 的误差均值和标准差仅次于 RPnP 算法，但求解 T 的误差较大；

（3）正交迭代算法和 P3P 算法 误差中等，但 P3P 算法误差的标准差较大，说明 1000 组解中可能存在一部分误差较大的错误 解，使得误差均值增加，这也与 P3P 算法的多解选择有关；

（4）P4P 算 法求解 R 和 T 的误差均值和标准差都较大，说明其求解精度较低，且 R 的求解精度低于 T 的 求解精度。

（5）对于特征点数的影响，各算法总体趋势为：特征点数越多则误差越小。但随着特征点数增 多，算法误差并不是越来越小，而是在 10 或 12 个特征点之后趋于平稳，因此，在对特征点数 没有限制的应用场合，取 10 或 12 个特征点较为合适；

（6）Tsai 算法的求解时间最长，主要是由于其第二步基于最小二乘法迭代求解 t z 分量的精确解耗时较长。正交迭代算法求解时间次长，也与其迭代求 解过程有关。其余非迭代算法实时性都较好。

可以得到如下结论：

（1）RPnP 算法性能较好；

（2）Tsai 算法求解 R 的精度较高，求解 T 的精度较低，可利用冗余信息对 Tsai 算法 求解 T 的过程进行改进；

（3）P3P 算法精度适中，但其稳定性曲线呈双峰；

（4）正交迭代算法精度适中，考虑到本节采用初始值未知的正交迭代算法，以零矩阵作为 R 的初始值，可能影响正交迭代算法性能，需要讨论正交迭代算法初始值选取问题；

（5）P4P 算法求解精度相比于其他算法较低，且 R 的求解精度低于 T 的求解精度。