% Cluster the matches for  3D line segments
% This little maneuver may cost us several days for large datasets and I am
% trying to improve it.
load([folder 'process\X.txt']) 
pairwise=load([folder 'process\pairwise.mat']);pairwise=pairwise.pairwise;

[stereos1,stereos2,namesID]=read_Stereo_Index(write_folder);

minsegs=3;% the minimum 3D lines in the cluster
p2=1;

match_all=[];
for i=1:size(stereos1,1)
    lm12=candis{i}.line+1;
    lines1=load([write_folder stereos1{i} '.lines']);
    lines2=load([write_folder stereos2{i} '.lines']);
    
    Xraw=X;
    Xraw_L=Xraw(candis{i}.stid:candis{i}.stid+size(lm12,1)-1);
    [cmatch_l,boolm]=match_assign(lm12,Xraw_L,size(lines1,1),size(lines2,1));
    cmatch_l=cmatch_l(1:int32(size(cmatch_l,1)*p2),:);
    
    header=ones(size(cmatch_l,1),2);
    header(:,1)= header(:,1)*namesID(stereos1{i});
    header(:,2)= header(:,2)*namesID(stereos2{i});
    
    match_all=[match_all; header cmatch_l];
end
str=[write_folder 'match_all.mat'];
save(str,'match_all');



%Cluster 3D lines
imstr=img_style;

[~,~,namesID]=read_Stereo_Index(write_folder);

names=namesID.keys;
  
for i=1:length(names);
    im2=imread([folder 'images\' names{i},imstr]);
    rr1{i}=0;
    rr2{i}=size(im2,1);
    cc1{i}=0;
    cc2{i}=size(im2,2);
    linesAll{i}=load([write_folder names{i} '.lines']);
    PAll{i}=load([folder 'cameras\' names{i} '.txt']);
end
str=[write_folder 'match_all.mat'];
match_all=load(str);match_all=match_all.match_all;
[Mgroup,Mnear] = Match2Group(match_all);

Mgroup=epipoleLineValid(PAll,linesAll,Mgroup,500);


for i=1:length(Mgroup)
    used{i}=zeros(size(Mgroup{i},1),1);
end

dist=2;
ecrosst=0.4;

XS=zeros(size(match_all,1),3);
YS=zeros(size(match_all,1),3);
C=zeros(size(match_all,1),1);
group=zeros(500,2);
ls=zeros(500,4);

sumCounter=0;
for i=1:length(Mgroup)
    length(Mgroup)-i
    for j=1:size(Mgroup{i},1)
        if used{i}(j)==1
            continue;
        end
        sumCounter=sumCounter+1;
        used{i}(j)=1;
        gcounter=1;
        group(gcounter,:)=[Mgroup{i}(j,1),Mgroup{i}(j,3)];
        gcounter=gcounter+1;
        group(gcounter,:)=[Mgroup{i}(j,2),Mgroup{i}(j,4)];
        
        v1=Mgroup{i}(j,1);
        v2=Mgroup{i}(j,2);
        ps{1}=PAll{v1};
        ps{2}=PAll{v2};
        lid1=Mgroup{i}(j,3);
        lid2=Mgroup{i}(j,4);
        
        ls(1:2,:)=[linesAll{v1}(lid1,:);
            linesAll{v2}(lid2,:)];
        [X,Y,bool3]=get3Dline_multi(ls(1:2,:),ps,dist);
        
       
        for mm=1:length(Mnear{i})
            m=Mnear{i}(mm);
            if i==m
                continue;
            end
            
            PP1=PAll{Mgroup{m}(1,1)};
            PP2=PAll{Mgroup{m}(1,2)};
            
            x1=PP1*[X 1]';
            x1=x1(1:2)/x1(3);
            y1=PP1*[Y 1]';
            y1=y1(1:2)/y1(3);
            
            v1=Mgroup{m}(1,1);
            if x1(1)<cc1{v1}||x1(1)>cc2{v1}||...
               x1(2)<rr1{v1}||x1(2)>rr2{v1}||...
               y1(1)<cc1{v1}||y1(1)>cc2{v1}||...
               y1(2)<rr1{v1}||y1(2)>rr2{v1}
                continue;
            end
            
            x2=PP2*[X 1]';
            x2=x2(1:2)/x2(3);
            y2=PP2*[Y 1]';
            y2=y2(1:2)/y2(3);
            
            v1=Mgroup{m}(1,1);
            if x2(1)<cc1{v1}||x2(1)>cc2{v1}||...
                    x2(2)<rr1{v1}||x2(2)>rr2{v1}||...
                    y2(1)<cc1{v1}||y2(1)>cc2{v1}||...
                    y2(2)<rr1{v1}||y2(2)>rr2{v1}
                    continue;
            end
            
            
            for n=1:size(Mgroup{m},1)
                if  used{m}(n)==1
                    continue;
                end
                
                v3=Mgroup{m}(n,1);
                id3=Mgroup{m}(n,3);
                l3=linesAll{v3}(id3,:);
                [boolv1,err1]=twoLinesAnalysis_EEE(l3,x1,y1,dist,ecrosst);
                
                if ~boolv1
                    continue;
                end
                v4=Mgroup{m}(n,2);
                id4=Mgroup{m}(n,4);
                l4=linesAll{v4}(id4,:);
                [boolv2,err2]=twoLinesAnalysis_EEE(l4,x2,y2,dist,ecrosst);
                
                if ~boolv2
                    continue;
                end
                
                
                
                gcounter=gcounter+1;
                group(gcounter,:)=[Mgroup{m}(n,1),Mgroup{m}(n,3)];
                gcounter=gcounter+1;
                group(gcounter,:)=[Mgroup{m}(n,2),Mgroup{m}(n,4)];
                
                
                for k=1:gcounter
                    v=group(k,1);
                    lid=group(k,2);
                    ps{k}=PAll{v};
                    ls(k,:)=linesAll{v}(lid,:);
                end
                [X1,Y1,bool3]=get3Dline_multi(ls(1:k,:),ps,dist);
                if ~bool3
                    gcounter=gcounter-2;
                else
                    X=X1;Y=Y1;
                    used{m}(n)=1;
                    break;
                end
                
            end
        end
        
        XS(sumCounter,:)=X;
        YS(sumCounter,:)=Y;
        C(sumCounter,:)=gcounter;
    end
end
XX=XS(C>minsegs,:);YY=YS(C>minsegs,:);

export2obj(XX,YY,[folder 'visual\D3lines.obj']);



