位姿测量 | 正交迭代（OI）算法的原理及其MATLAB实现

简介

本文介绍正交迭代算法的求解思路和MATLAB代码实现。正交迭代算法（orthogonal iteration algorithm），简称OI算法，用来求解相对位置和相对姿态参数。

注意，本文只介绍OI算法的求解流程以及相关MATLAB代码实现。
具体的推导思路见参考文献：C.P. Lu, G. Hager, E. Mjolsness. Fast and globally convergent pose estimation from video images[J]. IEEE Trans, Pattern Analysis and Machine Intelligence 2000, 22(5):610-622.。

OI算法

公式推导

OI算法最重要的公式有几个，首先是误差函数：

$$E\left(R,T\right)=\sum _{i=1}^m{\Vert {{\mathbf{e}}_L}_{{}_i}\Vert }^2=\sum _{i=1}^m\sum _{j=1}^2{\Vert \left(I-V_i\right)\left(R{P}_i^j+T\right)\Vert }^2$$

其中$I$表示单位矩阵，$V_i$表示沿实现方向的投影矩阵，$R$表示旋转矩阵，$P$表示特征点在目标坐标系下的位置，$Q$表示特征点在相机坐标系下的位置。

其次是求解当前帧的最优位置参数$T$：
T ( k ) ( R ( k ) ) = 1 n ( I − 1 n ∑ i = 1 n V