昨天完成了用最小二乘法進行相機標定的實驗。

1. 最小二乘法進行相機標定的原理

1.1 座標系變換

將實物拍到相機的膠片上需要進過幾個座標系的變換：

世界座標系—>相機座標系—成像座標系—>畫素座標系

在座標系變換的過程中可以看做是將變換矩陣線性作用於齊次向量

從世界座標系變換到相機座標系：

其中為世界座標系下的座標，為相機座標系下的座標，為畫素座標系下的座標。

那麼可以得到：

我們已知世界座標系下的座標和畫素座標下的座標，求解矩陣M中的未知量就是對相機進行標定。

1.2 最小二乘法求解

用最小二乘法進行求解就是要使的U，那麼就要求達到最小，就是要求：

所以只要求解最小特徵值對應的特徵向量，就可以求解出最優解。

2.程式碼

[M,N]=size(P);
%矩陣A
for i=1:N
    A(2*i-1,1:4)=[P(1:3,i);1]';
    A(2*i-1,5:8)=[0 0 0 0];
    A(2*i-1,9:12)=-P(4,i)*[P(1:3,i);1]';
    A(2*i,1:4)=[0 0 0 0];
    A(2*i,5:8)=[P(1:3,i);1]';
    A(2*i,9:12)=-P(4,i)*[P(1:3,i);1]';
end
AA=A'*A;
[V D]=eig(AA);
%求解特徵值和特徵向量
a(1,1:3)=V(1,1:3);b(1,1)=V(1,4);[M,N]=size(P);
%求得最小的特徵值對應的特徵向量
a(1,1:3)=V(1,1:3);b(1,1)=V(1,4);
a(2,1:3)=V(1,5:7);b(2,1)=V(1,8);
a(3,1:3)=V(1,9:11);b(3,1)=V(1,12);
e=1;
%ε=1
p=e/norm(a(3,:));
% ρ=ε/||a3||
r(3,:)=p*a(3,:);
%r3=p* a3
u0=p^2*a(1,:)*(a(3,:))';
%u0=P2*a1*a3
v0=p^2*a(2,:)*(a(3,:))'; 
%v0=P2*a2*a3
cos_zeta=-(cross(a(1,:),a(3,:))*(cross(a(2,:),a(3,:)))')/(norm(cross(a(1,:),a(3,:)))*norm(cross(a(2,:),a(3,:))));
%cos(ө)=-(（a1×a3）‘*（a2×a3）)/(|(|a2*a3|)|*||a1×a3||)
zeta=acos(cos_zeta);
alpha=p^2*norm(cross(a(1,:),a(3,:)))*sin(zeta);
%α=ρ2 ||a1×a3||
beta=p^2*norm(cross(a(2,:),a(3,:)))*sin(zeta); 
%β=ρ2 ||a2×a3||
t_x=p/alpha*(b(1)+alpha/(2*beta)*sin(2*zeta)*b(2)-(alpha/(2*beta)*sin(2*zeta)*v0+u0)*b(3));
%t_x=ρ/α*(b1+α/(2ρ)*sin(2ө)*b2-b3*(α/(2β)*sin(2ө)*v0+u0)
t_y=p*sin(zeta)/beta*(b(2)-v0*b(3));
%t_y=ρ*sin(ө)/β*(b2-v0b3)
t_z=p*b(3);
%t_z=ρ*b3