旧日志整理：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;