【姿态估计】Pose estimation algorithm 之 Robust Planar Pose (RPP)algorithm

The RPP algorithm gives a more stable tracking (less jitter) than ARToolKit's pose estimation algorithm.

The robust pose estimator algorithm has been provided by G. Schweighofer and A. Pinz (Inst.of l.Measurement and Measurement Signal Processing, Graz University of Technology). Details about the algorithm are given in a Technical Report: TR-EMT-2005-01, available here. Thanks go to Thomas Pintaric for implementing the C++ version of this algorithm.

计算机视觉

1. 内参数标定

2. 外参数标定即姿态估计问题。从一组2D点的映射中估计物体的3D姿态。

3. 从三个对应点中恢复姿态，需要的信息是最少的，称为“三点透视问题”即P3P。同理，扩展到N个点，就称为“PnP”。

4. 基于视觉的姿态估计根据使用的摄像机数目分为单目视觉和多目视觉。根据算法又可以分为基于模型的姿态估计和基于学习的姿态估计。

5. OpenCV中有solvePnP以及solvePnPRansac用来实现已知平面四点坐标确定摄像头相对世界坐标系的平移和旋转。cvPOSIT基于正交投影，用仿射投影模型近似透视投影模型，不断迭代计算出估计值。此算法在物体深度相对于物体到相机的距离比较大的时候，算法可能不收敛。

6. 从世界坐标系到相机坐标系的转换，需要矩阵[R|t]，其中R是旋转矩阵，t是位移向量。如果世界坐标系为X，相机坐标系对应坐标为X‘，那么X' = [R|t]*X。从相机坐标系到理想屏幕坐标系的变换就需要内参数矩阵C。那么理想屏幕坐标系L = C*[R|t]*X。如何获得[R|t]，大致是已知模板上的几个关键点在世界坐标系的坐标即X已知，然后在摄像头捕获的帧里获得模板上对应点在屏幕坐标系的坐标即L已知，通过求解线性方程组得到[R|t]的初值，再利用非线性最小二乘法迭代求得最优变换矩阵[R|t]。

7. 大多数情况下，背景是二维平面，识别的物体也是二维平面。对于ARToolkit，识别的Targets就是平面的（但是这种方法鲁棒性不好）。如果内参数矩阵是已知的，那么知道4个或者更多共面不共线的点就可以计算出相机的姿态。

8. 相机姿态估计的问题就是寻找相机的外参数，即是最小化误差函数的问题。误差函数有的基于image-space，有的基于object-space。

9. RPP算法基于object-space为误差函数提供了一种可视化的方法。误差函数有两个局部极小值。在无噪声条件下，第一个局部极小值跟正确的姿态对应。另外的误差函数的极小值就是标准姿态估计算法为什么会抖动的原因。由于姿态估计算法最小化误差函数总是要使用迭代算法，因此需要一个初值。如果初值接近第二个局部极小值，那么迭代算法就收敛到错误的结果。

10. 估计第一个姿态，RPP算法使用任何已知的姿态估计算法，在这里里，使用迭代算法。从第一个姿态使用P3P算法估计第二个姿态。这个姿态跟误差函数的第二个局部极小值接近。使用估算的第二个姿态作为初值，使用迭代算法获得第二个姿态。最终正确的姿态是有最小误差的那个。

11. 这类问题最终都是解线性方程组AX=b的问题。当b∈R(A)时，x=A的广义逆*b;当b∈不R(A)时，能否是Ax接近b呢，即是否有x使||Ax-b||最小，习惯上用2-范数即欧式范数来度量。最小二乘解常存在，然后这样的解未必是唯一的。当在方程无解的情况下，要找到最优解。就是要最小化所有误差的平方和，要找拥有最小平方和的解，即最小二乘。最小化就是把误差向量的长度最小化。