% Copyright 2017 The Johns Hopkins University Applied Physics Laboratory.
% Licensed under the MIT License. See LICENSE.txt in the project root for full license information.

% Reference: 
% [1] M. Bosch, Z. Kurtz, S. Hagstrom, M. Brown 
% A Multiple View Stereo Benchmark for Satellite Imagery.
% Proceedings of IEEE Applied Imagery Pattern Recognition (AIPR).
% Washington DC, October 2016



% To reproduce results reported by [1]: set =>
% FASTCONFIG=0 and rundemo=0
%for faster (slightly less accurate) run: set => FASTCONFIG=1
clear all
close all
warning('off','all')
addpath(genpath('./3rdparty'))

%set FASTCONFIG =1 runs OF faster but lower slight lower accuracy and resolution
% 0- it reproduces results reported by [1]
FASTCONFIG=1;

siteID = 0;
%0 -demo site
%1 -Explorer site
%2 -MasterProvisional1 site
%3 -MasterProvisional2 site
%4 -MasterProvisional3 site
%5 -MasterSequestered1 site
%6 -MasterSequestered2 site
%7 -MasterSequestered3 site
%8 -MasterSequesteredPark site

pathCROPTIFs = '..\crop_imgs\';
resultpathpartial='..\las\partialarray_';%stores individual point clouds for each pair with displayresults set = 1
resultpath='..\las\Masterarray.las';%stores final point cloud in LAS format
resultpathgeotiff='..\las\Masterarray.tif';%stores final point cloud in GEOTIFF format
metricsexecpath='..\execs\run-metrics_master.exe';

evaluation=0;%if gt is available for the site and you want to compute metrics set it to 1

%read nitf and kml and generate tif or read precropped tifs
%flag for reading from already cropped images
cropread=1;
if cropread==1
    [Nimg,gtpath]=setup_imagepaths(siteID,pathCROPTIFs);    
else
    %demo:
    if siteID==0       
        command{1} = (['..\execs\crop-image.exe -k ..\kml\demo.kml -r ..\rpc\rpc1.out -o ..\imgs\img1.tif ' pathNITFs '18DEC15WV031000015DEC18140533-P1BS-500515572050_01_P001_________AAE_0AAAAABPABJ0.NTF']);
        command{2} = (['..\execs\crop-image.exe -k ..\kml\demo.kml -r ..\rpc\rpc2.out -o ..\imgs\img2.tif ' pathNITFs '18DEC15WV031000015DEC18140455-P1BS-500515572010_01_P001_________AAE_0AAAAABPABJ0.NTF']);
        command{3} = (['..\execs\crop-image.exe -k ..\kml\demo.kml -r ..\rpc\rpc3.out -o ..\imgs\img3.tif ' pathNITFs '18DEC15WV031000015DEC18140544-P1BS-500515572060_01_P001_________AAE_0AAAAABPABJ0.NTF']);
    else
       pathNITFs = '';%C:\Users\boschm1\src\Algorithms\IARPA_challenge\run_MSV_baseline\nitfs\';
       if isempty(pathNITFs)==1
            error('Indicate the location to the NITF images')
       end

        %use this to reproduce results:
        %explorer(siteID 1)
         command{1} = ('..\execs\crop-image.exe -k ..\kml\Explorer.kml -r ..\rpc\rpc1.out -o ..\imgs\img1.tif \\qie-bigraid2\volume2\EOPCC\160308-PackageForChallengeParticipants\NITF\18DEC15WV031000015DEC18140533-P1BS-500515572050_01_P001_________AAE_0AAAAABPABJ0.NTF');
         command{2} = ('..\execs\crop-image.exe -k ..\kml\Explorer.kml -r ..\rpc\rpc2.out -o ..\imgs\img2.tif \\qie-bigraid2\volume2\EOPCC\160308-PackageForChallengeParticipants\NITF\18DEC15WV031000015DEC18140455-P1BS-500515572010_01_P001_________AAE_0AAAAABPABJ0.NTF');
         command{3} = ('..\execs\crop-image.exe -k ..\kml\Explorer.kml -r ..\rpc\rpc3.out -o ..\imgs\img3.tif \\qie-bigraid2\volume2\EOPCC\160308-PackageForChallengeParticipants\NITF\18DEC15WV031000015DEC18140544-P1BS-500515572060_01_P001_________AAE_0AAAAABPABJ0.NTF');
         command{4} = ('..\execs\crop-image.exe -k ..\kml\Explorer.kml -r ..\rpc\rpc4.out -o ..\imgs\img4.tif \\qie-bigraid2\volume2\EOPCC\160308-PackageForChallengeParticipants\NITF\18DEC15WV031000015DEC18140510-P1BS-500515572040_01_P001_________AAE_0AAAAABPABJ0.NTF');
         %master1,seq1,seq2(siteID 2,5,6)
%             command{1} = (['..\execs\crop-image_master.exe -k ..\kml\MasterProvisional1.kml -r ..\rpc\rpc1.out -o ..\imgs\img1.tif ' pathNITFs '18DEC15WV031000015DEC18140533-P1BS-500515572050_01_P001_________AAE_0AAAAABPABJ0.NTF']);
%             command{2} = (['..\execs\crop-image_master.exe -k ..\kml\MasterProvisional1.kml -r ..\rpc\rpc2.out -o ..\imgs\img2.tif ' pathNITFs '18DEC15WV031000015DEC18140455-P1BS-500515572010_01_P001_________AAE_0AAAAABPABJ0.NTF']);
%             command{3} = (['..\execs\crop-image_master.exe -k ..\kml\MasterProvisional1.kml -r ..\rpc\rpc3.out -o ..\imgs\img3.tif ' pathNITFs '18DEC15WV031000015DEC18140544-P1BS-500515572060_01_P001_________AAE_0AAAAABPABJ0.NTF']);
%             command{4} = (['..\execs\crop-image_master.exe -k ..\kml\MasterProvisional1.kml -r ..\rpc\rpc4.out -o ..\imgs\img4.tif ' pathNITFs '18DEC15WV031000015DEC18140510-P1BS-500515572040_01_P001_________AAE_0AAAAABPABJ0.NTF']);
% 
%          %    %master2 (siteID 3)
%               command{1} = (['..\execs\crop-image_650rad.exe -k ..\kml\MasterProvisional2.kml -r ..\rpc\rpc1.out -o ..\imgs\img1.tif ' pathNITFs '22OCT15WV031000015OCT22140432-P1BS-500497282010_01_P001_________AAE_0AAAAABPABS0.NTF']);
%               command{2} = (['..\execs\crop-image_650rad.exe -k ..\kml\MasterProvisional2.kml -r ..\rpc\rpc2.out -o ..\imgs\img2.tif ' pathNITFs '27SEP15WV031000015SEP27140912-P1BS-500497284100_01_P001_________AAE_0AAAAABPABQ0.NTF']);     
%               command{3} = (['..\execs\crop-image_650rad.exe -k ..\kml\MasterProvisional2.kml -r ..\rpc\rpc3.out -o ..\imgs\img3.tif ' pathNITFs '03OCT15WV031000015OCT03140452-P1BS-500497283050_01_P001_________AAE_0AAAAABPABR0.NTF']);
%               command{4} = (['..\execs\crop-image_650rad.exe -k ..\kml\MasterProvisional2.kml -r ..\rpc\rpc4.out -o ..\imgs\img4.tif ' pathNITFs '01SEP15WV031000015SEP01135603-P1BS-500497284040_01_P001_________AAE_0AAAAABPABP0.NTF']);
% 
%          %    %master3,seq3,park (siteID 4,7,8)
%               command{1} = (['..\execs\crop-image_650rad.exe -k ..\kml\MasterProvisional3.kml -r ..\rpc\rpc1.out -o ..\imgs\img1.tif ' pathNITFs '27SEP15WV031000015SEP27140912-P1BS-500497284100_01_P001_________AAE_0AAAAABPABQ0.NTF']);
%               command{2} = (['..\execs\crop-image_650rad.exe -k ..\kml\MasterProvisional3.kml -r ..\rpc\rpc2.out -o ..\imgs\img2.tif ' pathNITFs '01SEP15WV031000015SEP01135603-P1BS-500497284040_01_P001_________AAE_0AAAAABPABP0.NTF']);     
%               command{3} = (['..\execs\crop-image_650rad.exe -k ..\kml\MasterProvisional3.kml -r ..\rpc\rpc3.out -o ..\imgs\img3.tif ' pathNITFs '14SEP15WV031000015SEP14140305-P1BS-500497285020_01_P001_________AAE_0AAAAABPABS0.NTF']);
    end

    Nimg=length(command);%number of images to be used
    %run the executable
    fprintf('reading NITFs and extracting kml area\n\n');%generating 
    for ini=1:Nimg
        %%crop-image.exe: it only crops within the kml limits
        %%crop-image_master.exe: it crops to and area of +/-1000 pixels in each
        %dimension with respect to the kml's centroid (larger area to avoid
        %occlusions in the area of interest
        %%crop-image_650rad: same as crop-image_master.exe but with a padding
        %%of +/-650 pixels wrt kml's centroid    
        % in order to run this executable in windows make sure you can build the sampled code provided
        system(command{ini})
    end
end

if siteID==0
    evaluation=0;
end   
%setup paths from the nitf reader exec
imgpath{1} = '..\imgs\img1.tif';
imgpath{2} = '..\imgs\img2.tif';
imgpath{3} = '..\imgs\img3.tif';
imgpath{4} = '..\imgs\img4.tif';
imgpath{5} = '..\imgs\img5.tif';
rpcpath{1} = '..\rpc\rpc1.out';
rpcpath{2} = '..\rpc\rpc2.out';
rpcpath{3} = '..\rpc\rpc3.out';
rpcpath{4} = '..\rpc\rpc4.out';
rpcpath{5} = '..\rpc\rpc5.out';



%set up flags
displayresults=0;%1 to display intermediate figures
SMOOTHPC=0;%emphasizes smoothness rather than locality (optical flow)
bwmotion = 0;%background motion for occlusions
verbose=false;%display intermediate information in the triangulation steps
INVALIDZ = -1000;%numeric value to indicate invalid points 
LM=0;%non linear triangulation refinement using Lev-Mar algo (very memory hungry; increase trainds to use less points!!!)
BlockAdjust=0;%BA of rpc parameters before final triangulation (slow! needs speed up)
triands=2;%subsample factor for triangulation and point cloud

Downscale_factor = 1;
if FASTCONFIG==1
   Downscale_factor = 2; 
   triands=3;
end
densePC_track=[];
cind=1;%counter for image pairs successful
for nn=1:Nimg-1
err2d_ave=0;err3d_ave=0;
for i=nn+1:Nimg
    fprintf('pair %d-%d\n',nn,i );
    if i~=nn
        %read tif images and rpc metadata files
        if i==nn+1
            im1=imread(imgpath{nn});%(imread(strcat(pathname,imgnames(i+2).name)));
            rpc_param_1=dlmread(rpcpath{nn});
            im2=imread(imgpath{i});
            rpc_param_2=dlmread(rpcpath{i});

        else
            im2=imread(imgpath{i});
            rpc_param_2=dlmread(rpcpath{i});
        end
        
        
        %%%%%RPC parameter setup%%%%%%%
        globalposc1 = rpc_param_1(95);
        globalposr1 = rpc_param_1(96);
        globalposc2 = rpc_param_2(95);
        globalposr2 = rpc_param_2(96);        
        p1_1=rpc_param_1(11:30);p2_1=rpc_param_1(31:50);
        p3_1=rpc_param_1(51:70);p4_1=rpc_param_1(71:90);
        p1_2=rpc_param_2(11:30);p2_2=rpc_param_2(31:50);
        p3_2=rpc_param_2(51:70);p4_2=rpc_param_2(71:90);
        scale_offsets_1 = [rpc_param_1(8) rpc_param_1(3) ...
            rpc_param_1(9) rpc_param_1(4) rpc_param_1(10) rpc_param_1(5) ...
            rpc_param_1(1) rpc_param_1(2) rpc_param_1(6) rpc_param_1(7)];
        scale_offsets_2 = [rpc_param_2(8) rpc_param_2(3) ...
            rpc_param_2(9) rpc_param_2(4) rpc_param_2(10) rpc_param_2(5) ...
            rpc_param_2(1) rpc_param_2(2) rpc_param_2(6) rpc_param_2(7)];

        pair.matches=[];
        [mn]=size(im1);
        m=mn(1);
        n=mn(2);
        %fix the random seed for result replication (ransac)
%       s = RandStream('mcg16807','Seed',10); 
%       RandStream.setGlobalStream(s);

        
        %%% sift match and planar rectification
        fprintf('start planar rectification\n');
        tic;
        pair = match2viewSIFT(im2single(im1),im2single(im1),im2single(im2),FASTCONFIG);
        m1=[pair.matches(1:2,:);ones(1,size(pair.matches,2))];
        m2= [pair.matches(3:4,:);ones(1,size(pair.matches,2))];
        [H, inliers] = ransacfithomography_vgg(m1, m2, 0.005);
        invH=inv(H);            
        toc;

        %Block adjust of RPC params
        %it estimates a Dx and Dy offsets that reduce the projection error
        %due to RPC metadata
        if BlockAdjust==1
            fprintf('start BlockAdjust\n');
            tic;
            for in=1:length(inliers)                    
                r1 = globalposr1 + pair.matches(2,inliers(in));
                c1 = globalposc1 + pair.matches(1,inliers(in));
                r2 = globalposr2 + pair.matches(4,inliers(in));
                c2 = globalposc2 + pair.matches(3,inliers(in));
                [Xu,Yu,Zu,err3d,err2d] = triangulationRPC(p1_1,p2_1,p3_1,p4_1,...
                    p1_2,p2_2,p3_2,p4_2,r1,c1,r2,c2,scale_offsets_1,...
                    scale_offsets_2,false);                
                sparsePC(in,1) = Xu;
                sparsePC(in,2) = Yu;
                sparsePC(in,3) = Zu;
                err2d_ave = err2d_ave + err2d;
                err3d_ave = err3d_ave + err3d;
                r1_a(in)=r1;
                c1_a(in)=c1;
                r2_a(in)=r2;
                c2_a(in)=c2;
            end
            
            err2d_ave = err2d_ave / length(inliers);
            err3d_ave = err3d_ave / length(inliers);
            err2d_ave_80=err2d_ave;
            err2d_ave=0;err3d_ave=0;

            [PDelta1,PDelta2]=RPCBlockAdjust(sparsePC', r1_a,c1_a,r2_a, ...
                c2_a, scale_offsets_1,scale_offsets_2,...
                p1_1, p2_1, p3_1, p4_1, ...
                p1_2, p2_2, p3_2, p4_2);

            %validation of 12 parameter triangulation
             for in=1:length(inliers)                    
                r1 = globalposr1 + pair.matches(2,inliers(in));
                c1 = globalposc1 + pair.matches(1,inliers(in));
                r2 = globalposr2 + pair.matches(4,inliers(in));
                c2 = globalposc2 + pair.matches(3,inliers(in));
                [Xu,Yu,Zu,err3d,err2d] = triangulationRPC_BA(p1_1,p2_1,p3_1,p4_1,...
                    p1_2,p2_2,p3_2,p4_2,PDelta1,PDelta2,r1,c1,r2,c2,scale_offsets_1,...
                    scale_offsets_2,false);                
%                     sparsePC(in,1),Xu,
%                     sparsePC(in,2),Yu,
%                     sparsePC(in,3),Zu,
                err2d_ave = err2d_ave + err2d;
                err3d_ave = err3d_ave + err3d;
            end
            err2d_ave = err2d_ave / length(inliers);
            err3d_ave = err3d_ave / length(inliers);
            fprintf('error reprojection improvement:%f,%f',err2d_ave_80,err2d_ave);
            toc;
        end
                
             
        %%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%dense OF%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%
        tic;
        fprintf('start OF\n');
        %warping second reference frame
        inH=inv(H);
        xtl = inH * [1;1;1];
        xtr = inH * [n;1;1];
        xbl = inH * [1;m;1];
        xbr = inH * [n;m;1];
        xtl_n = xtl./xtl(3);
        xtr_n = xtr./xtr(3);
        xbl_n = xbl./xbl(3);
        xbr_n = xbr./xbr(3);
        ma=ceil(max(xbl_n(2),xbr_n(2)));
        na=ceil(max(xtr_n(1),xbr_n(1)));
        if ma/m>3 || na/n>3 || ma<0 || na<0           
            if isempty(densePC_track)==0%not first pair?
                densePC_track(:,:,:,cind)=zeros(round(size(densePC_track,1)),round(size(densePC_track,2)),3);
            else
                densePC_track(:,:,:,cind)=zeros(round(size(im1,1)/triands),round(size(im1,2)/triands),3);
            end
            validpair(cind)=0;
            fprintf('Image Registration failed size\n');
        else            
            [jjj,iii]=meshgrid(1:na,1:ma);
            i_v=reshape(iii',[1 ma*na]);j_v=reshape(jjj',[1 ma*na]);
            xp_new = H*[j_v;i_v;ones(1,ma*na)];
            xp_new=round(xp_new./repmat(xp_new(3,:),[3 1]));            
            posgrid=ones(1,ma*na);
            posgrid(xp_new(1,:)<1 | xp_new(1,:)>size(im2,2))=0;
            posgrid(xp_new(2,:)<1 | xp_new(2,:)>size(im2,1))=0;
            imt2=zeros(ma,na);
            ppo=find(posgrid==1);      
            cnt=1;
            for iii=1:ma
                for jjj=1:na
                    if posgrid(cnt)==1
                        imt2(iii,jjj) = im2(round(xp_new(2,cnt)),round(xp_new(1,cnt)),1);
                    end                        
                    cnt=cnt+1;
                end
            end           
              
            % check validity of registration, if not good skip pair
            % computation
            [r2,c2]=find(imt2~=0);
            szim2r = max(r2)-min(r2);
            szim2c = max(c2)-min(c2);    
            difr=size(im1,1)/szim2r;
            if isempty(difr)==1
                difr=0;
            end
            difc=size(im1,2)/szim2c;
            if isempty(difc)==1
                difc=0;
            end
            %if planar registration failed skip current pair 
            %(failure is checked by the registered image boundaries
            %wrt reference image img1)
            if difr>1.3 || difr<0.83 || difc>1.3 || difc<0.83 
                if displayresults==1
                    anaglyph=zeros(max(size(im1,1),size(imt2,2)),max(size(im1,2),size(imt2,2)),3);
                    anaglyph(1:size(im1,1),1:size(im1,2),1)=im1;
                    anaglyph(1:size(imt2,1),1:size(imt2,2),2)=imt2;
                    anaglyph(1:size(imt2,1),1:size(imt2,2),3)=imt2;
                    figure, imshow(uint8(anaglyph)), axis on,grid on,title('registration result failed!')
                end
                  
                if isempty(densePC_track)==0
                  densePC_track(:,:,:,cind)=zeros(round(size(densePC_track,1)),round(size(densePC_track,2)),3);
                else
                  densePC_track(:,:,:,cind)=zeros(round(size(im1,1)/triands),round(size(im1,2)/triands),3);
                end
                validpair(cind)=0;
                fprintf('Image Registration failed\n');
            else
                if displayresults==1
                  anaglyph=zeros(max(size(im1,1),size(imt2,2)),max(size(im1,2),size(imt2,2)),3);
                  anaglyph(1:size(im1,1),1:size(im1,2),1)=im1;
                  anaglyph(1:size(imt2,1),1:size(imt2,2),2)=imt2;
                  anaglyph(1:size(imt2,1),1:size(imt2,2),3)=imt2;
                  figure, imshow(uint8(anaglyph)), axis on,grid on,title('registration result')
                end
                im1of=im1;im2of=imt2;
                %start sift flow 
                cellsize=1:2;%3
                gridspacing=1;
                sift1 = mexDenseSIFT(imresize(im1of,1/Downscale_factor,'bicubic'),cellsize,gridspacing);
                sift2 = mexDenseSIFT(imresize(im2of,1/Downscale_factor,'bicubic'),cellsize,gridspacing);
                %       alpha:        (0.01) the weight of the truncated L1-norm regularization on the flow
                %       d:            (1) the threshold of the truncation
                %       gamma:        (0.001) the weight of the magnitude of the flow
                %       nIterations:  (40) the number of iterations
                %       nHierarchy:   (2)  the number of hierarchies in the efficient BP implementation
                %       wsize:        (5)  the half size of the search window 
              
                if SMOOTHPC==1
                    SIFTflowpara.alpha=10*255;%the weight of the truncated L1-norm regularization on the flow
                    SIFTflowpara.d=40*255;%L1 truncation
                    SIFTflowpara.gamma=0.005*255;%the weight of the magnitude of the flow
                    SIFTflowpara.nlevels=8;
                    SIFTflowpara.wsize=4;
                    SIFTflowpara.topwsize=20;
                    SIFTflowpara.nTopIterations = 30;
                    SIFTflowpara.nIterations= 10;
                else
                    SIFTflowpara.alpha=2*255;%the weight of the truncated L1-norm regularization on the flow
                    SIFTflowpara.d=40*255;%L1 truncation
                    SIFTflowpara.gamma=0.005*255;%the weight of the magnitude of the flow
                    SIFTflowpara.nlevels=8;
                    SIFTflowpara.wsize=4;
                    SIFTflowpara.topwsize=10;
                    SIFTflowpara.nTopIterations = 30;
                    SIFTflowpara.nIterations= 10;
                end


                uvo=[];uvofw=[];
                [vx,vy,energylist]=SIFTflowc2f(sift1,sift2,SIFTflowpara,1,im1of);
                toc;
                uvofw(:,:,1)=Downscale_factor*imresize(vx,Downscale_factor,'bicubic');
                uvofw(:,:,2)=Downscale_factor*imresize(vy,Downscale_factor,'bicubic');

                if displayresults==1
                   figure,imshow(flowToColor(2*uvofw));axis on,grid on, title('Optical Flow results')
                end
                
                fprintf('start RPC triangulation\n');
                %%%%%%%%%%%%%%%%%%%%%
                %'RPC triangulation'%
                %%%%%%%%%%%%%%%%%%%%%
                lx=length(1:triands:size(im1of,2));
                ly=length(1:triands:size(im1of,1));
                ll=length(1:triands:size(im1of,1))*length(1:triands:size(im1of,2)); 
                densePC=zeros(ly,lx,3);               
                if BlockAdjust==1
                    err2d=0;err3d=0;
                    err2d_ave=0;err3d_ave=0;cn=0;
                    tic;jjn=0;iin=0;
                       for in=1:triands:size(im1of,1)            
                            iin=iin+1;jjn=0;
                            if (iin*lx+jjn>=(cn)*ll/10) && (iin*lx+jjn<(cn+1)*ll/10)
                               fprintf('done triangulating %d percent of points \n',(cn)*10)
                               cn=cn+1;
                            end
                            for jn=1:triands:size(im1of,2)
                                jjn=jjn+1;
                              match_im2_x = (jn+uvofw(in,jn,1));
                              match_im2_y = (in+uvofw(in,jn,2));
                              xp_new = H*[match_im2_x;match_im2_y;1];
                              xp_new = xp_new/(xp_new(3));
    %                         [xp_new(1) xp_new(2) xp_new(1)/xp_new(3) xp_new(2)/xp_new(3)]
                              r1 = globalposr1 + in;
                              c1 = globalposc1 + jn;
                              r2 = globalposr2 + xp_new(2);
                              c2 = globalposc2 + xp_new(1);
                              [xu,yu,zu,err3d] = triangulationRPC_BA(p1_1,p2_1,p3_1,p4_1,...
                                  p1_2,p2_2,p3_2,p4_2,PDelta1,PDelta2,r1,c1,r2,c2,scale_offsets_1,...
                                  scale_offsets_2,false);                     
                               densePC(iin,jjn,1) = xu;
                               densePC(iin,jjn,2) = yu;
                               densePC(iin,jjn,3) = zu;
                               err2d_ave=err2d_ave+err2d;
                               err3d_ave=err3d_ave+err3d;
                             end
                       end
                        
                         toc;
                else
                     err2d=0;err3d=0;
                     err2d_ave=0;err3d_ave=0;cn=0;                 
                     %coordinate match
                     err2d=0;err3d=0;
                     err2d_ave=0;err3d_ave=0;cn=0;cnn=1;
                     tic;
                     lx=length(1:triands:size(im1of,2));
                     ly=length(1:triands:size(im1of,1));
                     ru1=zeros(lx*ly,1);
                     cu1=zeros(lx*ly,1);
                     ru2=zeros(lx*ly,1);
                     cu2=zeros(lx*ly,1);
                     [jn,in] = meshgrid(1:triands:size(im1of,2),1:triands:size(im1of,1));
                     match_im2_x = jn+uvofw(1:triands:size(im1of,1),1:triands:size(im1of,2),1);
                     match_im2_y = in+uvofw(1:triands:size(im1of,1),1:triands:size(im1of,2),2);
                     match_im2_x = reshape(match_im2_x',[1 ly*lx]);
                     match_im2_y = reshape(match_im2_y',[1 ly*lx]);
                     xp_new=H*[match_im2_x;match_im2_y;ones(1,ly*lx)];
                     xp_new=xp_new./repmat(xp_new(3,:),[3 1]);
                     ru1 = globalposr1+reshape(in',[1 ly*lx]);
                     cu1 = globalposc1+reshape(jn',[1 ly*lx]);
                     ru2 = globalposr2+xp_new(2,:);
                     cu2 = globalposc2+xp_new(1,:);

                     cnn=lx*ly;
                     
                     tic;
                     [Xu,Yu,Zu,error3d,error2d] = triangulationRPC_array(p1_1, ...
                         p2_1,p3_1,p4_1,p1_2,p2_2,p3_2,p4_2,ru1',cu1',ru2',cu2', ...
                         scale_offsets_1,scale_offsets_2,verbose,LM);
                     toc;
                    %store intermediate point clouds
                    densePC(:,:,1)=reshape(Xu',[lx ly])';
                    densePC(:,:,2)=reshape(Yu',[lx ly])';
                    densePC(:,:,3)=reshape(Zu',[lx ly])';
                   
                end
                
                Mim2=ly;Nim2=lx;
                if displayresults==1                
                 %pixel reprojection error
                  err2d_ave = sum(error2d(:)) / (size(densePC,1)*size(densePC,2))
                  %discrepancy in 3D point projection
                  err3d_ave = sum(error3d(:)) / (size(densePC,1)*size(densePC,2))
                end
                if cind==1
                    Mim1=size(densePC,1);
                    Nim1=size(densePC,2);
                    densePC_track(:,:,:,cind) = densePC;
                    [x,y]=meshgrid(1:Nim1,Mim1:-1:1);
                    tri = delaunay(x,y);
%                         figure(2*cind);subplot(1,2,2),trisurf(tri, x, y,  densePC(:,:,3));light;lighting phong;
%                         camlight('left');shading interp;title('dem');axis equal
                end
                posy = min(Mim2,min(Mim1,size(im1of,1)));
                posx = min(Nim2,min(Nim1,size(im1of,2)));
                if cind>1
                    densePC_track(1:posy,1:posx,:,cind) = densePC(1:posy,1:posx,:);                
                    [x,y]=meshgrid(1:posx,posy:-1:1);
                    tri = delaunay(x,y);
%                         figure(2*cind);subplot(1,2,2),trisurf(tri, x, y,  densePC(1:posy,1:posx,3));light;lighting phong;
%                         camlight('left');shading interp;title('dem');axis equal
                end
                validpair(cind)=1;%set this image pair to valid (for final fusion)
                Haccum{cind}=H;%save the transformation for this pair
                uvofwaccum{cind} = uvofw; %save the optical flow (debug purposes)
            if displayresults==1
                %it will save the intermediate point cloud as well as
                %estimate its accuracy and completeness
                fprintf('write partial LAS file (lat,lon,elev and luma)\n');
                tic;
                fusedPCfinal=[];
                fusedPCfinal(:,:,1) = imresize(densePC(:,:,1),[size(im1,1) size(im1,2)]);
                fusedPCfinal(:,:,2) = imresize(densePC(:,:,2),[size(im1,1) size(im1,2)]);
                fusedPCfinal(:,:,3) = imresize(densePC(:,:,3),[size(im1,1) size(im1,2)]);
                fusedPCUTM_vec=[];
                [fusedPCUTM_vec(:,2),fusedPCUTM_vec(:,1),utmzone] = deg2utm(reshape(...
                fusedPCfinal(:,:,2),[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]), ...
                reshape(fusedPCfinal(:,:,1),[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]));
                luma_vec = reshape(im1,[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]);
                fusedPCUTM_vec(:,3)=reshape(fusedPCfinal(:,:,3),[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]);
                pp=find(fusedPCUTM_vec(:,3)~=INVALIDZ);
                pctemp1=fusedPCUTM_vec(pp,1);
                pctemp2=fusedPCUTM_vec(pp,2);
                pctemp3=fusedPCUTM_vec(pp,3);
                fusedPCUTM_vec=[];
                fusedPCUTM_vec(:,1) = pctemp1;
                fusedPCUTM_vec(:,2) = pctemp2;
                fusedPCUTM_vec(:,3) = pctemp3;
                pctemp3=luma_vec(pp);
                luma_vec=pctemp3;
                testobj3 = lasdata('../las/dummy.las');
                testobj3.scan_angle=int8(zeros(length(fusedPCUTM_vec(:,2)),1));
                testobj3.selection=logical(zeros(length(fusedPCUTM_vec(:,2)),1));
                testobj3.intensity=uint16(luma_vec);
                testobj3.z=fusedPCUTM_vec(:,3);
                testobj3.y=fusedPCUTM_vec(:,1);
                testobj3.x=fusedPCUTM_vec(:,2);
                testobj3.header.number_of_point_records=length(fusedPCUTM_vec(:,1));
                testobj3.header.min_x=min(testobj3.x);
                testobj3.header.max_x=max(testobj3.x);
                testobj3.header.min_y=min(testobj3.y);
                testobj3.header.max_y=max(testobj3.y);
                testobj3.header.min_z=min(testobj3.z);
                testobj3.header.max_z=max(testobj3.z);
                testobj3.header.x_offset=min(testobj3.x);
                testobj3.header.y_offset=min(testobj3.y);
                testobj3.header.z_offset=min(testobj3.z);
                write_las(testobj3, [resultpathpartial num2str(cind) '.las']);
                toc;
               if evaluation==1
                 fprintf('run challenge metrics\n');
                 tic;
                 command=([metricsexecpath ' -t ' gtpath ' -i ' ...
                    [resultpathpartial num2str(cind) '.las']]);
                 system(command)
                 toc;
               end
            end

            end
                
          end  
          cind=cind+1;%next image pair
            
    end
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%final fusion of all image pairs%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 fprintf('fuse individual dems\n');
 im1=imread(imgpath{1});%(imread(strcat(pathname,imgnames(i+2).name)));
 
%retrieve all valid pairs (planar registration was successful)
tic; 
pp=1;
posvalid=[];   
for inim=1:Nimg-1
    for jnim=inim+1:Nimg
        if validpair(pp)==1
             posvalid=[posvalid;pp];
        end
        pp=pp+1;
    end
end
%initialization
densePC_track_warp=densePC_track;
fusedPC=densePC_track_warp(:,:,:,posvalid(1));
%transform to reference grid (first valid image used)
fprintf('register all point clouds to first valid image lattice\n');
pp=1;
posvalid=[];
 for inim=1:Nimg-1
     densePC_track_warp(:,:,:,inim)=densePC_track(:,:,:,inim);     
    for jnim=inim+1:Nimg        
        latticereference=zeros(Mim1,Nim1,3);
        if inim==1
            H1=eye(3);
            offield=zeros(Mim1*triands,Nim1*triands,2);
        else
            H1=(Haccum{inim-1});
            offield=uvofwaccum{inim-1};
        end
        
        if isempty(H1)~=1
            val(1)=densePC_track_warp(1,1,1,1);
            val(2)=densePC_track_warp(1,1,2,1);
            val(3)=densePC_track_warp(1,1,3,1);
            
            for ik=1:triands:Mim1*triands
                for jk=1:triands:Nim1*triands
                    
                    pos_w = H1*[jk;ik;1];
                    pos_w = pos_w/pos_w(3);
                    % pos_w=pos_w+of;
                    pos_w(2)=pos_w(2)+offield(min(size(offield,1),ik),min(size(offield,2),jk),2);%row
                    pos_w(1)=pos_w(1)+offield(min(size(offield,1),ik),min(size(offield,2),jk),1);%col
                    val(1)=densePC_track_warp(max(1,round(ik/triands)),max(1,round(jk/triands)),1,inim);
                    val(2)=densePC_track_warp(max(1,round(ik/triands)),max(1,round(jk/triands)),2,inim);
                    val(3)=densePC_track_warp(max(1,round(ik/triands)),max(1,round(jk/triands)),3,inim);
                    pos_w=pos_w/triands;
                    if round(pos_w(2))>0 && round(pos_w(1))>0 && round(pos_w(2))<Mim1+1 && round(pos_w(1))<Nim1+1
                        latticereference(max(1,round(ik/triands)),max(1,round(jk/triands)),1) = ...
                            densePC_track(round(pos_w(2)),round(pos_w(1)),1,pp);
                        latticereference(max(1,round(ik/triands)),max(1,round(jk/triands)),2) = ...
                            densePC_track(round(pos_w(2)),round(pos_w(1)),2,pp);
                        latticereference(max(1,round(ik/triands)),max(1,round(jk/triands)),3) = ...
                            densePC_track(round(pos_w(2)),round(pos_w(1)),3,pp);
                        val = latticereference(max(1,round(ik/triands)),max(1,round(jk/triands)),:);
                    else
                        latticereference(max(1,round(ik/triands)),max(1,round(jk/triands)),1) = val(1);
                        latticereference(max(1,round(ik/triands)),max(1,round(jk/triands)),2) = val(2);
                        latticereference(max(1,round(ik/triands)),max(1,round(jk/triands)),3) = val(3);
                    end                     
                end
            end
            if validpair(pp)==1
                posvalid=[posvalid;pp];
            end
        end
      
        densePC_track_warp(:,:,:,pp)=latticereference; 
       
        pp=pp+1;
    end
 end
%plane sweep to find best z match
fprintf('minimize SAD difference for all registered point clouds\n');
refplane=densePC_track_warp(:,:,3,1);
minerror=10000000000;
offset=0;
for inum=1:length(posvalid)
    minerror=10000000000;
    for lev=-40:0.2:40
        error=sum(sum(abs(densePC_track_warp(:,:,3,posvalid(inum))+ ...
            repmat(lev,[size(densePC_track_warp,1) size(densePC_track_warp,2)])- ...
            refplane)))/(size(densePC_track_warp,1)*size(densePC_track_warp,2));
        if error<minerror
            offset=lev;
            minerror=error;
        end
    end    
    densetemp=densePC_track_warp(:,:,3,posvalid(inum));
    pp=find(densetemp~=INVALIDZ);
    densetemp(pp) = densetemp(pp)+offset;
    densetemp(find(densetemp<-100))=INVALIDZ;
    densePC_track_warp(:,:,3,posvalid(inum))=densetemp;
end
fprintf('point cloud fusion\n');
%median 
fusedPC(:,:,1) = median(densePC_track_warp(:,:,1,posvalid),4);
fusedPC(:,:,2) = median(densePC_track_warp(:,:,2,posvalid),4);
fusedPC(:,:,3) = median(densePC_track_warp(:,:,3,posvalid),4);
%resize to image dimensions in case downsampled point cloud
fusedPCfinal=[];
if Downscale_factor==1
    fusedPCfinal(:,:,1) = imresize(fusedPC(:,:,1),[size(im1,1) size(im1,2)]);%Lon
    fusedPCfinal(:,:,2) = imresize(fusedPC(:,:,2),[size(im1,1) size(im1,2)]);%Lat
    fusedPCfinal(:,:,3) = imresize(fusedPC(:,:,3),[size(im1,1) size(im1,2)]);%Ele
else
    fusedPCfinaltemp(:,:,1) = imresize(fusedPC(:,:,1), ...
        [size(im1,1)+triands^2 size(im1,2)+triands^2]);%Lon
    fusedPCfinaltemp(:,:,2) = imresize(fusedPC(:,:,2), ...
        [size(im1,1)+triands^2 size(im1,2)+triands^2]);%Lat
    fusedPCfinaltemp(:,:,3) = imresize(fusedPC(:,:,3), ...
        [size(im1,1)+triands^2 size(im1,2)+triands^2]);%Ele
    
    fusedPCfinal(:,:,1)=fusedPCfinaltemp(1:size(im1,1),1:size(im1,2),1);
    fusedPCfinal(:,:,2)=fusedPCfinaltemp(1:size(im1,1),1:size(im1,2),2);
    fusedPCfinal(:,:,3)=fusedPCfinaltemp(1:size(im1,1),1:size(im1,2),3);
end

if displayresults==1    
    [x,y]=meshgrid(1:size(fusedPCfinal,2),size(fusedPCfinal,1):-1:1);
    tri = delaunay(x,y);
    figure;trisurf(tri, x, y, fusedPCfinal(:,:,3));light;lighting phong;
    camlight('left');shading interp;title('fused dem');axis equal
    % intermediate pairs
         for iji=1:size(densePC_track,4)
           [x,y]=meshgrid(1:size(densePC_track,2),size(densePC_track,1):-1:1);
           tri = delaunay(x,y);
           figure,trisurf(tri, x, y, densePC_track(:,:,3,iji));light;lighting phong;camlight('left');shading interp;axis equal;grid on 
         end     
end
    
toc;

%convert to utm
fprintf('convert to utm\n');
tic;
fusedPCUTM_vec=[];
[fusedPCUTM_vec(:,2),fusedPCUTM_vec(:,1),utmzone] = deg2utm(reshape(...
    fusedPCfinal(:,:,2),[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]), ...
    reshape(fusedPCfinal(:,:,1),[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]));
luma_vec = reshape(imresize(im1,[size(fusedPCfinal,1) size(fusedPCfinal,2)]), ...
    [size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]);
toc;

%write geotiff
%write geotiff
geotiffwrite(resultpathgeotiff, ...
    reshape(fusedPCfinal(:,:,3),[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]), ...
    [reshape(fusedPCfinal(:,:,1),[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]) ...
    reshape(fusedPCfinal(:,:,2),[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1])]);

%write las
fprintf('write LAS file (lat,lon,elev and luma)\n');
tic;
fusedPCUTM_vec(:,3)=reshape(fusedPCfinal(:,:,3),[size(fusedPCfinal,1)*size(fusedPCfinal,2) 1]);
%only valid points
pp=find(fusedPCUTM_vec(:,3)~=INVALIDZ & fusedPCUTM_vec(:,3)>(INVALIDZ+max(max(fusedPCfinal(:,:,3))))/2);
pctemp1=fusedPCUTM_vec(pp,1);
pctemp2=fusedPCUTM_vec(pp,2);
pctemp3=fusedPCUTM_vec(pp,3);
fusedPCUTM_vec=[];
fusedPCUTM_vec(:,1) = pctemp1;
fusedPCUTM_vec(:,2) = pctemp2;
fusedPCUTM_vec(:,3) = pctemp3;
pctemp3=luma_vec(pp);
luma_vec=pctemp3;


testobj3 = lasdata('../las/dummy.las');%open to overwrite the fields we care
testobj3.scan_angle=int8(zeros(length(fusedPCUTM_vec(:,2)),1));
testobj3.selection=logical(zeros(length(fusedPCUTM_vec(:,2)),1));
testobj3.intensity=uint16(luma_vec);
testobj3.z=fusedPCUTM_vec(:,3);
testobj3.y=fusedPCUTM_vec(:,1);
testobj3.x=fusedPCUTM_vec(:,2);
testobj3.header.number_of_point_records=length(fusedPCUTM_vec(:,1));
testobj3.header.min_x=min(testobj3.x);
testobj3.header.max_x=max(testobj3.x);
testobj3.header.min_y=min(testobj3.y);
testobj3.header.max_y=max(testobj3.y);
testobj3.header.min_z=min(testobj3.z);
testobj3.header.max_z=max(testobj3.z);
testobj3.header.x_offset=min(testobj3.x);
testobj3.header.y_offset=min(testobj3.y);
testobj3.header.z_offset=min(testobj3.z);
write_las(testobj3, resultpath);
toc;
if evaluation==1                         
    fprintf('run challenge metrics\n');
    tic;
    command=([metricsexecpath ' -t ' gtpath ' -i ' resultpath]);
    system(command)
    toc;
end
