在进行摄像机标定过程中,有一些衍生问题在这里总结一下
基于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;