matlab 在求定标中单映性矩阵

从年前就开始编写一个标定的程序，可是总是在不小心程序改动一点，算出来的内参是复数形式，不知道问题出在哪里，构建求单映性矩阵H的矩阵L的时候，如果图像上角点选取不同就会出现很大的差异，今天终于又调试了很多，看到一篇论文写到，因为矩阵L上数量级相差太大，因为里面有图像平面坐标，数量级是100~1，而世界坐标与图像坐标乘积数量积将在1000~1之间，得到的S矩阵对角元素数量级为10的8次幂。所以对L进行了改造，将图像坐标归一化变换了一下。在下面的normUVfun()函数中；而且今天在计算L矩阵时，选取了图像的中间的两个点，在本例中就是第23个到26的点。

function H=funH(M,m,flag)%g根据读入的信息，求出H,flag是表示第几幅图像的意思；H为单应性矩阵；

%%%%---输入变量 M:为模板齐次坐标，维数为3*npt; 
%%%%           m:图像像素齐次坐标， 维数为：3*npt*flag
%%%            flag:图像个数，几副图像,这里取了三副；
%%%%---输出变量：H,是模板坐标矩阵与图像坐标矩阵之间的单映性矩阵；

%%%%---H应该满足min∑||mi-mi'||的最小平方和；其中mi'表示消去s计算得到图像坐标期望值。
%%%---依据：sm=HM. 消去s计算得到图像坐标期望值：mi'=1／h3*Mi[h1*Mi;h2*Mi] Hi表示H的i行向量；
%%%---[MT 0T -uMT ] [h1']
%     [ 0T MT -vMT ] [h2']=0;
%                    [h3']
                  

[l,npt]=size(M);%npt是模板上有多少个点。
maxone=ones(1,npt);
%M=[M;maxone];
u=m(1,:,flag); %u为三副图像的x坐标，
v=m(2,:,flag); %v三副图像的y的坐标

%构造l矩阵，奇异分解求H.
O=zeros(1,3);
L=zeros(2*npt,9);
MT=M';
%讲实际图像平面坐标变化，因为L中有世界坐标、图像坐标数量级相差太大，所以L矩阵条件数大
[uN,vN,Hinv]=normUVfun(u,v);
UM=zeros(npt,3);

% for i=1:length(u)
%     UM(i,:)=uT(i).*MT(i,:);
% end
% for i=1:length(v)
%     VM(i,:)=vT(i).*MT(i,:);
% end
% 
% L=[M' O' -UM;O' M' -VM]; %L*H=0;求解H，选取n个点，L是2n*9个的矩阵；
%下面需要分解L就出的解为H
% L55=[L(7:12,:);L(49:54,:)];% 一副图像选取了6个点；
% [U,S,V]=svd(L55'*L55);

% L=[M' O' -UM;O' M' -VM]; 
for i=1:length(M)
    L(2*i-1,:)=[MT(i,:) O -uN(1,i)*MT(i,:)];
    L(2*i,:)= [O MT(i,:) -vN(1,i)*MT(i,:)];
    
end
%      -- %L*H=0;求解H，选取n个点，L是2n*9个的矩阵； 
%       --[u s v]=SVD(L'L)
%       --选取中间的4个点计算H，这个四个点表示为(npt-3)行到（npt+4）行；
%       --Lnpt是8*9矩阵；
Lnpt=L(floor(npt)-3:floor(npt)+4,:);
[U S V]=svd(Lnpt'*Lnpt);

[R,M]=size(V);

%HH(:,:)=V(:,M)./V(L,M);%H的解就是 V的最后一列，并且把最后一个元素变成1。
HH=V(:,M);
Hnorm=reshape(HH,3,3);
%空间平面与实际图像坐标平面的单映性矩阵H:
H=Hnorm*Hinv;

H=H/H(3,3);

下面是归一化变换函数

%
function [uN,vN,Hinv]=normUVfun(u,v)     
%%%%%%function:将图像坐标归一化处理，采用非方式处理两个坐标轴上的数据，缩放比例不相等，但经处理后使各点到原点距离的平均值为1.
%       平均值：mu=mean(ui),mv=mean(vi),
%       各个点相对应平均值的相对量：du=ui-mu,
%                                 dv=vi=mv;
%       求u,v轴的缩放因子：su=1/mean(abs(dui)),
%                         sv=1/mean(abs(dvi))
%       总的变换关系：uN=dui/mean(abs(dui)),
%                    vN=dvi=/mean(abs(dvi))
%     --输入变量(u,v)
%     --输出为归一化后图像坐标（uN,vN)和反变换矩阵Hinv
%%%-----------author:liyingyan-----------------------------
%%%---------Date:2009.03.13--------------------------

[r,npt]=size(u);
du=zeros(r,npt);
dv=zeros(r,npt);
mu=mean(u);
mv=mean(v);
for i=1:npt
    du(i)=abs(u(i)-mu);
    dv(i)=abs(v(i)-mv);
end
su=1/mean(du);
sv=1/mean(dv);
uN=du*su;
vN=dv*sv;
%m=Hinv*mN--变换后的图像平面坐标需要与一个反变换矩阵相乘猜得到实际图像平面坐标；
Hinv=[1/su 0 mu;0 1/sv mv;
    0 0 1];