【计算机视觉】直接线性变换（DLT）求解P矩阵（3 加入坐标的归一化）（附MATLAB代码）

引言

本来上一篇帖子就已经达到了精度要求，不过经过同学提醒才发现老师的作业要求中有要求考虑归一化。
emmmmm

坐标归一化

进行归一化的必要性和方法参考 《计算机视觉中的多视几何》中的描述：


上面的是从 2D到2D的结论，不过与从3D到2D的结论是相似的：

总结一下，就是在计算之前，要将空间点和平面点都进行归一化，空间点要归一化到距离原点的平均距离为√3，平面点要归一化到距离原点的平均距离为√2。在计算完过后在进行去归一化。其他的计算过程和上一篇帖子讲的完全相同。
如何将点位归一化到这种状态可以参考这篇帖子：对三维点集的归一化变换

MATLAB代码

%%
clc;clear;close all;
%% DLT 算法需要用到 6 对点 ，前三列是 X Y Z 表示空间坐标 ，后两列 x y 表示图像坐标,下面是测试数据
Points = [  99.1517085791261, -0.892099762666786, 3.06159510601795, 163.209290388816, 182.199675950568;
		100.558334460204, 0.868021368458366, 2.25981241694746, 608.892566967667, 701.375883991155;
		100.137647321744, -0.0612187178835884, 2.02380413900248, 449.262241640596, 377.206100107244;
		99.6742452887978, -0.675635383613515, 3.58856908136781, 316.188427147902, 257.277246206713;
		99.6224300840896, 0.0570662710124255, 2.33129745899956, 267.575494465364, 430.054554508315;
		100.203963882803, -0.474057430919711, 3.30815819695356, 439.688462560083, 287.229433675101]';
P3d = Points(1:3,:);
P2d = Points(4:5,:);

% 转为齐次坐标
P3d_ = cat(1,P3d,[1,1,1,1,1,1]);
P2d_ = cat(1,P2d,[1,1,1,1,1,1]);

%  求归一化的缩放矩阵
T1 = normalize3d(P3d);
T2 = normalize2d(P2d);

nP3d_ = T1*P3d_;
nP2d_ = T2*P2d_;

% 赋值 有6 个点，所以C的大小为 12×12,
C = zeros(12,12);
for i = 1:6
    X = nP3d_(1,i);Y = nP3d_(2,i);Z = nP3d_(3,i);x = nP2d_(1,i);y = nP2d_(2,i);
    C(i*2-1,:) = [X,Y,Z,1,0,0,0,0,-x*X,-x*Y,-x*Z,-x];
    C(i*2,:)   = [0,0,0,0,X,Y,Z,1,-y*X,-y*Y,-y*Z,-y];
    
end

[U,S,V] = svd(C);

L = V(:,end);

P_ = reshape(L,[4,3])';

% 去归一化
P = inv(T2)*P_*T1;

P = P/P(3,3);

%% 验证
Point3d = ones(4,6);
Point3d(1:3,:) = Points(1:3,:);

Point2d = Points(4:5,:);

Res = P*Point3d;

Res2d = Res(1:2,:)./Res(3,:);

disp(Point2d)
disp(Res2d)

fprintf("重投影误差：\n");
diff = Point2d - Res2d;
disp(diff);



function [T] = normalize3d(w)
    % 变换目标：中心在原点且到原点的平均距离为sqrt(3)
    if  size(w,1)<size(w,2)
        w = w';
    end
    t = -mean(w(:,1:3),1);
    w = w(:,1:3) + repmat(t,[size(w,1),1]);
    dis = mean(sqrt(w(:,1).*w(:,1)+w(:,2).*w(:,2)+w(:,3).*w(:,3)));
    v = sqrt(3)/dis;
    t = t'*v;
    T = [eye(3)*v,t];
    T = [T;0,0,0,1];
end

function [T] = normalize2d(w)
    % 变换目标：中心在原点且到原点的平均距离为sqrt(2)
    if  size(w,1)<size(w,2)
        w = w';
    end
    t = -mean(w(:,1:2),1);
    w = w(:,1:2) + repmat(t,[size(w,1),1]);
    dis = mean(sqrt(w(:,1).*w(:,1)+w(:,2).*w(:,2)));
    v = sqrt(2)/dis;
    t = t'*v;
    T = [eye(2)*v,t];
    T = [T;0,0,1];
end

运行结果

结语

可以看出，相较于没有进行归一化（上一篇帖子）的结果，精度确实得到了提高。
至此，整个DLT便算是彻底完成了！