findvanishingpoints.m

使用支持向量机（svm）方法进行图像的单视度量(single view metrology)的vc++（需要用到wxwindows2.4.2）和matlab源代码.

% Implementaton of "A new Approach for Vanishing Point Detection in
% Architectural Environments" by Carstern Rother 2002
% Author: Jan Hoeft ([e-mail])
function [Points]=findvanishingpoints(Lines,picsize,Weigth,debugout)
if(nargin==3)
    debugout=0;
end

numberOfLines=size(Lines,1);

tic
%cross all lines with each other to get the Vanishing Points(VP)
VP=[];
for i=1:numberOfLines-1
    LinesToCross=Lines((i+1):numberOfLines,:);
    LineToCross=repmat(Lines(i,:),numberOfLines-i,1);
    VP=[VP;cross(LinesToCross,LineToCross,2)];
end

% to take the biggest lines as infinite vanishingpoints
% was important when the line finding algorithm was not so good
% % tmp=Lines(find(Weigth>0.5),:);
% % tmp(:,3)=0;
% % t=tmp(:,1);
% % tmp(:,1)=-tmp(:,2);
% % tmp(:,2)=t;
% % VP=[VP;tmp];

numberOfVPs=size(VP,1);

% a vp far outside the immage becomes an infinite vp
v=VP(:,1:2)./repmat(VP(:,3)+real(VP(:,3)==0),1,2);
v(VP(:,3)==0,:)=0;
toinf=find((abs(v(:,1))>picsize(2)*10 | abs(v(:,2))>picsize(1)*10) & VP(:,3)~=0);
VP(toinf,3)=0;



%%%build up accumulator
accumulator=zeros(numberOfVPs,1);
%config
% 1 degree wrong equals 4 pixel wrong 
weigthpixel=1;
weigthdegree=4;
% so count if below 20 pixel or 5 degree
threshold=20;
% w1 and w2 from the paper
w1=0.8;
w2=0.2;

%accumulate
whichvoted=sparse(numberOfVPs,numberOfLines);
for i=1:numberOfVPs
    actualvp=VP(i,:);
    [D,Alpha]=vpdistance(actualvp,Lines);
    distance=weigthpixel*D+weigthdegree*Alpha*180/pi;
    
    votingcells=find(distance<threshold);
    whichvoted(i,:)=(distance<threshold)';    
    
%     % nice debug output
%     H=drawlines(Lines(votingcells,:),picsize,[1 0 1],2);
%     h=drawpoints(actualvp,picsize,[0 1 0],20);
%     delete(H);
%     delete(h);
    
    accumulator(i)=sum(w1*(1-distance(votingcells)/threshold)+w2*Weigth(votingcells));
end


%%%%delete similar VPs
%compare ones that ARE NOT at INFINITY
%VPs nearer to each other than sqrt(threshold_notInf) become one
threshold_notInf=30 ^2;

indexOfInfVPs=(VP(:,3)==0);
notInfVP=VP(~indexOfInfVPs,:);
notInfAC=accumulator(~indexOfInfVPs,:);
notInfVO=whichvoted(~indexOfInfVPs,:);
inhomogen_notInfVP=notInfVP(:,1:2)./repmat(notInfVP(:,3),1,2);
todel=logical(zeros(size(notInfAC)));
for i=1:size(inhomogen_notInfVP,1)
    if(~todel(i))
        for j=i+1:size(inhomogen_notInfVP,1)
            if(~todel(j))
                if(sum((inhomogen_notInfVP(i,:)-inhomogen_notInfVP(j,:)).^2)<threshold_notInf)
                    if(notInfAC(j)>notInfAC(i))
                        notInfAC(i,:)=notInfAC(j,:);
                        notInfVP(i,:)=notInfVP(j,:);
                        notInfVO(i,:)=notInfVO(j,:);
                    end
                    todel(j)=true;
                end
            end
        end
    end
end
notInfVP(todel,:)=[];
notInfAC(todel)=[];
notInfVO(todel,:)=[];

%compare ones that ARE at INFINITY
%VPs nearer to each other than acos(treshold_Inf) become one
treshold_Inf=cos(pi/16);

InfVP=VP(indexOfInfVPs,:);
InfAC=accumulator(indexOfInfVPs,:);
InfVO=whichvoted(indexOfInfVPs,:);
inhomogen_InfVP=InfVP(:,1:2);
todel=logical(zeros(size(InfAC)));
for i=1:size(inhomogen_InfVP,1)
    if(~todel(i))
        for j=i+1:size(inhomogen_InfVP,1)
            if(~todel(j))
                if(abs(inhomogen_InfVP(i,:)*inhomogen_InfVP(j,:)') ...
                        /(norm(inhomogen_InfVP(i,:))*norm(inhomogen_InfVP(j,:)) ) ...
                        >treshold_Inf)
                    if(InfAC(j)>InfAC(i))
                        InfAC(i,:)=InfAC(j,:);
                        InfVP(i,:)=InfVP(j,:);
                        InfVO(i,:)=InfVO(j,:);
                    end
                    todel(j)=true;
                end
            end
        end
    end
end
InfVP(todel,:)=[];
InfAC(todel)  =[];
InfVO(todel,:)=[];

%unify again the arrays
VP          =[InfVP;notInfVP];
accumulator =[InfAC;notInfAC];
whichvoted  =[InfVO;notInfVO];



%take only the 200 best
%normally wont truncate the set
%but if it happened the computer would work forever (remember O(n^2))
if(length(accumulator)>200)
    [dummy,IX]=sort(accumulator);
    IX=flipdim(IX,1);
    IX(200:end)=[];
    accumulator=accumulator(IX);
    VP=VP(IX,:);
    whichvoted=whichvoted(IX,:);
end
toc



tic
%take the max and put it at the beginning
maxpos=find(max(accumulator)==accumulator);     %pos of max
maxpos=maxpos(1);                               %take only one
accumulator=[accumulator(maxpos);accumulator];  %to the start of the acc
accumulator(maxpos+1)=[];
VP=[VP(maxpos,:);VP];                           %to the start of the VPs
VP(maxpos+1,:)=[];
whichvoted=[whichvoted(maxpos,:);whichvoted];   %to the start of the VotedList
whichvoted(maxpos+1,:)=[];

%debug output
if debugout>1
    ma=max(max(accumulator));
    drawpoints(VP,picsize,[accumulator./ma,zeros(size(accumulator)),1-accumulator./ma],(accumulator./ma*100));
    drawnow;
end

%search step
maxvote=0;
H=[];
for i=2:length(accumulator)
    j=i+1;
    var_i_got_sense=true;
    while j<=length(accumulator) && var_i_got_sense
        V=[VP(1,:);VP(i,:);VP(j,:)];

        %debug output 1=yellow i=green j=blue
        if debugout>2
            delete(H);
            H=drawpoints(V,picsize,[1 1 0;0 1 0;0 0 1],40);
            
            %debug breakpoint
            if(j==i+1)
                new_VPi=true;
            end
        end

        
        %%vanishing line criterion
        vancrit=crit_vanline(V,Lines,i,j,whichvoted,picsize);
        
        %if the vanishing line criterion for VP1 and VPi is not fulfilled
        if vancrit==-1
            %skip inner part
            var_i_got_sense=false;
        end

        %if the vanishing line criterion is fulfilled
        if(vancrit==1)
            
            %%camera and orthogonal criterion
            camorthcrit_prob=crit_camera_and_orthogonal(V,picsize);
            
            if(camorthcrit_prob)
                %calculate the vote and multyply it by the probability
                %of the camera configuretion
                vote=camorthcrit_prob*(accumulator(1)+accumulator(i)+accumulator(j));

                %if this was the best combination until now take it
                if(vote>maxvote)
                    besti=i;
                    bestj=j;
                    maxvote=vote;
                end
            end
        end

        j=j+1;
    end %while j
end %for i
toc

%return the found VP combination
if(maxvote~=0)
    Points=[VP(1,:); VP(besti,:); VP(bestj,:)];

    %debug output    
    if debugout==1
        voted=whichvoted([1 besti bestj],:);
        drawlines(Lines(find(max(voted)),:),picsize,[.5 .5 .5],1);
    end
    if debugout>1
        delete(H);
        drawpoints(Points,picsize,[0 1 0],20);
        drawnow;
    end
    
else
    Points=[];
end