【实验室：三维重构】已知三维点坐标和对应点图像坐标，求摄像机矩阵P

实验室三维重构的一个步骤：已知三维点坐标和对应点图像坐标，求摄像机矩阵P

算法描述在《计算机视觉中的多视图几何》P123

归一化后用DLT求初值。方法是将3*4的矩阵P改写为12*1矩阵（直接每行转置后竖着排列起来）。

x1 y1 z1是三维点的坐标，pt3是图像上对应点的坐标，K是已标定好的内参

function P3=newtonGetP3(x1,y1,z1,pt3,K)

XYZ=[x1';y1';z1';ones(1,size(x1,1))];% 物点坐标
uv=[pt3';ones(1,size(pt3,1))];% 像点坐标
A1=[x1 y1 z1 ones(size(x1,1),1);
    zeros(size(x1,1),4)];% 前四列
A2=[zeros(size(x1,1),4);
    x1 y1 z1 ones(size(x1,1),1)];% 中间四列
A3=[-x1.*pt3(:,1) -y1.*pt3(:,1) -z1.*pt3(:,1) -pt3(:,1)
    -x1.*pt3(:,2) -y1.*pt3(:,2) -z1.*pt3(:,2) -pt3(:,2)];% 最后四列
b=[pt3(:,1);pt3(:,2)];
A=[A1 A2 A3 ];
P3=A\b;
err0=mean(abs(A*P3-b));
% A0=A;b0=b;
P3=[P3(1:4)';P3(5:8)';P3(9:11)',P3(12)];% 初始值 最后一位是1因为相差一个尺度
rt=K\P3;
% P3是相差一个尺度因子 故rt也相差了一个尺度因子

syms x y z t1 t2 t3
rt_2 = [cos(y)*cos(z), sin(x)*sin(y)*cos(z)-sin(z),cos(x)*sin(y)*cos(z)-sin(x)*sin(y),t1
       sin(y)*sin(z), sin(x)*sin(y)*sin(z)+cos(x)*cos(z), cos(x)*sin(y)*sin(z)+sin(x)*cos(z) t2
       -sin(y), sin(x)*cos(y), cos(x)*cos(y) t3];
b=asin(-rt(3,1));c=asin(-rt(2,1)/rt(3,1));a=asin(rt(3,2)/cos(b));
res=[ a b c rt(1,4) rt(2,4) rt(3,4)];% 初值
 
fun1=K*rt_2*XYZ;
E=reshape([fun1(1,:)-uv(1,:).*fun1(3,:);fun1(2,:)-uv(2,:).*fun1(3,:)],[size(uv,2)*2,1]);
Jac=[diff(E,x) diff(E,y) diff(E,z) diff(E,t1) diff(E,t2) diff(E,t3)];
err_or2=mean(abs(subs(E,[x y z t1 t2 t3],res)));
num=0;
disp(strcat('初始值方程组残差',num2str(err_or2)))
disp(strcat('初始值摄像机矩阵残差',num2str(mean(abs(err0)))))
while 1
   err=subs(E,[x y z t1 t2 t3],res);
   step=subs(Jac,[x y z t1 t2 t3],res)\err;
   res=res-step';
   num=num+1;
    if  num>30 || mean(abs(err))<0.01*err_or2
           % || mean(abs(err))>mean(abs(err_or2))
         break;
    end
end
disp(strcat('牛顿法求得方程组残差',num2str(mean(abs(err)))))
rt=subs(rt_2,[x y z t1 t2 t3],res);
P3=K*rt;
P3_2=[P3(1,:)';P3(2,:)';P3(3,:)']/P3(3,4);
%disp(strcat('牛顿法求摄像机矩阵残差',num2str(mean(abs(A0*P3_2(1:11)-b0 )))))
end

求得初值后，将x-PX改写成方程的形式，然后使用牛顿迭代法。

2015.7.20 修改后的程序 运行速度快很多

function [ P,err_2 ] = genCameraMatrixBy3DPoints_0609(XYZ,uv,K )
% 根据物点和像点坐标以及内参值，迭代优化得到摄像机矩阵P
% XYZ: n*4 物点坐标
% uv： n*3 像点坐标
% K： 内参
% err_2 残差

uv = double(uv);
XYZ = double(XYZ);
uv_t=uv(:,:);% 保存未被修改的像点坐标
XYZ_t=XYZ(:,:);%  保存未被修改的物点坐标

% 归一化
[uv,T] = normalize(uv');
[XYZ,U] = normalize(XYZ');
uv = uv';
XYZ = XYZ';

%% DLT
A1 = [  zeros(size(XYZ,1),4)
    XYZ];
A2 = [  -XYZ
    zeros(size(XYZ,1),4)];
A3 = [ uv(:,2).*XYZ(:,1),uv(:,2).*XYZ(:,2),uv(:,2).*XYZ(:,3),uv(:,2).*XYZ(:,4)
    -uv(:,1).*XYZ(:,1),-uv(:,1).*XYZ(:,2),-uv(:,1).*XYZ(:,3),-uv(:,1).*XYZ(:,4)];
A=[A1 A2 A3];

[~,~,P3] = svd(A);
P3 = P3(:,end);
P3 = [P3(1:4)';P3(5:8)';P3(9:12)'];% 初始值
P3_norm = P3;
P3 = T\P3_norm*U;

rt3 = K\P3;
[uu,~,vv] = svd(rt3(1:3,1:3));

if det(uu*vv')<0
    rt3 = [-uu*vv',rt3(:,end)];
    % rt3=[-uu*vv',rt3(:,end)./(dd(1,1)*dd(2,2)*dd(3,3))];
else
    rt3 = [uu*vv',rt3(:,end)];
    %      rt3=[uu*vv',rt3(:,end)./(dd(1,1)*dd(2,2)*dd(3,3))];
end

b = asin(-rt3(2,3));
c = asin(rt3(2,1)/cos(b));
a = asin(-rt3(1,3)/cos(b));
res = [a b c rt3(:,end)'];% 摄像机矩阵初值`


getP_fun = @(res)errCal(res, XYZ_t, uv_t, K);
opt = optimset('LargeScale','off','Display','off','TolFun',1e-30,...
    'TolX',1e-35,'Algorithm','levenberg-marquardt','MaxFunEvals',1e+100,'MaxIter',1000);
[res_2, err_2] = lsqnonlin(getP_fun, res, [], [], opt);

x = res_2(1);
y = res_2(2);
z = res_2(3);
t1 =res_2(4);
t2 =res_2(5);
t3 =res_2(6);
Rx = [cos(x) 0 -sin(x)
    0    1  0
    sin(x) 0  cos(x)];
Ry = [1 0 0;0 cos(y) -sin(y);0 sin(y) cos(y)];
Rz = [cos(z) -sin(z) 0;sin(z) cos(z) 0; 0 0 1];
R = Rx*Ry*Rz;
P = K*[R,[t1 t2 t3]'];

end