舊日誌整理：MATLAB標定工具箱的使用（三）

在進行攝像機標定過程中，有一些衍生問題在這裏總結一下

  

  

基於SVD特徵值分解的超定方程求解

function x=SvdEquation(A,b)
%用奇異值分解求解病態線性方程組
%A爲係數矩陣,b爲右向量
r=rank(A);
n=length(A);
epsilon=1e-10;
x=zeros(n,1);
[U,S,V]=svd(A);
U1=U(:,1:r);
V1=V(:,1:r);
isig=inv(S(1:r,1:r));
x=V1*isig*U1'*b;

 

旋轉矩陣和旋轉參數的相互轉化

function R=getR(fai,ksai,tao)
% 由旋轉參數計算旋轉矩陣
R=zeros(3,3);
R(1,1)=cos(fai)*cos(tao);
R(2,1)=-cos(fai)*sin(tao);
R(3,1)=sin(fai);
R(1,2)=cos(ksai)*sin(tao)+sin(ksai)*sin(fai)*cos(tao);
R(2,2)=cos(ksai)*cos(tao)-sin(ksai)*sin(fai)*sin(tao);
R(3,2)=-sin(ksai)*cos(fai);
R(1,3)=sin(ksai)*sin(tao)-cos(ksai)*sin(fai)*cos(tao);
R(2,3)=sin(ksai)*cos(tao)+cos(ksai)*sin(fai)*sin(tao);
R(3,3)=cos(ksai)*cos(fai);

 

function Re=getAngle(R)
% 得到旋轉矩陣的旋轉參數
fai(1)=asin(R(3,1));
fai(2)=pi-fai(1);
ksai(1)=asin(R(3,2)*(-1)/cos(fai(1)));
ksai(2)=pi-ksai(1);
ksai(3)=pi+ksai(1);
ksai(4)=-ksai(1);
tao(1)=asin(R(2,1)*(-1)/cos(fai(1)));
tao(2)=pi-tao(1);
tao(3)=pi+tao(1);
tao(4)=-tao(1);
out=zeros(1,3);
fai
tao
ksai
for i=1:2
    for j=1:4
        for k=1:4
R1=getR(fai(i),ksai(j),tao(k));
if i==1&&j==1&&k==1
    mind=sum(sum(abs(R1-R)));
      out=[i j k];
else
    if i==2&&j==2
        R1;
    end
if sum(sum(abs(R1-R)))<mind
    out=[i j k];
    mind=sum(sum(abs(R1-R)));
end
end
end
end
end
out
Re=[fai(out(1)),ksai(out(2)),tao(out(3))];
getR(fai(out(1)),ksai(out(2)),tao(out(3)))

 

function X= Reconstruction(Pt1,Pt2,P1,P2) 
%通過兩幅圖像中的匹配點重建該匹配點在世界座標系的座標
%輸入說明
%   Pt1   第一幅圖像中匹配點的像素座標(2列)
%   Pt2   第二幅圖像中匹配點的像素座標（2列）
%   P1    第一幅圖像的射影矩陣（3*4）
%   P2    第二幅圖像的射影矩陣

Np=size(Pt1,1);
X=zeros(Np,4);
for i=1:Np
A=zeros(4,4);
A(1,:)=P1(1,:)-Pt1(i,1)*P1(3,:);
A(2,:)=P1(2,:)-Pt1(i,2)*P1(3,:);
A(3,:)=P2(1,:)-Pt2(i,1)*P2(3,:);
A(4,:)=P2(2,:)-Pt2(i,2)*P2(3,:);
T= MinEigVector ( A'*A );%求最小特徵值對應的特徵向量
X(i,:)=(T/T(4))';
end

 

返回一個矩陣的最小特徵值對應的特徵向量

  function v = MinEigVector ( M )
 % 返回一個矩陣的最小特徵值對應的特徵向量
   [ P,D ]=eig( M );
   r=size(M,1);
   min=1;
   for i = 2:r
       if D(i,i)<D(min,min)
           min=i;
       end
   end
   v = P(:,min); 
   if v(1)<0
       v=-v;
   end;