% Huehistograms function will return two hue histograms for a given image file 
% (assuming there is building in the image, so principle directions can be defined) 
% First line segments are detected, and vanishing points are estimated based on EM iterations
% Then pixels are grouped accordingly, and two hue histograms are estimated
% Wei Zhang, George Mason University, 2005

function [Hhist, Vhist] = Huehistograms(file)

if findstr(file, 'ppm') % file is ppm image
    InputImage = loadppm(file);
else    % other format like jpg and bmp
    InputImage = imread(file);
end;

if isrgb(InputImage);
    im = double(rgb2gray(InputImage));
else
    error('The image must be color in order to compute hue histogram');
end;
[ydim,xdim] = size(im);

imSubSample = 0;
% subsample if xdim is large
if xdim > 400
    imSubSample = 1;
end;
InputImage = imresize(InputImage, 1/2^imSubSample); %this is color image
% used better subsampling for image to be used for pixel grouping
im = blurDn(im, imSubSample, 'qmf9');
[ydim,xdim] = size(im);
%%%%%%%%%%%%%%%%%%%%%%
% compute derivatives
prefilt = [0.223755 0.552490 0.223755];
derivfilt = [-0.453014 0 0.45301];
fx     = conv2( conv2( im, prefilt', 'same' ), derivfilt, 'same' );
fy     = conv2( conv2( im, prefilt, 'same' ), derivfilt', 'same' );
magn = sqrt(fx.^2 + fy.^2);
direct = (atan2(-fy,fx)*180/pi);  % orientation -pi .. pi
index = find(direct < 0);
direct(index) = direct(index)+180;
direct = uint8(direct);

%//linesegment, is written in C for better efficiency. which is compliled
%as .dll file to be called by Matlab
%output:
%1. uint8 image with line segment.
%2. a two dimention array 18*n (number of line). to use it, must first
%rotate, to comply with our use habit.
%input
%1. input image is uint8(grayscale).    it is rotated for parameter reason,
%(because C and Matlabe has different way to store two dimmension array).
%2. length threshold. default 12.
[testresult, lseg_group] = line_segment(uint8(im)',round(12*xdim/500)); %length threshold is related to size of image.
lseg_group = lseg_group';
lnum_group = size(lseg_group,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% lseg_group(i,:)= 
% 1     sum(magn) - normalized gradient magnitude summed over all line pixels
% 2     cond_num - v_min/v_max - ratio of eigenvalues
% 3     ro - distance of the line from the origin
% 4     theta 
% 5     slope - slope of the line
% 6,7     v1,v2 - two eigenvalues 
% 8:11     vec(:,1)',vec(:,2)' - two eigenvectors
% 12,13     xm,ym  line midpoint segment
% 14:17     x1,x2,y1,y2 - are the lines endpoints for printing,  here x is column index and y is row index
% 18   length of the line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  

label = zeros(lnum_group,1);
index = lseg_group(:,12)*1000+lseg_group(:,13);
[temp,ind] = sort(index);

xcenter = xdim/2;     
ycenter = ydim/2;
center = [xcenter; ycenter;0];

%some lines are repeatively detected.
%check whether they have same middle point.
temp=temp(2:lnum_group)-temp(1:lnum_group-1); 
%check whether their slope is close
temp2=abs(lseg_group(ind(2:lnum_group),4)-lseg_group(ind(1:lnum_group-1),4));  
ind2=find(temp2<0.01 & temp==0)+1;  
label(ind(ind2))=1;

% some false lines come from the boundary of image
%vertical case
ind=find(lseg_group(:,12)<5 | (xdim-lseg_group(:,12))<5);
ind2=find(abs(lseg_group(ind,4))<0.001 | abs(pi/2-lseg_group(ind,4))<0.001);
label(ind(ind2))=1;     
%horizontal case
ind=find(lseg_group(:,13)<5 | (ydim-lseg_group(:,13))<5);
ind2=find(abs(lseg_group(ind,5))<0.001);
label(ind(ind2))=1; 
%remove them
ind = find(label==0);
lseg_group = lseg_group(ind,:);
lnum_group = size(lseg_group,1);
cond_th = max(6e-3/(lnum_group/200), 3e-3); %treshold for how straigh the support is

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% treshold lines with low magnitude and high condition number
%%%%%%%%%%%%%%%
ind=find(lseg_group(:,2)<cond_th);
lseg_group=lseg_group(ind,:);
lnum_group=size(lseg_group,1);

linequality = -lseg_group(:,18);
temp = find(lseg_group(:,2)==0);
 %if the cond_num of line segment is very good, increase their prior
linequality(temp) = linequality(temp)-ydim/100; 
[tt, ind] = sort(linequality);
number = 200;  %the number lines to keep
if lnum_group > number   %keep only top quanlity line segments
    lseg_group = lseg_group(ind(1:number),:);
    lnum_group = number;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% create histogram of line angles from -pi/2 to pi/2
% number of bins nb - first and last bin are identified
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
nb = 60; inc = pi/nb;
hl = zeros(1,nb+1); % first and last bin are identified
cl = zeros(1,nb+1); 

% compute centroids of the bins
for i = 1:nb+1
    cl(i) = -(pi/2) + (i-1)*inc;
end;

for i=1:lnum_group
    tmp = round(lseg_group(i,4)/inc) + (nb)/2 + 1; % compute hist bin index 
    hl(tmp) = hl(tmp) + 1;
end;

%handle the border of histogram
hh = hl(1)+hl(nb+1);  
hl(1) = hh; hl(nb+1) = hh; 

peaks = findpeaks(hl); %search for the peaks in the histogram
mixture_num = size(peaks,2);
if peaks(mixture_num)-peaks(1) > nb-3
    peaks(1) = 1;
    mixture_num = mixture_num-1;
    peaks=peaks(1:mixture_num);
end;
ap = cl(peaks);              % medians of the bins

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% group the lines for EM initialization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
g.index = [];
g.size = [];
g.mean = [];

gmax = 1;
j = 0;
np = size(peaks,2);
w = 0.5; % tolerance +/- 1 degree
w = 3; % number of bins around the peak
for i = 1:np 
    if (ap(i) - pi/nb) <= -pi/2  % if there is a peak around zero
        ind = find((pi/2 - w*pi/nb) < lseg_group(:,4));
        ind1 = find(lseg_group(:,4)<(-pi/2 + w*pi/nb) & abs(lseg_group(:,5))>0.02 );
        ind = [ind; ind1];
        j  = j + 1;
        g(j).index  = ind;
    else
        ind =  find(abs(lseg_group(:,4) - ap(i)) <=  w*pi/nb & abs(lseg_group(:,5))>0.02);
        j = j + 1;
        g(j).index  = ind;
    end
end

% get initial hypotheses 
f.index = [];
j = 0;
for i = 1:np       
    if size(g(i).index,1) > 1 
        j = j+1;
        f(j).index = g(i).index;
    end;
end;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize vanishing points
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mixture_num = j;
% get normals
K = [1 0 0; 0 1 0; 0 0 1];  %a calibration matrix, refer to ECCV paper
[scalefactor, planes]= plineNormal(lseg_group,K,xdim,ydim, 1);

for i = 1:mixture_num
    ind = f(i).index;
    s = size(ind);
    
    vps(:,i) = vpEstimate(ones(1,size(f(i).index,1)),lseg_group(f(i).index,:), planes(:,f(i).index), cond_th);
    tindex = find(vps(3,:)<0.001);
    vps(3,tindex) = 0.001; % protection from divided by 0 
    vpsn(:,i) = vps(:,i)/vps(3,i);   % normalized vps
    vpsn(:,i) = vpsn(:,i)/scalefactor+center;
    vpsn(2,i) = -vpsn(2,i);
end
vps = vpsn(1:2,:);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

niter = 10;
%%%%%%%%%%%%%%%%%%%%%%
% iteration of EM 
%%%%%%%%%%%%%%%%%%%

for ni = 1:niter
    res = zeros(lnum_group,mixture_num+1);
    wt = zeros(lnum_group,mixture_num+1);
    Lhood = zeros(lnum_group,mixture_num+1);
    distance = zeros(lnum_group,mixture_num+1);
    
    if ni < 7
        sigma2(1:mixture_num) = (0.27-ni*0.03)*ones(1,mixture_num);
    end;    
    
    %%%%%%%%%%%%
    % E-step
    %%%%%%%%%%%%
    support = zeros(1,mixture_num+1);
    linesupport = zeros(1,mixture_num+1);
    
    for j = 1:lnum_group
        % for each line compute the residual to each VP    
        for l = 1:mixture_num+1
            res(j,l) = pi; %initialize with a high value, so that smallest residule can be found;
        end;
        
        for mi = 1:mixture_num
            dir(:,mi) = [vps(1,mi)-lseg_group(j,12); 
                vps(2,mi)-(-lseg_group(j,13))]; % negative y-coor
            
            angle2 = atan(dir(2,mi)/dir(1,mi));
            angle1 = abs(lseg_group(j,5)-angle2);      
            tp = min(angle1,pi-angle1); 
            res(j,mi) = tp;
            Lhood(j,mi) = exp(-(res(j,mi))^2/(2*sigma2(mi)^2));
        end; % end of mi
        
        if ni>10
            res(j,mixture_num+1) = pi/12;
        else           
            res(j,mixture_num+1) = pi/16*(max((mixture_num/3),1));  % outlier process
        end;
        
        LastIndex = mixture_num+1;
        Lhood(j,mixture_num+1) = exp(-(res(j,mixture_num+1))^2/(2*sigma2(mi)^2));
        
        [val,ind] = sort(res(j,:));              
        linesupport(ind(1)) = linesupport(ind(1))+1;        
        nrm = sum(Lhood(j,1:mixture_num+1));
        if(nrm>1e-5)
            wt(j,:) = Lhood(j,:)/nrm; % assignment estimation  
        else
             % all the Lhood is very small, tend to be an outliers, so assign small weight
            wt(j,:) = 1e-5; 
        end;       
    end  % end of E step    
  
    %not to use nearly horizontal line
    validnumber = lnum_group;
    nothorizontal = abs(lseg_group(:,5))>0.001;
    for i = 1: mixture_num
        wt(:,i)=wt(:,i).*nothorizontal;
    end;
    validnumber = size(find(nothorizontal>0),1);
    
    %support of group is defined as number of lines with residual less than some threshold.
    for j = 1:mixture_num+1
        support(j) = size(find(res(:,j)<0.5/(ceil(ni/4))), 1);
    end;    
    
    supportthreshold = min(4, floor(validnumber/6));   
    if ni>7  % when two vps are competing for group of lines, they all have small wt, and both are got rid off, so do it sequntially.
        lll=zeros(1,mixture_num);
        for mi = 1:mixture_num
            if size(find(wt(:,mi)>0.7),1)<2  %to get rid of false group formed by line of other group
                lll(mi) = 1;
            end;         
        end;    
        ttemp = find(lll);
        if size(ttemp,2)>1
            [ttt,iidd] = min(linesupport(ttemp));
            support(ttemp(iidd)) = 1;
        end;
    end;
    
    % delete small group
    mi = 1;
    while mi < mixture_num +1
        if(support(mi)<supportthreshold & support(mi)>-1 | linesupport(mi)<2)
            res(:, mi:mixture_num-1) = res(:, mi+1:mixture_num);
            res(:,mixture_num) = pi;
            vps(:, mi:mixture_num-1) = vps(:, mi+1:mixture_num);
            support(mi:mixture_num) = support(mi+1: mixture_num+1);
            linesupport(mi:mixture_num) = linesupport(mi+1: mixture_num+1);
            wt(:, mi:mixture_num) = wt(:, mi+1: mixture_num+1);
            mixture_num = mixture_num - 1;
            support = support(1:mixture_num+1);
            linesupport = linesupport(1:mixture_num+1);
            vps = vps(:,1:mixture_num);
            mi = mi-1;     
        end;
        mi=mi+1;
    end;
    
    if ni > 7  
        threshold = min(ni*0.1,0.8);
        wt = wt.*(wt>threshold);  % use only high weight line to compute
    end;
    
    %%%%%%%%%%%%%%%%%%%%%%%5
    % M step 
    %%%%%%%%%%%%%%%%%%%%%%%%
    % compute intersection of all lines, recompute the vanishing points
    index = 1:lnum_group;
    new_estimation=[];
    vpsn=[];
    %might consider early terminate the iteration if np doesn't change.
    for i = 1:mixture_num
        new_estimation(:,i) = vpEstimate(wt(:,i),lseg_group,planes, cond_th);
        if new_estimation(3,i)==0  %exception case, corresponding to the case for parallel lines??
            new_estimation(3,i)=1e-10;
        end;
        vpsn(:,i) = new_estimation(:,i)/new_estimation(3,i);   % normalized vps
        vpsn(:,i)=vpsn(:,i)/scalefactor+center;
        if ni>5 
            % if vp suddenly changed, keep the last one
            if sign(vpsn(1,i))==-sign(vps(1,i))
                vpsn(1,i)=vps(1,i);
                vpsn(2,i)=-vps(2,i);
            end;
        end;
    end;
    vps = vpsn(1:2,:);
    vps(2,:)=-vps(2,:);        
    
    % merge closed vanishing point
    mi = 1;
    factor = (ni/3);    
    while mi < mixture_num + 1
        angle1 = atan2( (-vps(2,mi)-ycenter), (vps(1, mi)-xcenter));  %the angle between line and vanishing direction
        dist1 = norm((-vps(2,mi)-ycenter), (vps(1, mi)-xcenter));     %the distance between vanishing point and line's middle point
        ki = mi+1;
        while ki < mixture_num+1
            angle2 = atan2((-vps(2,ki)-ycenter), (vps(1, ki)-xcenter));
            dist2 = norm((-vps(2,ki)-ycenter), (vps(1, ki)-xcenter));
            difference = min(abs(angle1-angle2),pi*2-abs(angle1-angle2));
            if difference < 0.1*factor; %if the direction difference is really small, merge them
                %get the ratio between distance.
                if dist1 > dist2 
                    distrate = dist1/dist2;  
                    aa = angle1;
                else
                    distrate = dist2/dist1;
                    aa = angle2;
                end
                if distrate > 5 & min(dist1,dist2) < 20
                    % one is very close to center, so do not merge them, nothing happened here
                else
                    pp = support(mi)/(support(mi)+support(ki));
                    vptest = (vps(:, mi)*support(mi) + vps(:, ki)*support(ki))/(support(mi)+support(ki));
                    % use geometric mean instead
                    vps(:, mi) = sign(vptest).*(abs(vps(:, mi))).^pp .* (abs(vps(:, ki))).^(1-pp);
                    if min(abs(aa-pi/2), abs(aa+pi/2)) < 0.3   %close to vertical, use larger value, geometric mean tend to be small
                        vps(:,mi) = vptest;
                    end;
                    vps(:, ki:mixture_num-1) = vps(:, ki+1:mixture_num);
                    support(mi) = support(mi) + support(ki);
                    support(ki:mixture_num) = support(ki+1: mixture_num+1);
                    support = support(1:mixture_num+1);
                    
                    linesupport(mi) = linesupport(mi)+linesupport(ki);
                    linesupport(ki:mixture_num) = linesupport(ki+1: mixture_num+1);
                    linesupport = linesupport(1:mixture_num+1);
                    
                    wt(:, mi) = max(wt(:,mi), wt(:, ki));
                    wt(:, ki:mixture_num) = wt(:, ki+1: mixture_num+1);
                    
                    res(:, mi:mixture_num+1) = pi;
                    mixture_num = mixture_num-1;
                    vps = vps(:,1:mixture_num);
                    ki = ki-1;
                end;            
            end;
            ki = ki+1;         
        end;
        mi = mi+1;      
    end;     
end;  %end of EM


% delete small group
mi = 1;
while mi < mixture_num +1
    if(linesupport(mi) < 3)
        vps(:, mi:mixture_num-1) = vps(:, mi+1:mixture_num);
        linesupport(mi:mixture_num) = linesupport(mi+1: mixture_num+1);
        wt(:, mi:mixture_num) = wt(:, mi+1: mixture_num+1);
        mixture_num = mixture_num-1;
        linesupport = linesupport(1:mixture_num+1);
        vps = vps(:,1:mixture_num);
        mi = mi-1;     
    end;
    mi = mi+1;
end;

if mixture_num < 2  %only one vanishing point
    Hhist = zeros(1,16);
    Vhist = zeros(1,16);
    return;  % exception occured.
end;

if mixture_num > 2  % in the last iteration, keep at most 3 vanishing point
    [temp, index1] = sort(-linesupport(1:mixture_num));
    index1 = index1(1:3);  %use three with most support. may need to consider position of vp
    vps = vps(:,index1);   
    mixture_num = 3;
    linesupport = linesupport(1:mixture_num+1);
    
    temp = vps;
    temp(1,:) = abs(temp(1,:)-xcenter);
    temp(2,:) = abs(temp(2,:)-ycenter);
    iidd = find(temp(1,:)==0);
    temp(1,iidd) = 1e-6;
    temp = temp(2,:)./temp(1,:);
    [temp,index1] = sort(-temp);
    index1 = index1([2,1,3]); %put vp from left, vertical, right.
    vps = vps(:,index1);
    if vps(1,1) > vps(1,3)
        index1 = [3,2,1];
        vps = vps(:,index1);
    end;
else
    if mixture_num == 2
        temp = vps;
        temp(1,:) = abs(temp(1,:)-xcenter);
        temp(2,:) = abs(temp(2,:)-ycenter);
        temp = temp(2,:)./temp(1,:);
        [temp,index1] = sort(-temp);
        index1 = index1([2,1]); %put vp from left, vertical.
        vps = vps(:,index1);
    end;
end

%%%%%%%%%%%group each pixel
[xind, yind] = meshgrid(1:xdim,1:ydim);   %note meshgrid use x as horizontal direction
sim = ones(4,ydim,xdim)*100;  %in case there is not third vp
iidd = find(fx==0);
fx(iidd) = 1e-6;
direct = -atan(fy./fx)*180/pi;
index = find(direct<0);
direct(index) = direct(index)+180;

for k = 1:mixture_num
    x = vps(1,k);
    y = -vps(2,k);
    kk = (x-xind)./(y-yind);  
    tslope = atan(kk)*180/pi;
    index = find(tslope<0);
    tslope(index) = tslope(index)+180;
    temp = abs(double(direct)-tslope);
    sim(k,:,:) = min(temp, 180-temp);
end;
sim(4,:,:) = ones(ydim,xdim)*30;  %outlier process;
[tt,label] = min(sim);
label = squeeze(label);

newlabel = zeros(ydim, xdim);
for kk = 1:3
    BW2 = zeros(ydim, xdim);
    BW2(find(label==kk)) = 255;
    [labelimage,tt] = clabel(uint8(BW2),ydim/2); %clabel is function written in C, doing connect component analysis. and remove small component threshold by second parameter.
    iidd = find(labelimage);
    newlabel(iidd) = kk;
end;

label = newlabel;
%%%%%%%%%%%%%%%%%%%%%
%finally, lable out background and clutter.
%labe the boundary of image as background. or they maybe label wrong, like left and right boundary would likely be labeled as group 2
label(1,1:xdim) = 0;
label(ydim,1:xdim) = 0;
label(1:ydim,1) = 0;
label(1:ydim, xdim) = 0;

mask = find(magn<2);
label(mask) = 0;  %set those with no gradient as background
%%%%%%%%%%%%%%%%%%%%%%%end grouping pixel


%linear inteplation the histogram. Even though this takes more time, it's proved to work better.
iidd = find(label==3);
label(iidd) = 1;  %treat the two horizontal direction as one direction.

%contruct the hue image
Hueimage = convert2Hue(InputImage);  %the returned hue is from -1 to 1;
Hueimage = (Hueimage+1)*8+0.5;  %make it 16 bins,  centers of bins are 1, 2, 3, ....16
xx = 1:16;
floorH = floor(Hueimage);
ceilH = ceil(Hueimage);
floordiff = Hueimage-floorH;
ceildiff = ceilH-Hueimage;

Hhist = zeros(1,16);
iidd = find(floorH==0 & label==1);
Hhist(1) = size(iidd,1);

iidd = find(ceilH==17 & label==1);
Hhist(16) = size(iidd,1);
for i = 1:16
    iidd = find(floorH==i &label==1);
    tt = sum(ceildiff(iidd));
    Hhist(i) = Hhist(i)+tt;
    iidd = find(ceilH==i &label==1);
    tt = sum(floordiff(iidd));
    Hhist(i) = Hhist(i)+tt;
end;

Vhist = zeros(1,16);
iidd = find(floorH==0 & label==2);
Vhist(1) = size(iidd,1);

iidd = find(ceilH==17 & label==2);
Vhist(16) = size(iidd,1);
for i=1:16
    iidd = find(floorH==i &label==2);
    tt = sum(ceildiff(iidd));
    Vhist(i) = Vhist(i)+tt;
    iidd = find(ceilH==i &label==2);
    tt = sum(floordiff(iidd));
    Vhist(i) = Vhist(i)+tt;
end;