POSIT算法

一、基本思想：算法分两部分

(1)带有比例系数的正交投影变换SOP，根据线性方程组求出旋转矩阵和平移向量；

(2)由得出的旋转矩阵和平移向量系数，更新比例系数，（scale factor），再由比例系数更新原有的点进行迭代。

二、算法过程：

(1)假设矩阵 和平移向量，f是焦距，在透视投影变换中，，

而在SOP中，，其中比例因子是s=f/Z0。

(2)作为基本的透视投影变换，将3D点透视投影到图像平面上得到齐次坐标，

变换过程为，因为m是齐次坐标，所以等式右边除以Tz，不会受到影响，得到：其中s=f / Tz    即得到：其中。

（3）现在变换过程为，即为方程组，

w初始值为1；

（4），

，A为（n+1）×4矩阵，；然后初始方程组变成，

得到解。

（5）至少有4个不共面的2D-3D点对，（求出K1K2后，将其除以已知的定值s，可得到R1，R2，Tx，Ty，然后得到R3=R1×R2，并且将R1R2R3归一化为单位向量）。

（6）然后更新，因为对不同的2D-3D点对，s=f / Tz是定值，f是焦距，Tz看作所有3D点Z坐标的平均值，不同的3D点，a不同w也就不同。这样将原来的2D点变为。

（7）重复（2）由原有3D点和更新后的2D点，用最小二乘法解方程组，得到新的K1K2再更新w，更新2D坐标。