matlab練習程序（DLT）

在計算位姿的時候，一般我們有一些觀測量，這些觀測量有些是三維的、有些是二維的，因此需要用到不同的方法。

如果是3D-3D的位姿計算，一般可以用這幾種方法（【1】,【2】,【3】,【4】）。

如果是3D-2D的位姿計算，一般可以用PnP-BA或者是本篇的DLT（直接線性變換）方法。

如果是2D-2D的位姿計算，一般可以用對極幾何的方法，不過算不出尺度。

三維點投影到歸一化二維平面可以用下面公式表示：

其中uv是二維歸一化平面點，XYZ是三維投影點。

整理後可得AX=0這種形式，X就是公式中的12個l，A矩陣形式如下：

對於AX=0這種問題，一般都是對A奇異值分解或者對A'A特徵值分解，然後拿其最小特徵值對應的特徵向量，作爲該問題的解。

matlab代碼如下：

clear all;close all;clc;

p = [rand(100,3)*100 ones(100,1)]';

i=1.2*pi/180.0;
j=88*pi/180.0;
k=-5.1*pi/180.0;
Rx=[1 0 0;0 cos(i) -sin(i); 0 sin(i) cos(i)];
Ry=[cos(j) 0 sin(j);0 1 0;-sin(j) 0 cos(j)];
Rz=[cos(k) -sin(k) 0;sin(k) cos(k) 0;0 0 1];
R=Rz*Ry*Rx;
T = [200;40;50];

srcR = [R T;0 0 0 1]

newp = inv(srcR)*p;
uv =[newp(1,:)./newp(3,:); newp(2,:)./newp(3,:)];

xyz = p(1:3,:)';

[newR, err] = DLTcalib(xyz, uv')

function [Tr, x] = Normalization(nd, x)

[m, s] = deal(mean(x, 1), std(x));

if nd == 2
    Tr = [s(1), 0, m(1);
        0, s(2), m(2);
        0, 0, 1];
else
    Tr = [s(1), 0, 0, m(1);
        0, s(2), 0, m(2);
        0, 0, s(3), m(3);
        0, 0, 0, 1];
end

Tr = inv(Tr);
x = (Tr * [x'; ones(1, size(x, 1))]).';
x = x(:, 1:nd);

end

function [H, err] = DLTcalib(xyz, uv)
n = size(xyz, 1);

[Txyz, xyzn] = Normalization(3, xyz);
[Tuv, uvn] = Normalization(2, uv);

A = zeros(2*n, 12);

for i = 1:n
    x = xyzn(i, 1);
    y = xyzn(i, 2);
    z = xyzn(i, 3);
    u = uvn(i, 1);
    v = uvn(i, 2);
    A(2*i-1, :) = [x, y, z, 1, 0, 0, 0, 0, -u*x, -u*y, -u*z, -u];
    A(2*i, :) = [0, 0, 0, 0, x, y, z, 1, -v*x, -v*y, -v*z, -v];
end

[~, ~, V] = svd(A);

% Camera projection matrix
H = reshape(V(:,end), [4, 3])';

% Denormalization
H = pinv(Tuv) * H * Txyz;

H = inv([H;0 0 0 1]);
H(1:3,1:3) = H(1:3,1:3)./norm(H(1:3,1:3));

uv2 = (inv(H) * [xyz'; ones(1, n)])';
uv2 = uv2(:, 1:2) ./ uv2(:, 3);

% Mean distance:
err = sqrt(mean(sum((uv2 - uv).^2, 2)));

end

可以看出計算得到的位姿和最初設置的位姿是一樣的。

不過該方法計算時沒有考慮旋轉矩陣的約束，即A'A=I這種條件，因此計算的到的旋轉量可能會有問題。

PnP-BA這種方法是不存在這種問題的。