位姿测量 | 正交迭代（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\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}} }$

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

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

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

对特征点归一化
$\begin{array}{l} \bar P = \frac{1}{n}\sum\limits_{i = 1}^n { {P_i}} \\ P_i^{'} = {P_i} - \bar P\\ \bar Q\left( { {R^{\left( k \right)}}} \right) = \frac{1}{n}\sum\limits_{i = 1}^n { {Q_{\bf{i}}}\left( { {R^{\left( k \right)}}} \right)} \\ Q_i^{'}\left( { {R^{\left( k \right)}}} \right) = {Q_i}\left( { {R^{\left( k \right)}}} \right) - \bar Q\left( { {R^{\left( k \right)}}} \right) \end{array}$
求解M矩阵
$M=\sum\limits_{i = 1}^n { {P{_i}^{'}Q_i^{'}(R(k))^T}}$
奇异值分解
$M\left( { {R^{\left( k \right)}}} \right) = U\sum {V^T}$
求解 $R$ 值
$R = VU^T$
将R值带入下一帧的T值求解公式中，迭代过程。

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

MATLAB程序



function OI()

% 输入参数
P = [-2,0,0;-2,2,0;2,0,0;2,2,0;0,0,0]'; % 3D point
Q = [-2,0,2;-2,2,2;2,0,2;2,2,2;0,0,2]'; % 3D point

% OI算法
initR = [1,0,0;0,1,0;0,0,1]; % R的初始值
[R, t] = ObjPose(P,Q,initR);
R % 估计旋转矩阵
t % 估计平移矩阵




function [R, t] = ObjPose(P, Q, initR)

% 初始化
TOL = 1E-5; % 收敛条件：abs(new_value-old_value)/old_value<tol
EPSILON = 1E-8; % 目标函数的下界1e-8

n = size(P,2); % 特征点数量

% 计算投影矩阵
F(1:3,1:3,1:n) = 0; % 沿视线方向的投影矩阵
V(1:3) = 0; % 归一化图像平面的像点
for i = 1:n
    V = Q(:,i)/Q(3,i); % vi = (RPi+T)/(r3Pi+tz)
    F(:,:,i) = (V*V.')/(V.'*V);
end

% 计算t所需的第一个矩阵因子
tFactor = inv(eye(3)-sum(F,3)/n)/n;

% 计算t的最优解
it = 0; % 初始迭代
Ri = initR; % Ri的初始值
Sum(1:3,1) = 0; % 计算t所需的第二个矩阵因子
for i = 1:n
    Sum = Sum + (F(:,:,i)-eye(3))*Ri*P(:,i);
end
ti = tFactor*Sum; % t的最优解

% 计算ti条件下的当前误差
Qi = xform(P, Ri, ti); % 在给定P和Ri|ti条件下的Qi值
old_err = 0;
vec(1:3,1) = 0; % 目标函数E(R,T)
for i = 1 : n
    vec = (eye(3)-F(:,:,i))*Qi(:,i); % RPi+T - Vi*(RPi+T)
    old_err = old_err + dot(vec,vec);
end

% 计算当前时刻的姿态估计值

[Ri, ti, Qi, new_err] = calculateR(P, Qi, F, tFactor);
it = it + 1;

% 迭代求最优值
while (abs((old_err-new_err)/old_err) > TOL) && (new_err > EPSILON)

    old_err = new_err;

    % 计算R的优化估计解
    [Ri, ti, Qi, new_err] = calculateR(P, Qi, F, tFactor);
    it = it + 1;

end

R = Ri;
t = ti;

if t(3) < 0
    R = -R;
    t = -t;
end


function [R, t, Qout, err2] = calculateR(P, Q, F, tFactor)
%% 初始值
n = size(P,2);
%% 计算P'和Q'的值
P1 = P;
pbar = sum(P,2)/n;
qbar = sum(Q,2)/n;
for i = 1:n
  P(:,i) = P(:,i)-pbar;
  Q(:,i) = Q(:,i)-qbar;
end

%% SVD 分解
% 计算M矩阵
M(1:3,1:3) = 0;
for i = 1:n
    M = M+P(:,i)*Q(:,i).';
end

% SVD分解求U和V
[U,S,V] = svd(M);

% 计算旋转矩阵R
R = V*(U.');

%% 得到R值后，计算t值
Sum(1:3,1) = 0; % 计算t所需的第二个矩阵因子
for i = 1:n
  Sum = Sum + (F(:,:,i)-eye(3))*R*P1(:,i);
end
t = tFactor*Sum; % T的最优解
Qout = xform(P, R, t); % Qout = RP+t

%% 计算误差
err2 = 0;
vec(1:3,1) = 0;
for i = 1 : n
  vec = (eye(3)-F(:,:,i))*Qout(:,i); % RPi+T - Vi*(RPi+T)
  err2 = err2 + dot(vec,vec);
end

结尾

本文介绍的程序为MATLAB版本，十年前的航空领域哪怕是图像算法也多用MATLAB来实现，近年来用C++ 和python的研究人员才开始躲起来。以后有机会再更新OI算法的python版本。