funmatdemotest.m

计算基本矩阵的matlab例程

            bars=[bars;meantorr_estf,stddevtorr_estf];
            bartime=[bartime;timetorr_estf];
            xlabelbars=[xlabelbars sprintf(' %d.- torr estf ',indexbars)];
            if mod(indexbars,8)==0, xlabelbars=[xlabelbars '\n']; end
            indexbars=indexbars+1;
            
        case 17,
            disp('Method: bookstein implemented by Torr')
            tic
            [f, f_sq_errors, n_inliers,inlier_index,Ftorr_estf_bookstein] = torr_estimateF( M', 1, [], 'bookstein', 0);
            timetorr_estf_bookstein=toc;
            Ftorr_estf_bookstein=T1'*Ftorr_estf_bookstein'*T2;
            Ftorr_estf_bookstein=Ftorr_estf_bookstein./norm(Ftorr_estf_bookstein);
            [meantorr_estf_bookstein,stddevtorr_estf_bookstein,minimtorr_estf_bookstein,maximtorr_estf_bookstein]=funmatError(Mini,Ftorr_estf_bookstein);
            disp('Funamental Matrix')
            disp(Ftorr_estf_bookstein)
            disp(sprintf('Distance point-epipolar line\nmean: %f  stdev: %f  min: %f  max: %f\n',meantorr_estf_bookstein,stddevtorr_estf_bookstein,minimtorr_estf_bookstein,maximtorr_estf_bookstein));
            disp(sprintf('Rank-2: %d',rank(Ftorr_estf_bookstein)==2))            
            disp(sprintf('Time: %f',timetorr_estf_bookstein))            
            disp('*******************************************************************')
            if redraw, 
                fummatplot(Mini,Ftorr_estf_bookstein)   
            end
            bars=[bars;meantorr_estf_bookstein,stddevtorr_estf_bookstein];
            bartime=[bartime;timetorr_estf_bookstein];
            xlabelbars=[xlabelbars sprintf(' %d.- torr estf bookstein ',indexbars)];
            if mod(indexbars,8)==0, xlabelbars=[xlabelbars '\n']; end
            indexbars=indexbars+1;
            
        case 18,
            disp('Method: bookstein+sampson implemented by Torr')
            tic
            [f, f_sq_errors, n_inliers,inlier_index,Ftorr_estf_bookstein_sam] = torr_estimateF( M', 1, [], 'b+sampson', 0);
            timetorr_estf_bookstein_sam=toc;
            Ftorr_estf_bookstein_sam=T1'*Ftorr_estf_bookstein_sam'*T2;
            Ftorr_estf_bookstein_sam=Ftorr_estf_bookstein_sam./norm(Ftorr_estf_bookstein_sam);
            [meantorr_estf_bookstein_sam,stddevtorr_estf_bookstein_sam,minimtorr_estf_bookstein_sam,maximtorr_estf_bookstein_sam]=funmatError(Mini,Ftorr_estf_bookstein_sam);
            disp('Funamental Matrix')
            disp(Ftorr_estf_bookstein_sam)
            disp(sprintf('Distance point-epipolar line\nmean: %f  stdev: %f  min: %f  max: %f\n',meantorr_estf_bookstein_sam,stddevtorr_estf_bookstein_sam,minimtorr_estf_bookstein_sam,maximtorr_estf_bookstein_sam));
            disp(sprintf('Rank-2: %d',rank(Ftorr_estf_bookstein_sam)==2))            
            disp(sprintf('Time: %f',timetorr_estf_bookstein_sam))            
            disp('*******************************************************************')
            if redraw, 
                fummatplot(Mini,Ftorr_estf_bookstein_sam)   
            end
            bars=[bars;meantorr_estf_bookstein_sam,stddevtorr_estf_bookstein_sam];
            bartime=[bartime;timetorr_estf_bookstein_sam];
            xlabelbars=[xlabelbars sprintf(' %d.- torr estf bookstein sam ',indexbars)];
            if mod(indexbars,8)==0, xlabelbars=[xlabelbars '\n']; end
            indexbars=indexbars+1;
            
        case 19,
            disp('Method: linear+non linear implemented by Torr')
            tic
            [f, f_sq_errors, n_inliers,inlier_index,Ftorr_estf_lin_non_lin] = torr_estimateF( M', 1, [], 'lin+non_lin', 0);
            timetorr_estf_lin_non_lin=toc;
            Ftorr_estf_lin_non_lin=T1'*Ftorr_estf_lin_non_lin'*T2;
            Ftorr_estf_lin_non_lin=Ftorr_estf_lin_non_lin./norm(Ftorr_estf_lin_non_lin);
            [meantorr_estf_lin_non_lin,stddevtorr_estf_lin_non_lin,minimtorr_estf_lin_non_lin,maximtorr_estf_lin_non_lin]=funmatError(Mini,Ftorr_estf_lin_non_lin);
            disp('Funamental Matrix')
            disp(Ftorr_estf_lin_non_lin)
            disp(sprintf('Distance point-epipolar line\nmean: %f  stdev: %f  min: %f  max: %f\n',meantorr_estf_lin_non_lin,stddevtorr_estf_lin_non_lin,minimtorr_estf_lin_non_lin,maximtorr_estf_lin_non_lin));
            disp(sprintf('Rank-2: %d',rank(Ftorr_estf_lin_non_lin)==2))            
            disp(sprintf('Time: %f',timetorr_estf_lin_non_lin))            
            disp('*******************************************************************')
            if redraw, 
                fummatplot(Mini,Ftorr_estf_lin_non_lin)   
            end
            bars=[bars;meantorr_estf_lin_non_lin,stddevtorr_estf_lin_non_lin];
            bartime=[bartime;timetorr_estf_lin_non_lin];
            xlabelbars=[xlabelbars sprintf(' %d.- torr estf lin non lin ',indexbars)];
            if mod(indexbars,8)==0, xlabelbars=[xlabelbars '\n']; end
            indexbars=indexbars+1;
            
        case 20,
            disp('Method: seven points implemented by Torr')
            tic
            [nf,f]=torr_F_constrained_fit(M(1,1:7),M(2,1:7),M(3,1:7),M(4,1:7),1);
            timetorr_F_constrained_fit=toc;
            Ftorr_F_constrained_fit=[f(nf,1) f(nf,2) f(nf,3); f(nf,4) f(nf,5) f(nf,6); f(nf,7) f(nf,8) f(nf,9)];
            Ftorr_F_constrained_fit=T1'*Ftorr_F_constrained_fit'*T2;
            Ftorr_F_constrained_fit=Ftorr_F_constrained_fit./norm(Ftorr_F_constrained_fit);
            [meantorr_F_constrained_fit,stddevttorr_F_constrained_fit,minimtorr_F_constrained_fit,maximtorr_F_constrained_fit]=funmatError(Mini,Ftorr_F_constrained_fit);
            disp('Funamental Matrix')
            disp(Ftorr_F_constrained_fit)
            disp(sprintf('Distance point-epipolar line\nmean: %f  stdev: %f  min: %f  max: %f\n',meantorr_F_constrained_fit,stddevttorr_F_constrained_fit,minimtorr_F_constrained_fit,maximtorr_F_constrained_fit));
            disp(sprintf('Rank-2: %d',rank(Ftorr_F_constrained_fit)==2))            
            disp(sprintf('Time: %f',timetorr_F_constrained_fit))            
            disp('*******************************************************************')
            if redraw, 
                fummatplot(Mini,Ftorr_F_constrained_fit)   
            end
            bars=[bars;meantorr_F_constrained_fit,stddevttorr_F_constrained_fit];
            bartime=[bartime;timetorr_F_constrained_fit];
            xlabelbars=[xlabelbars sprintf(' %d.- torr F constrained fit ',indexbars)];
            if mod(indexbars,8)==0, xlabelbars=[xlabelbars '\n']; end
            indexbars=indexbars+1;
            
        case 21,
            disp('Method: MLESAC implemented by Torr')
            tic
            [f, f_sq_errors, n_inliers,inlier_index,Fmlesac] = torr_estimateF( M', 1, [round(log(1-0.99)/log(1-(1-0.25)^7)) 0.0039], 'mlesac', 0);
            [f,f_sq_errors] = torr_estimateF(M(:,inlier_index)', 1, [], 'non_linear',1,f);
            timeMLESAC=toc;
            Fmlesac=reshape(f,3,3);
            Fmlesac=T1'*Fmlesac*T2;
            Fmlesac=Fmlesac./norm(Fmlesac);
            [meanmlesac,stddevmlesac,minimmlesac,maximmlesac]=funmatError(Mini,Fmlesac);
            disp('Funamental Matrix')
            disp(Fmlesac)
            disp(sprintf('Distance point-epipolar line\nmean: %f  stdev: %f  min: %f  max: %f\n',meanmlesac,stddevmlesac,minimmlesac,maximmlesac));
            disp(sprintf('Rank-2: %d',rank(Fmlesac)==2))            
            disp(sprintf('Outliers: %d/%d',size(M,2)-size(Mini(:,inlier_index),2),size(M,2)))
            disp(sprintf('Time: %f',timeMLESAC))            
            disp('*******************************************************************')
            if redraw, 
                fummatplot(Mini,Fmlesac)   
            end
            redrawnow=1;
            Mfi=Mini(:,inlier_index);
            bars=[bars;meanmlesac,stddevmlesac];
            bartime=[bartime;timeMLESAC];
            xlabelbars=[xlabelbars sprintf(' %d.- MLESAC ',indexbars)];
            if mod(indexbars,8)==0, xlabelbars=[xlabelbars '\n']; end
            indexbars=indexbars+1;
            
        case 22,
            disp('Method: MAPSAC implemented by Torr')
            tic
            [f, f_sq_errors, n_inliers,inlier_index,Fmapsac] = torr_estimateF( M', 1, [round(log(1-0.99)/log(1-(1-0.25)^7)) 0.0039], 'mapsac', 0);
            [f,f_sq_errors] = torr_estimateF(M(:,inlier_index)', 1, [], 'non_linear',1,f);
            timeMAPSAC=toc;
            Fmapsac=reshape(f,3,3);
            Fmapsac=T1'*Fmapsac*T2;
            Fmapsac=Fmapsac./norm(Fmapsac);
            [meanmapsac,stddevmapsac,minimmapsac,maximmapsac]=funmatError(Mini(:,inlier_index),Fmapsac);
            disp('Funamental Matrix')
            disp(Fmapsac)
            disp(sprintf('Distance point-epipolar line\nmean: %f  stdev: %f  min: %f  max: %f\n',meanmapsac,stddevmapsac,minimmapsac,maximmapsac));
            disp(sprintf('Rank-2: %d',rank(Fmapsac)==2))            
            disp(sprintf('Outliers: %d/%d',size(M,2)-size(Mini(:,inlier_index),2),size(M,2)))
            disp(sprintf('Time: %f',timeMAPSAC))            
            disp('*******************************************************************')
            if redraw, 
                fummatplot(Mini(:,inlier_index),Fmapsac);
            end
            redrawnow=1;
            Mfi=Mini(:,inlier_index);
            bars=[bars;meanmapsac,stddevmapsac];
            bartime=[bartime;timeMAPSAC];
            xlabelbars=[xlabelbars sprintf(' %d.- MAPSAC ',indexbars)];
            if mod(indexbars,8)==0, xlabelbars=[xlabelbars '\n']; end
            indexbars=indexbars+1;
            
        case 23,
            disp('Method: FNS implemented by Anton van den Hengel')
            tic
            [f, f_sq_errors, n_inliers,inlier_index,Ffns] = torr_estimateF( M', 1, [500 1], 'fns', 0);         
            timeFNS=toc;
            Ffns=T1'*Ffns'*T2;
            Ffns=Ffns./norm(Ffns);
            [meanfns,stddevfns,minimfns,maximfns]=funmatError(Mini,Ffns);
            disp('Funamental Matrix')
            disp(Ffns)
            disp(sprintf('Distance point-epipolar line\nmean: %f  stdev: %f  min: %f  max: %f\n',meanfns,stddevfns,minimfns,maximfns));
            disp(sprintf('Rank-2: %d',rank(Ffns)==2))            
            disp(sprintf('Time: %f',timeFNS))            
            disp('*******************************************************************')
            if redraw, 
                fummatplot(Mini,Ffns)
            end
            bars=[bars;meanfns,stddevfns];
            bartime=[bartime;timeFNS];
            xlabelbars=[xlabelbars sprintf(' %d.- FNS ',indexbars)];
            if mod(indexbars,8)==0, xlabelbars=[xlabelbars '\n']; end
            indexbars=indexbars+1;
            
        case 24,
            disp('Method: CFNS implemented by Anton van den Hengel')
            tic
            [f, f_sq_errors, n_inliers,inlier_index,Fcfns] = torr_estimateF( M', 1, [500 1], 'cfns', 0);
            timeCFNS=toc;
            Fcfns=T1'*Fcfns'*T2;
            Fcfns=Fcfns./norm(Fcfns);
            [meancfns,stddevcfns,minimcfns,maximcfns]=funmatError(Mini,Fcfns);
            disp('Funamental Matrix')
            disp(Fcfns)
            disp(sprintf('Distance point-epipolar line\nmean: %f  stdev: %f  min: %f  max: %f\n',meancfns,stddevcfns,minimcfns,maximcfns));
            disp(sprintf('Rank-2: %d',rank(Fcfns)==2))
            disp(sprintf('Time: %f',timeCFNS))            
            disp('*******************************************************************')
            if redraw, 
                fummatplot(Mini,Fcfns) 
            end   
            bars=[bars;meancfns,stddevcfns];
            bartime=[bartime;timeCFNS];
            xlabelbars=[xlabelbars sprintf(' %d.- CFNS ',indexbars)];
            if mod(indexbars,8)==0, xlabelbars=[xlabelbars '\n']; end
            indexbars=indexbars+1;
            
    end
        
    if redrawnow & redraw,
        plotimagepoints(Mfi,nomia,nomib)
    end
%    pause
end

figure;
bar(bars);
legend('Mean','Stdev')               
xlabel(sprintf(xlabelbars))
ylabel(sprintf('pix.'))
title(['Distance points-epipolar lines'])
set(gca,'XTick',[1:size(method,2)]);
set(gca,'XLim',[0 size(method,2)+1]);
zoom on

figure;
bar(bartime);
xlabel(sprintf(xlabelbars))
ylabel(sprintf('sec.'))
title(['Time'])
set(gca,'XTick',[1:size(method,2)]);
set(gca,'XLim',[0 size(method,2)+1]);
zoom on