位姿测量 | 正交迭代（OI）算法流程及其Python代码

简介

接位姿测量 | 正交迭代（OI）算法的原理及其MATLAB实现，本文介绍正交迭代算法的python 代码实现。

OI算法

正交迭代算法流程参见文章位姿测量 | 正交迭代（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算法最重要的公式有几个，首先是误差函数：

E ( R , T ) = ∑ i = 1 m ∥ e L i ∥ 2 = ∑ i = 1 m ∑ j = 1 2 ∥ ( I − V i ) ( R P i j + T ) ∥ 2 E\left( {R,T} \right) = \sum\limits_{i = 1}^m {{{\left\| {{{\bf{e}}_L}_{_i}} \right\|}^2}} = \sum\limits_{i = 1}^m {\sum\limits_{j = 1}^2 {{{\left\| {\left( {I - {V_i}} \right)\left( {RP_i^j + T} \right)} \right\|}^2}} } E(R,T)=i=1∑m​∥eL​i​​∥2=i=1∑m​j=1∑2​ ​(I−Vi​)(RPij​+T) ​2

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

其次是求解当前帧的最优位置参数$T$：
$$T^{(k)}\left(R^{(k)}\right)=\frac{1}{n}{\left(I-\frac{1}{n}\sum _{i=1}^n{V}_i\right)}^{-1}\sum _{i=1}^n\left(V_i-I\right)R^{(k)}{P}_i$$

然后在最优$T$值的基础上求解旋转矩阵$R$。旋转矩阵$R$的解法为：

1. 对特征点归一化
   $$\begin{array}{l}\bar{P}=\frac{1}{n}\sum _{i=1}^n{P}_i\\ P_i^{{}^{\prime }}=P_i-\bar{P}\\ \bar{Q}\left(R^{\left(k\right)}\right)=\frac{1}{n}\sum _{i=1}^n{Q}_{\mathbf{i}}\left(R^{\left(k\right)}\right)\\ Q_i^{{}^{\prime }}\left(R^{\left(k\right)}\right)=Q_i\left(R^{\left(k\right)}\right)-\bar{Q}\left(R^{\left(k\right)}\right)\end{array}$$

2. 求解M矩阵
   $$M=\sum _{i=1}^{n}P{{}_i}^{{}^{\prime }}{Q}_i^{{}^{\prime }}(R(k){)}^T$$

3. 奇异值分解
   $$M\left(R^{\left(k\right)}\right)=U\sum V^T$$

4. 求解$R$值
   $$R=V{U}^T$$

5. 将R值带入下一帧的T值求解公式中，迭代过程。

注意：
OI算法整体求解流程是非常简单的，公式推导部分比较复杂，但是在仿真时，只需要看重最关键的几个迭代公式即可。
算法部分并不复杂。

Python代码

import numpy as np
import math

def Xform(P, R, t):

    n = P.shape[1]
    Q = np.zeros((3,n))

    for i in range(n):
        Q[:,i] = (np.dot(R, P[:,i].reshape((3,1))) + t).reshape((3,))

    return Q

def abskernel(P, Q, F, tFactor):
    n = P.shape[1]

    # 计算P' 和 Q' 的值
    P1 = P.copy()
    pbar = sum(P.T)/n
    qbar = sum(Q.T)/n
    P = P - pbar.reshape((3,1))
    Q = Q - qbar.reshape((3,1))

    # SVD分解
    # 计算M矩阵
    M = np.zeros((3,3))
    for i in range(n):
        M += np.dot(P[:,i].reshape((3,1)), Q[:,i].reshape((3,1)).T)

    [U,S,V] = np.linalg.svd(M)

    # 计算旋转矩阵R
    R = np.dot(V, U.T)

    # 得到R值后， 计算t值
    Sum = np.zeros((3,1))
    for i in range(n):
        Sum += np.dot(np.dot(F[i]-np.eye(3), R), P1[:,i].reshape((3,1)))

    t = np.dot(tFactor, Sum)

    Qout = Xform(P, R, t)

    # 计算误差
    err2 = 0
    vec = np.zeros((3,1))
    for i in range(n):
        vec = np.dot(np.eye(3)-F[i], Qout[:,i])
        err2 += (vec[0]**2 + vec[1]**2 + vec[2]**2)

    return R, t, Qout, err2



def objpose(P,Q,initR):
    # 初始化
    TOL = 1E-5  # 收敛条件：abs(new_value - old_value) / old_value < tol
    EPSILON = 1E-8  # 目标函数的下界1e - 8
    n = P.shape[1]  # 特征点数量

    # 计算投影矩阵
    F = []  # 沿视线方向的投影矩阵
    V = np.zeros((3,1))  # 归一化图像平面的像点
    for i in range(n):
        V = (Q[:,i]/Q[2,i]).reshape((3,1))
        F.append(np.dot(V, V.T)/np.dot(V.T, V))

    # 计算t所需的第一个矩阵因子
    tFactor = np.linalg.inv(np.eye(3) - sum(F)/n)/n

    # 计算t的最优解
    it = 0  # 初始迭代
    Ri = initR  # Ri的初始值
    Sum = np.zeros((3,1)) # 计算t所需的第二个矩阵银子
    for i in range(n):
        Sum += np.dot(np.dot((F[i] - np.eye(3)), Ri), P[:,i].reshape((3,1)))
    ti = np.dot(tFactor, Sum)  # t的最优解

    # 计算ti条件下的当前误差
    Qi = Xform(P, Ri, ti)
    old_err = 0
    vec = np.zeros((3,1))
    for i in range(n):
        vec = np.dot((np.eye(3) - F[i]), Qi[:,i].reshape((3,1)))
        old_err = old_err + (vec[0,0]**2 + vec[1,0]**2 + vec[2,0]**2)

    Ri, ti, Qi, new_err = abskernel(P, Qi, F, tFactor)
    it += 1

    # 迭代求最优值

    while (abs(old_err - new_err)/old_err > TOL) and (new_err > EPSILON):

        old_err = new_err
        # 计算R的优化估计解
        Ri, ti, Qi, new_err = abskernel(P, Qi, F, tFactor)
        it = it + 1

    R = Ri
    t = ti

    # 方向判断
    if t[2] < 0:
        R = -R
        t = -t

    return R, t





if __name__ == '__main__':

    P = np.array([[-2,0,0],[-2,2,0],[2,0,0],[2,2,0],[0,0,0]]).T # 3D point
    Q = np.array([[-2,0,2],[-2,2,2],[2,0,2],[2,2,2],[0,0,2]]).T# 3D point

    initR = np.eye(3)  # R的初始值
    [R, t] = objpose(P,Q,initR)
    print(R)
    print(t)

结尾

在用正交迭代算法优化位姿的时候，我以为当特征点数量很少时，能够提升OI算法的收敛速度。在实际算法验证的过程中，我发现这种思路是错误的。如果不设置迭代次数的终止条件，当特征点个数少时迭代过程很可能陷入死循环，永远收敛不到误差小于某个小数值的情况。

总结就是，用实际经验教训验证了一个错误的想法。

提示：
设置迭代次数终止条件的代码是，在while循环中增加跳出次数的判断条件：

    # 迭代求最优值
	k_max = **  # 最高迭代次数
    while ((abs(old_err - new_err)/old_err > TOL) and (new_err > EPSILON)) and it<k_max :

        old_err = new_err
        # 计算R的优化估计解
        Ri, ti, Qi, new_err = abskernel(P, Qi, F, tFactor)
        it = it + 1