source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/LearningCode/Inference/ReportPlaneParaMRF_Conditioned.m @ 37

% *  This code was used in the following articles:
% *  [1] Learning 3-D Scene Structure from a Single Still Image, 
% *      Ashutosh Saxena, Min Sun, Andrew Y. Ng, 
% *      In ICCV workshop on 3D Representation for Recognition (3dRR-07), 2007.
% *      (best paper)
% *  [2] 3-D Reconstruction from Sparse Views using Monocular Vision, 
% *      Ashutosh Saxena, Min Sun, Andrew Y. Ng, 
% *      In ICCV workshop on Virtual Representations and Modeling 
% *      of Large-scale environments (VRML), 2007. 
% *  [3] 3-D Depth Reconstruction from a Single Still Image, 
% *      Ashutosh Saxena, Sung H. Chung, Andrew Y. Ng. 
% *      International Journal of Computer Vision (IJCV), Aug 2007. 
% *  [6] Learning Depth from Single Monocular Images, 
% *      Ashutosh Saxena, Sung H. Chung, Andrew Y. Ng. 
% *      In Neural Information Processing Systems (NIPS) 18, 2005.
% *
% *  These articles are available at:
% *  http://make3d.stanford.edu/publications
% * 
% *  We request that you cite the papers [1], [3] and [6] in any of
% *  your reports that uses this code. 
% *  Further, if you use the code in image3dstiching/ (multiple image version),
% *  then please cite [2].
% *  
% *  If you use the code in third_party/, then PLEASE CITE and follow the
% *  LICENSE OF THE CORRESPONDING THIRD PARTY CODE.
% *
% *  Finally, this code is for non-commercial use only.  For further 
% *  information and to obtain a copy of the license, see 
% *
% *  http://make3d.stanford.edu/publications/code
% *
% *  Also, the software distributed under the License is distributed on an 
% * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either 
% *  express or implied.   See the License for the specific language governing 
% *  permissions and limitations under the License.
% *
% */
function  ReportPlaneParaMRF_Conditioned(Default, Sup, SupOri,depthMap,VarMapRaw, ...
                RayOri, Ray, SupNeighborTable, maskSky,maskG, MultiScaleSupTable, ...
                StraightLineTable);
% This function runs the RMF over the plane parameter of each superpixels

% Input:
% Default- all default parameters
% Sup - Superpixel, in 2-d matrix. 
% SupOri - original superpixel before cleaning
% DepthMap - DepthMap from Learning or coarse measuring
% VarMap - confidance for DepthMap (latest algo VarMap used simply one since the learning not generalized well)
% RayOri - Rays without stiching to the boundary.
% Ray - rays after stiching
% SupNeighborTable - A lookup table for superpixels' neighbors (2-d sparse matrix)
% maskSky - Sky mask
% maskG - Ground mask
% MultiScaleSupTable - multiple scale segmentation, used to define the weights betwenn the stiching terms.
% StraightLineTable - straight line stiching, (not used in this version, but might be very usefulif implemented).

% ------------------------------------------Finish Input Definition ------------------------------------------------
% Parameter setting
% 1) Functional parameters
FlagRemoveVerticalSupport = 1; % Enable Removing Vertical Support Depth at the second inference (risky if occluded)
FlagEnableVarMap = 0; % Disabled 
gravity =true; % if true, apply the HoriConf and VertConf linear scale weight
CoPST = true; % if true, apply the Straight line prior as the Co-Planar constrain
% ============= Magic Number =============
StickHori = 5;  %0.1; % sticking power in horizontal direction
StickVert = 10;     % sticking power in vertical direction
Center = 2; % Co-Planar weight at the Center of each superpixel
HoriConf = 1; % set the confidant of the learned depth at the middle in Horizontal direction of the image
VertConf = 0.01; % set the confidant of the learned depth at the top of the image
BandWith = 1; % Nov29 1 Nov30 0.1 check result change to 0.1 12/1 0.1 lost detail
ClosestDist = 1; % define the closest distance that the MRF allows
FarestDist = 1.5*median(depthMap(:)); % tried on university % nogood effect but keep it since usually it the rangelike this   % change to 1.5 for church        
% The hand-tuned 14 dimensional vector correspose to the weights of 14 multiple segmentation
if ~isempty(MultiScaleSupTable)
        MultiScaleFlag = true;  % multiple segmentaition hypothesis
        WeiV = 2*ones(1,size(MultiScaleSupTable,2)-1);
else
        MultiScaleFlag = false;
        WeiV = 1;
end
WeiV(1,1:2:end) = 6; % emphasize the middle scale three times smaller than large scale
WeiV = WeiV./sum(WeiV);% normalize if pair of superpixels have same index in all the scale, their weight will be 10
ShiftStick = -.1;  % between -1 and 0, more means more smoothing.
ShiftCoP = -.5;  % between -1 and 0, more means more smoothing.
 % =========================================================================
% If you go up in the vertical direction in the image, the weights change in the vertical direction.
groundThreshold = cos([ zeros(1, Default.VertYNuDepth - ceil(Default.VertYNuDepth/2)+10) ...
                                linspace(0,15,ceil(Default.VertYNuDepth/2)-10)]*pi/180);
%  v1 15 v2 20 too big v3 20 to ensure non misclassified as ground.
%  verticalThreshold = cos(linspace(5,55,Default.VertYNuDepth)*pi/180); % give a vector of size 55 in top to down : 
verticalThreshold = cos([ 5*ones(1,Default.VertYNuDepth - ceil(Default.VertYNuDepth/2)) ...
                                linspace(5,55,ceil(Default.VertYNuDepth/2))]*pi/180); 
% give a vector of size 55 in top to down : 
% 50 means suface norm away from y axis more than 50 degree
% =========================================================================
ceiling = 0*Default.VertYNuDepth; % set the position of the ceiling, related to No plane coming back constrain % changed for newchurch
% ^^^^^This number means the lowest row that planes may go back just like a ceiling
% ============= End of Magic Number ======

% 2) Opimization parameters
% ============== parameters for the decomposition problem
% Optimal parameters for current code:
% For one machine: Sedumi: (1,1)        Lpsolve:(3,1)
% Multiple machines: Sedumi: (4,1)      LPsolve:(3,1)
% lpsolve running time is 22 seconds for (4,2) arrangement; but numerical
% accuracy needs to be resolved first.
XNuDecompose = 1; % up to 3 is stable      
YNuDecompose = 1;
% ============ parameters for the decomposition problem
solverVerboseLevel = 0; % set to 1 if you need msg out when solving optimization problem
NoSecondStep = 0; % set to 1 if only want first level opt = no second step of vertical and horizontal objective

% 3) Debug and evaluation paramters
ExtractRelationInfo = 1; % set to 1 if need to storage the coplanar and connectivity weight to analyze

% 4) Rendering parameters
GridFlag = 0; % set to 1 if the wrl need grid of the triangle overlay

% ==========================End of parameter setting ====================================
inferenceTime = tic;
fprintf(['\n     : Building Matrices....       ']);

% ======= intermediant Data =====================================================================
% confidence of the supporting depth changes according to the row and column =========
mapVert = linspace(VertConf,1,Default.VertYNuDepth); % modeling the gravity prior:  the topper the row is the lower the confidence of supporting depths
mapHori = [linspace(HoriConf,1,round(Default.HoriXNuDepth/2)) fliplr(linspace(HoriConf,1,Default.HoriXNuDepth-round(Default.HoriXNuDepth/2)))];
% the more peripheral column the lower the confidence of supporting depths
% ====================================================================================
% assign the confidance of the supporting depths
if FlagEnableVarMap
        VarMap = zeros( size(VarMapRaw));
else
        VarMap = VarMapRaw;
end
CleanedDepthMap = depthMap;
CleanedDepthMap(depthMap>FarestDist) = NaN; % set the supporting depths ti NaN means that depth is not effective in OPT, so we simply ignore the depth which is too far
Posi3D = im_cr2w_cr(CleanedDepthMap,permute(RayOri,[2 3 1])); % given the depth and ray as input, calculate the 3-d coordinate at each point.

% Clean the Sup near sky
maskSky = Sup == 0;
maskSkyEroded = imerode(maskSky, strel('disk', 4) );
SupEpand = ExpandSup2Sky(Sup,maskSkyEroded); % extend the sky using the cloesest Sup index, which mean the some part of the sky will be include in the sup but their supporting depth will not be used.

NuSup = setdiff(unique(Sup)',0); % unique index of sup (not including sky index which is '0')
NuSupSize = size(NuSup,2); % number of uniquew sup

% Sup index and planeParameter index inverse map ======= useful tool======
% since sup index is not continuous, but the Parameter index over sup is continuous. Sup2Para is the maping stand for sup index to Parameter index
Sup2Para = sparse(1,max(Sup(:))); 
Sup2Para(NuSup) = 1:NuSupSize;
% ========================================================================

% =================please ignore here since StraightLineTable = [], NuLine = 0 ========
% constructiion of the Straight line prior matrix Will be add in the CoPlane matrix
NuLine = size(StraightLineTable,2);
CoPSTList = [];

% effectively not running the straight line constraint here.
for i = 1:NuLine
        L = StraightLineTable{i};
        X = L(1:(end-1))';
        Y = L(2:end)';
        if isempty(X)
                continue;
        end
        for j = 1:size(X,1)
                if X(j)~=Y(j)
                        CoPSTList = [CoPSTList; X(j) Y(j)];
                end
        end
end
% Please ignore ===============================================================

% ============end of generating intermediant Data ===============================================

% Generate the Matrix for MRF ==============================================================
PosiM = sparse(0,0); % supporting depth times ray terms
VarM = sparse(0,0); % confidence terms
RayAllM = sparse(0,0); % collection of every ray

% keep record of the center of the sup is lower then the ceiling
YPointer = []; % set to one if lower then the ceiling
YPosition = []; % keep a record of the row of the center of the sup, useful when enforcing vertical and horizontal constrain

for i = NuSup
        mask = SupEpand ==i; % include the Ray that will be use to expand the NonSky
        RayAllM = blkdiag( RayAllM, Ray(:,mask)'); % RayAllM will be putting on constrain of every ray that will be rendered, so should use SupEpand as mask

        mask = Sup ==i; % Not include the Ray that will be use to expand the NonSky    
        [yt x] = find(mask);
        
        % find the center point of the sup
        CenterX = round(median(x));
        CenterY = round(median(yt));

        % if CenterY is lower than ceiling, then set YPointer for that sup to 1 so that it will not come back like a ceiling
        YPointer = [YPointer; CenterY >= ceiling]; 

        % the horizontal and vertical enforcing in the second step OPT will depends on the height of the sup, so keep record of CenterY
        YPosition = [YPosition; CenterY];

        % Not building PosiM and VarM, have to get rid of NaN depths
        mask(isnan(CleanedDepthMap)) = false;
        SupNuPatch(i) = sum(mask(:));
    
        % find the center point of the sup
        [yt x] = find(mask); % notice VarM depends on the position of mask if gravity == 1
  
        if ~all(mask(:)==0)
                if gravity
                        if any(CleanedDepthMap(mask) <=0)
                                CleanedDepthMap(mask)
                        end
                        PosiM = blkdiag(PosiM,Posi3D(:,mask)');%.*repmat( mapVert(yt)',[1 3]).*repmat( mapHori(x)',[1 3]));
                        VarM = [VarM; VarMap(mask).*(mapVert(yt)').*( mapHori(x)')];
                else
                        PosiM = blkdiag(PosiM,Posi3D(:,mask)');
                        VarM = [VarM; VarMap(mask)];
                end
        else
                PosiM = blkdiag(PosiM, Posi3D(:,mask)');
                VarM = [VarM; VarMap(mask)];
        end
end
% set 0 to -1 since this will make the sup higher than the ceiling enforce to come back, if you do not want this just keep 0 as 0
YPointer(YPointer==0) = -1;

% buliding CoPlane Matrix=========================================================================

% ===============please ignore here since CoPSTList = [], ===============
NuSTList = 0;
if CoPST
        NuSTList = size(CoPSTList,1);
        if ~isempty(CoPSTList)
                [V H] = size(SupNeighborTable);
                SupNeighborTable( CoPSTList(:,1)*V + CoPSTList(:,2)) = 1;
        SupNeighborTable( CoPSTList(:,2)*V + CoPSTList(:,1)) = 1;
        end
end
% ================please ignore==============================================

CoPM1 = sparse(0,3*NuSupSize);
CoPM2 = sparse(0,3*NuSupSize);
CoPEstDepth = sparse(0,0);
CoPNorM = [];
WeiCoP = [];
if ExtractRelationInfo == 1
        % keeps the Wei of the relational Coplanar term here
        WeiM = sparse(max(NuSup),max(NuSup));
end

for i = NuSup
        % pick the i's neightbot using SupNeighborTable
        Neighbor = find( SupNeighborTable(i,:) ~=0);
        Neighbor = Neighbor( Neighbor> i);
        % setup the relation between sup_i and sup_j since they are neighbors
        for j = Neighbor
                mask = Sup == i;
                SizeMaskAll = sum(mask(:));
                [y x] = find(mask);
                % Coplanar term only use the center ray of each sup to enfore the cost
                CenterX = round(median(x));
                CenterY = round(median(y));
                y = find(mask(:,CenterX));
                if ~isempty(y)
                        CenterY = round(median(y));
                end
                temp1 = sparse(1, 3*NuSupSize);
                temp2 = sparse(1, 3*NuSupSize);
                temp1(:,(Sup2Para( i)*3-2): Sup2Para( i)*3) = ...
                      RayOri(:,CenterY,CenterX)';
                temp2(:,(Sup2Para( j)*3-2): Sup2Para( j)*3) = ...
                      RayOri(:,CenterY,CenterX)';

                % assign wei for each pairs of neighbors
                if MultiScaleFlag 
                        vector = (MultiScaleSupTable(Sup2Para( i),2:end) == MultiScaleSupTable(Sup2Para( j),2:end));
                        expV = exp(-10*(WeiV*vector' + ShiftCoP) );
                        wei = 1/(1+expV);
                else
                        wei = 1;
                end
        
                if ExtractRelationInfo == 1; % keep record
                        WeiM(i,j) = wei;
                end

                oneRay1 = temp1*wei;
                oneRay2 = temp2*wei;
    
                tempWeiCoP = [SizeMaskAll];
                CoPEstDepth = [CoPEstDepth; max(median(CleanedDepthMap(mask)),ClosestDist)];
    
                mask = Sup == j;
                SizeMaskAll = sum(mask(:));
                [y x] = find(mask);
                
                % Coplanar term only use the center ray of each sup to enfore the cost
                CenterX = round(median(x));
                CenterY = round(median(y));
                y = find(mask(:,CenterX));
                if ~isempty(y)
                        CenterY = round(median(y));
                end

                temp1 = sparse(1, 3*NuSupSize);
                temp2 = sparse(1, 3*NuSupSize);
                temp1(:,(Sup2Para( i)*3-2): Sup2Para( i)*3) = ... 
                      RayOri(:,CenterY,CenterX)';
                temp2(:,(Sup2Para( j)*3-2): Sup2Para( j)*3) = ...
                      RayOri(:,CenterY,CenterX)';
    
                % Instead of having separate L-1 terms for symmetric co-planar constraint; do the following:
                % If the penaly was ||.||_2^2 + ||.||_2^2; then the co-efficients are
                % some kind of average of two rays.  For one norm; we take its average.
                % (do not divide by 2 because the penalty should be double.
    
                CoPM1 = [CoPM1; temp1*wei + oneRay1 ];
                CoPM2 = [CoPM2; temp2*wei + oneRay2 ];
    
                tempWeiCoP = [tempWeiCoP; SizeMaskAll];
                WeiCoP = [WeiCoP; tempWeiCoP];
                CoPEstDepth = [CoPEstDepth; max(median(CleanedDepthMap(mask)),ClosestDist)];
        end
end%=========================================================================================================

%== find the boundary point that might need to be stick ot each other==========================================
HoriStickM_i = sparse(0,3*NuSupSize);
HoriStickM_j = sparse(0,3*NuSupSize);
HoriStickPointInd = [];
EstDepHoriStick = [];

MAX_POINTS_STITCH_HORI = 2;    % the actual code will be modified in another copy of the file
MIN_POINTS_STITCH = 2;      % ERROR:  not used.

% ================================================================
% NOTE: The actual algorithm should be picking precisely 2 points which are
% FARTHEST away from the candidate set of neighbors.  This algorithm
% will ALWAYS work and produce no surprising results.

% In some cases, one may experiment with picking only 1 point when the 2
% points are too close ---- this will make the algorithm faster; but might
% produce surprising (e.g. a triangle sticking out) sometimes.
% An ideal algorithm will reduce the number of points by checking for loops
% passing through 3 or less superpixels through this matrix; and removing
% them such that the smallest loop passes through 4 superpixels. (see EE263
% for a quick algorithm to do this -- involves product of matrices.
% ==================================================================

DIST_STICHING_THRESHOLD_HORI = 0.4;    
DIST_STICHING_THRESHOLD_HORI_ONLYCOL = -0.5;    % effectively not used, 

% Ashutosh added to reduce linear dependancy of the objective =======
SupPixelNeighborList = sparse( max(Sup(:)), max(Sup(:)) );
SupPixelParsedList = sparse( max(Sup(:)), max(Sup(:)) );
recordAdded1 = sparse( max(Sup(:)), max(Sup(:)) );
recordAdded2 = sparse( max(Sup(:)), max(Sup(:)) );
addedIndexList = [ ];
% ===================================================================

% locate sup boundary at horizontal and vertical dircetion only for stitching terms
BounaryPHori = conv2(Sup,[1 -1],'same') ~=0;
BounaryPHori(:,end) = 0;
BounaryPVert = conv2(Sup,[1; -1],'same') ~=0;
BounaryPVert(end,:) = 0;

%    boundariesHoriIndex = find(BounaryPHori==1)';
% pre-select the boundary in order with the NuSup order
for l = NuSup
        mask = Sup == l;
        boundariesHoriIndex = find(BounaryPHori==1 & mask)';
        for i = boundariesHoriIndex
                j = i+Default.VertYNuDepth;
                if Sup(i) == 0 || Sup(j) == 0
                        continue;
                end
                SupPixelParsedList(Sup(i),Sup(j)) = SupPixelParsedList(Sup(i),Sup(j)) + 1;
    
                if SupPixelNeighborList(Sup(i),Sup(j)) == 0
                        recordAdded1(Sup(i),Sup(j)) = i;     
                elseif SupPixelNeighborList(Sup(i),Sup(j)) >= MAX_POINTS_STITCH_HORI
                        continue;
                elseif SupPixelNeighborList(Sup(i),Sup(j)) == 1  % inside this remove the close stiching terms
                        rowN = ceil(i/55);      colN = rem(i,55);
                        rowN_older = ceil( recordAdded1(Sup(i),Sup(j)) / 55);
                        colN_older = rem( recordAdded1(Sup(i),Sup(j)), 55);
                        if abs(rowN - rowN_older) + (55/305)*abs(colN - colN_older) > DIST_STICHING_THRESHOLD_HORI && ...
                                abs(colN - colN_older) > DIST_STICHING_THRESHOLD_HORI_ONLYCOL
                                recordAdded2(Sup(i),Sup(j)) = i;
                        else
                                continue;
                        end
                elseif SupPixelNeighborList(Sup(i),Sup(j)) == 2     %Assuming MAX_POINTS_STITCH = 3
                        rowN = ceil(i/55);      colN = rem(i,55);
                        rowN_older1 = ceil( recordAdded1(Sup(i),Sup(j)) / 55);
                        colN_older1 = rem( recordAdded1(Sup(i),Sup(j)), 55);
                        rowN_older2 = ceil( recordAdded2(Sup(i),Sup(j)) / 55);
                        colN_older2 = rem( recordAdded2(Sup(i),Sup(j)), 55);
        
                        if abs(rowN - rowN_older1) + (55/305)*abs(colN - colN_older1) > DIST_STICHING_THRESHOLD_HORI && ...
                                abs(rowN - rowN_older2) + (55/305)*abs(colN - colN_older2) > DIST_STICHING_THRESHOLD_HORI
                                ;
                        else
                                continue;
                        end   
                end

                % If you come here, it means you are probably adding it.
                SupPixelNeighborList(Sup(i),Sup(j)) = SupPixelNeighborList(Sup(i),Sup(j)) + 1;
                addedIndexList = [addedIndexList i];
        end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%    this is supposed to be where the y and nu information are computed,
%%%%    keep an eye on this.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% after analysis finally add to HoriStickM_j
WeightHoriNeighborStitch = [ ];
for i = addedIndexList
        j = i+Default.VertYNuDepth;
        WeightHoriNeighborStitch = [WeightHoriNeighborStitch;  SupPixelParsedList(Sup(i),Sup(j)) / ...
                                    SupPixelNeighborList(Sup(i),Sup(j)) ];

        Target(1) = Sup2Para(Sup(i));
        Target(2) = Sup2Para(Sup(j));
        rayBoundary(:,1) =  RayOri(:,i);
        rayBoundary(:,2) =  RayOri(:,i);
        if MultiScaleFlag
                vector = (MultiScaleSupTable(Sup2Para(Sup(i)),2:end) == MultiScaleSupTable(Sup2Para(Sup(j)),2:end));
                expV = exp(-10*(WeiV*vector' + ShiftStick) );
                betaTemp = StickHori*(0.5+1/(1+expV));
                % therr should always be sticking (know don't care about occlusion)
        else
                betaTemp = StickHori;
        end
        temp = sparse(3,NuSupSize);
        temp(:,Target(1)) = rayBoundary(:,1);
        HoriStickM_i = [HoriStickM_i; betaTemp*temp(:)'];
        temp = sparse(3,NuSupSize);
        temp(:,Target(2)) = rayBoundary(:,2);
        HoriStickM_j = [HoriStickM_j; betaTemp*temp(:)'];
        EstDepHoriStick = [EstDepHoriStick; sqrt(max(CleanedDepthMap(i),ClosestDist)*max(CleanedDepthMap(j),ClosestDist))];
        HoriStickPointInd = [HoriStickPointInd i ];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%    end of y and nu computation.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ==============================================

% ======== finding the unucessary stiching points in Vertical direction ====
VertStickM_i = sparse(0,3*NuSupSize);
VertStickM_j = sparse(0,3*NuSupSize);
VertStickPointInd = [];
EstDepVertStick = [];

MAX_POINTS_STITCH_VERT = 4;     %3
DIST_STICHING_THRESHOLD_VERT = 0.1;     %0.3 
DIST_STICHING_THRESHOLD_VERT_ONLYCOL = -0.5;    % effectively not used, ideally should be 0.5; i.e., the point should be farther in col direction because that is the direction of the edge.

% Ashutosh added to reduce linear dependancy of the objective =======
SupPixelNeighborList = sparse( max(Sup(:)), max(Sup(:)) );
SupPixelParsedList = sparse( max(Sup(:)), max(Sup(:)) );
recordAdded1 = sparse( max(Sup(:)), max(Sup(:)) );
recordAdded2 = sparse( max(Sup(:)), max(Sup(:)) );
addedIndexList = [ ];
% ===============================

% pre-select the boundary in order with the NuSup order
for l = NuSup
        mask = Sup == l;
        for i = find(BounaryPVert==1 & mask)'
                j = i+1;
                if Sup(i) == 0 || Sup(j) == 0
                        continue;
                end
                SupPixelParsedList(Sup(i),Sup(j)) = SupPixelParsedList(Sup(i),Sup(j)) + 1;
    
                if SupPixelNeighborList(Sup(i),Sup(j)) == 0
                        recordAdded1(Sup(i),Sup(j)) = i;
                elseif SupPixelNeighborList(Sup(i),Sup(j)) >= MAX_POINTS_STITCH_VERT
                        continue;
                elseif SupPixelNeighborList(Sup(i),Sup(j)) == 1  % inside this remove the close stiching terms
                        rowN = ceil(i/55);      colN = rem(i,55);
                        rowN_older = ceil( recordAdded1(Sup(i),Sup(j)) / 55);
                        colN_older = rem( recordAdded1(Sup(i),Sup(j)), 55);
                        if abs(rowN - rowN_older) + (55/305)*abs(colN - colN_older) > DIST_STICHING_THRESHOLD_VERT && ...
                                abs(colN - colN_older) > DIST_STICHING_THRESHOLD_VERT_ONLYCOL
                                recordAdded2(Sup(i),Sup(j)) = i;
                        else
                                continue;
                        end
                elseif SupPixelNeighborList(Sup(i),Sup(j)) == 2     %Assuming MAX_POINTS_STITCH = 3
                        rowN = ceil(i/55);      colN = rem(i,55);
                        rowN_older1 = ceil( recordAdded1(Sup(i),Sup(j)) / 55);
                        colN_older1 = rem( recordAdded1(Sup(i),Sup(j)), 55);
                        rowN_older2 = ceil( recordAdded2(Sup(i),Sup(j)) / 55);
                        colN_older2 = rem( recordAdded2(Sup(i),Sup(j)), 55);
        
                        if abs(rowN - rowN_older1) + (55/305)*abs(colN - colN_older1) > DIST_STICHING_THRESHOLD_VERT && ...
                                abs(rowN - rowN_older2) + (55/305)*abs(colN - colN_older2) > DIST_STICHING_THRESHOLD_VERT
                                ;
                        else
                                continue;
                        end        
                end

                % If you come here, it means you are probably adding it.
                SupPixelNeighborList(Sup(i),Sup(j)) = SupPixelNeighborList(Sup(i),Sup(j)) + 1;
                addedIndexList = [addedIndexList i];
        end
end

% after analysis finally add to VertStickM_j
WeightVertNeighborStitch = [ ];
for i = addedIndexList
        j = i+1;
        WeightVertNeighborStitch = [WeightVertNeighborStitch;  SupPixelParsedList(Sup(i),Sup(j)) / ...
                                    SupPixelNeighborList(Sup(i),Sup(j)) ];
        Target(1) = Sup2Para(Sup(i));
        Target(2) = Sup2Para(Sup(j));
        rayBoundary(:,1) =  RayOri(:,i);
        rayBoundary(:,2) =  RayOri(:,i);
        if MultiScaleFlag
                vector = (MultiScaleSupTable(Sup2Para(Sup(i)),2:end) == MultiScaleSupTable(Sup2Para(Sup(j)),2:end));
                expV = exp(-10*(WeiV*vector' + ShiftStick) );
                betaTemp = StickVert*(0.5+1/(1+expV));
                % therr should always be sticking (know don't care about occlusion)
        else
                betaTemp = StickVert;
        end
        temp = sparse(3,NuSupSize);
        temp(:,Target(1)) = rayBoundary(:,1);
        VertStickM_i = [VertStickM_i; betaTemp*temp(:)'];
        temp = sparse(3,NuSupSize);
        temp(:,Target(2)) = rayBoundary(:,2);
        VertStickM_j = [VertStickM_j; betaTemp*temp(:)'];
        EstDepVertStick = [EstDepVertStick; sqrt(max(CleanedDepthMap(i),ClosestDist)*max(CleanedDepthMap(j),ClosestDist))];
        VertStickPointInd = [VertStickPointInd i ];
end
% ====finished finding the unucessary stiching points in Vertical direction ===

% ======================================Finish building up matrix=====================hard work======================


% ================================================================================================================
% Start Decompose the image align with superpixels ==================================================================================
% define the decomposition in both X and Y direction
 
TotalRectX = 2*XNuDecompose-1;
TotalRectY= 2*YNuDecompose-1;
PlanePara = NaN*ones(3*NuSupSize,1); % setup the lookup table for the solved plane parameter

for k = 0:(TotalRectX-1)
        l = rem(k*2,(TotalRectX));
        RangeX = (1+ceil(Default.HoriXNuDepth/XNuDecompose)*l/2):...
                 min((1+ceil(Default.HoriXNuDepth/XNuDecompose)*(l/2+1)),Default.HoriXNuDepth);
        RangeX = ceil(RangeX);
        for q = 0:(TotalRectY-1)
                l = rem(q*2,(TotalRectY));
                RangeY = (1+ceil(Default.VertYNuDepth/YNuDecompose)*l/2):...
                         min((1+ceil(Default.VertYNuDepth/YNuDecompose)*(l/2+1)),Default.VertYNuDepth);
                RangeY = ceil(RangeY);
                mask = zeros(size(Sup));
                mask(RangeY,RangeX) = 1;
                mask =logical(mask);
                SubSup = sort(setdiff(unique( reshape( Sup(RangeY,RangeX),1,[])),0));
                BoundarySup = [];
    
                if any(SubSup <=0)
                        SubSup(SubSup<=0)
                end
                BoundarySup = find(sum(SupNeighborTable(SubSup,:), 1) ~=0);
                BoundarySup = unique(setdiff(BoundarySup,[0 SubSup] ));
    
                % chech if BoundarySup non-NaN in PlanePara
                checkNoNNaN = ~isnan(PlanePara(Sup2Para(BoundarySup)*3));
                BoundarySup = BoundarySup(checkNoNNaN);
                TotalSup = sort([SubSup BoundarySup]);

                % define the pointer to extract the data for this specific decomposition
                SubSupPtr = [ Sup2Para(SubSup)*3-2;...
                              Sup2Para(SubSup)*3-1;...
                              Sup2Para(SubSup)*3];
                SubSupPtr = SubSupPtr(:); 
                BoundarySupPtr = [ Sup2Para(BoundarySup)*3-2;...
                                   Sup2Para(BoundarySup)*3-1;...
                                   Sup2Para(BoundarySup)*3];
                BoundarySupPtr = BoundarySupPtr(:);
                NuSubSupSize = size(SubSup,2);
    
                % simply extract  NewRayAllM NewPosiM NewCoPM =========================================
                % NewHoriStickM NewVertStickM for the specific decomposition 
                NewRayAllM = RayAllM(:,SubSupPtr);
                tar = sum(NewRayAllM ~= 0,2) == 3;
                NewRayAllM = NewRayAllM(tar,:);
                NewPosiM = PosiM(:,SubSupPtr);
                tar = sum(NewPosiM ~= 0,2) == 3;
                NewPosiM = NewPosiM(tar,:);
                NewVarM = VarM(tar);
    
                NewCoPM = CoPM1(:,SubSupPtr) - CoPM2(:,SubSupPtr);
                NewCoPMBound = CoPM1(:,BoundarySupPtr) - CoPM2(:,BoundarySupPtr);
    
                tar = sum(NewCoPM ~= 0,2) + sum(NewCoPMBound ~= 0,2)==6;
                NewCoPMBound = NewCoPMBound*PlanePara(BoundarySupPtr);
                NewCoPM = NewCoPM(tar,:);
                NewCoPMBound = NewCoPMBound(tar);
                NewCoPEstDepth = CoPEstDepth(tar);   

                NewHoriStickM = HoriStickM_i(:,SubSupPtr)-HoriStickM_j(:,SubSupPtr);
                NewHoriStickMBound = HoriStickM_i(:,BoundarySupPtr)-HoriStickM_j(:,BoundarySupPtr);
    
                tar = sum(NewHoriStickM ~= 0,2)+  sum(NewHoriStickMBound ~= 0,2) ==6;
                NewHoriStickM = NewHoriStickM(tar,:);    
                NewEstDepHoriStick = EstDepHoriStick(tar);
                NewHoriStickMBound = NewHoriStickMBound*PlanePara(BoundarySupPtr);
                NewHoriStickMBound = NewHoriStickMBound(tar);
                NewWeightHoriNeighborStitch = WeightHoriNeighborStitch(tar);

                NewVertStickM = VertStickM_i(:,SubSupPtr)-VertStickM_j(:,SubSupPtr);
                NewVertStickMBound = VertStickM_i(:,BoundarySupPtr)-VertStickM_j(:,BoundarySupPtr);
    
                tar = sum(NewVertStickM ~= 0,2) + sum(NewVertStickMBound ~= 0,2)==6;
                NewVertStickM = NewVertStickM(tar,:);
                NewEstDepVertStick = EstDepVertStick(tar);
                NewVertStickMBound = NewVertStickMBound*PlanePara(BoundarySupPtr);
                NewVertStickMBound = NewVertStickMBound(tar);
                NewWeightVertNeighborStitch = WeightVertNeighborStitch(tar);
   
                WeightsSelfTerm = 1 ./ exp(abs(NewVarM)/BandWith);
                % ==========================end of extraction ====================

                fprintf(['     ' num2str( toc(inferenceTime) ) '\n     : In 1st level Optimization, using new solver.' ...
                        '(' num2str(k+1) '/' num2str((TotalRectX-1)+1) ',' num2str(l+1) '/' num2str((TotalRectY-1)+1) ')']);

                global A b S inq

                A = [ sparse(1:length(WeightsSelfTerm),1:length(WeightsSelfTerm),WeightsSelfTerm ) * NewPosiM;...
                        sparse(1:length(NewCoPEstDepth),1:length(NewCoPEstDepth),NewCoPEstDepth*Center ) * NewCoPM;...
                        sparse(1:length(NewEstDepHoriStick), 1:length(NewEstDepHoriStick), NewEstDepHoriStick.*NewWeightHoriNeighborStitch) * NewHoriStickM;...
                        sparse(1:length(NewEstDepVertStick), 1:length(NewEstDepVertStick), NewEstDepVertStick.*NewWeightVertNeighborStitch) * NewVertStickM;...
                        ];

                b = [ ones(size(NewPosiM,1),1) .* WeightsSelfTerm;...
                        -NewCoPMBound.*NewCoPEstDepth*Center;...
                        -NewHoriStickMBound.*NewEstDepHoriStick.*NewWeightHoriNeighborStitch;...
                        -NewVertStickMBound.*NewEstDepVertStick.*NewWeightVertNeighborStitch];
 
                temp = zeros(1, NuSubSupSize*3);
                temp(3*(1:NuSubSupSize)-1) = YPointer(Sup2Para(SubSup));
                temp = sparse(1:length(temp), 1:length(temp), temp);
                temp( sum(temp,2) ==0,:) = [];
                S = [temp;...
                        NewRayAllM;...
                        -NewRayAllM];
                inq = [ sparse(size(temp,1), 1);...
                        - 1/ClosestDist*ones(size(NewRayAllM,1),1);...
                        1/FarestDist*ones(size(NewRayAllM,1),1)];
                Para.ClosestDist = ClosestDist;
                Para.FarestDist = FarestDist;
                Para.ptry = [];
                Para.ptrz = [];
                Para.Dist_Start = [];

                % solve the OPT problem using the new solver
                [x_ashIterator, alfa, status, history, T_nt_hist] = ...
                        SigmoidLogBarrierSolver( Para, [], [], [], '', [], [], solverVerboseLevel);
                
                % check if the constrian still satisfied
                if any(S*x_ashIterator+inq > 0 )
                        disp('Inequality not satisfied');
                        max( S*x_ashIterator+inq)
                elseif status == 2
                        fprintf([' Success with alfa=' num2str(alfa)]); 
                end

                % assign the solution of specific decomposition to the corresponding entries of the whole problems
                PlanePara(SubSupPtr) = x_ashIterator;
        end
end

% build the whole image
PlanePara = reshape(PlanePara,3,[]);
% porject the ray on planes to generate the ProjDepth
FitDepthPPCP = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
FitDepthPPCP(~maskSkyEroded) = (1./sum(PlanePara(:,Sup2Para(SupEpand(~maskSkyEroded ))).*Ray(:,~maskSkyEroded ),1))';
FitDepthPPCP = reshape(FitDepthPPCP,Default.VertYNuDepth,[]);
[Position3DFitedPPCP] = im_cr2w_cr(FitDepthPPCP,permute(Ray,[2 3 1]));

if NoSecondStep % if no second step
        Position3DFitedPPCP(3,:) = -Position3DFitedPPCP(3,:);
        Position3DFitedPPCP = permute(Position3DFitedPPCP,[2 3 1]);
        RR =permute(Ray,[2 3 1]);
        temp = RR(:,:,1:2)./repmat(RR(:,:,3),[1 1 2]);
        PositionTex = permute(temp./repmat(cat(3,Default.a_default,Default.b_default),[Default.VertYNuDepth Default.HoriXNuDepth 1])+repmat(cat(3,Default.Ox_default,Default.Oy_default),[Default.VertYNuDepth Default.HoriXNuDepth 1]),[3 1 2]);
        PositionTex = permute(PositionTex,[2 3 1])
        disp('First level Wrl');

        % write wrl file
        WrlFacestHroiReduce(Position3DFitedPPCP,PositionTex,SupOri, [ Default.filename{1} '1st'],[ Default.filename{1} '1st'], ...
                            Default.OutPutFolder, GridFlag, 0);
        system(['gzip -9 -c ' Default.OutPutFolder Default.filename{1} '1st.wrl > ' ...
               Default.OutPutFolder Default.filename{1} '1st.wrl.gz']);
        copyfile([ Default.OutPutFolder Default.filename{1} '1st.wrl.gz '], ...
               [ Default.OutPutFolder Default.filename{1} '1st.wrl'],'f');
        delete([Default.OutPutFolder Default.filename{1} '1st.wrl.gz']);
end
%==================Finished for one step MRF==================================================================================================

if NoSecondStep
        return;
end

% ====================================following are 2nd step MRF to give more visually pleasing result=======================================
% generating new PosiMPPCP using the new position
normPara = norms(PlanePara);
normalizedPara = PlanePara ./ repmat( normPara, [3 1]);
groundPara = abs(normalizedPara(2,:)) >= groundThreshold(YPosition);
groundParaInd = find(groundPara);
verticalPara = abs(normalizedPara(2,:)) <= verticalThreshold(YPosition); % change to have different range of vertical thre ================
verticalParaInd = find(verticalPara);

indexVertical = find( verticalPara)*3-1;
indexGroundX = find( groundPara)*3-2;
indexGroundZ = find( groundPara)*3;

PosiMPPCP = sparse(0,0);
VarM2 = sparse(0,0);
   
% forming new supporting matrix using new depth and get rid of the support of the vertical plane
for i = NuSup
        mask = Sup == i;
        if any(verticalParaInd == Sup2Para(i)) & FlagRemoveVerticalSupport % Remove Vertical Depths Supports
                mask = logical(zeros(size(Sup)));
        end
        PosiMPPCP = blkdiag(PosiMPPCP, Position3DFitedPPCP(:,mask)');
        VarM2 = [VarM2; VarMap(mask)];
        if length( find(isnan(PosiMPPCP)) )
                disp('PosiMPPCP is NaN');
        end
end

% Start Decompose image =========================
TotalRectX = 2*XNuDecompose-1;
TotalRectY = 2*YNuDecompose-1;
PlanePara = NaN*ones(3*NuSupSize,1); % setup the lookuptable for the solved plane parameter

for k = 0:(TotalRectX-1)
        l = rem(k*2,(TotalRectX));
        RangeX = (1+ceil(Default.HoriXNuDepth/XNuDecompose)*l/2):...
                min((1+ceil(Default.HoriXNuDepth/XNuDecompose)*(l/2+1)),Default.HoriXNuDepth);
        RangeX = ceil(RangeX);
        for q = 0:(TotalRectY-1)
                l = rem(q*2,(TotalRectY));
                RangeY = (1+ceil(Default.VertYNuDepth/YNuDecompose)*l/2):...
                        min((1+ceil(Default.VertYNuDepth/YNuDecompose)*(l/2+1)),Default.VertYNuDepth);
                RangeY = ceil(RangeY);
                mask = zeros(size(Sup));
                mask(RangeY,RangeX) = 1;
                mask =logical(mask);
                SubSup = sort(setdiff( unique( reshape( Sup(RangeY,RangeX),1,[])),0));
                BoundarySup = [];
                BoundarySup = find(sum(SupNeighborTable(SubSup,:), 1) ~=0);
                BoundarySup = unique(setdiff(BoundarySup,[0 SubSup] ));

                % chech if BoundarySup non-NaN in PlanePara
                checkNoNNaN = ~isnan(PlanePara(Sup2Para(BoundarySup)*3));
                BoundarySup = BoundarySup(checkNoNNaN);
                TotalSup = sort([SubSup BoundarySup]);

                SubSupPtr = [ Sup2Para(SubSup)*3-2;...
                                Sup2Para(SubSup)*3-1;...
                                Sup2Para(SubSup)*3];
                SubSupPtr = SubSupPtr(:);
                BoundarySupPtr = [ Sup2Para(BoundarySup)*3-2;...
                                        Sup2Para(BoundarySup)*3-1;...
                                        Sup2Para(BoundarySup)*3];
                BoundarySupPtr =BoundarySupPtr(:);
                NuSubSupSize = size(SubSup,2);
                SubSup2Para = sparse(1,max(SubSup));
                SubSup2Para(SubSup) = 1:NuSubSupSize;
    
                % clearn RayAllM PosiM CoPM1 HoriStickM_i VertStickM_i
                NewRayAllM = RayAllM(:,SubSupPtr);
                tar = sum(NewRayAllM ~= 0,2) ==3;
                NewRayAllM = NewRayAllM(tar,:);

                NewPosiMPPCP = PosiMPPCP(:,SubSupPtr);
                tar = sum(NewPosiMPPCP ~= 0,2) ==3;
                NewPosiMPPCP = NewPosiMPPCP(tar,:);
                NewVarM = VarM2(tar);

                NewCoPM = CoPM1(:,SubSupPtr) - CoPM2(:,SubSupPtr);
                NewCoPMBound = CoPM1(:,BoundarySupPtr) - CoPM2(:,BoundarySupPtr);
                tar = sum( NewCoPM ~= 0,2) + sum( NewCoPMBound ~= 0,2) ==6;
                NewCoPM = NewCoPM(tar,:);
                NewCoPMBound = NewCoPMBound*PlanePara(BoundarySupPtr); % column vertor
                NewCoPMBound = NewCoPMBound(tar);
                NewCoPEstDepth = CoPEstDepth(tar);   

                NewHoriStickM = HoriStickM_i(:,SubSupPtr)-HoriStickM_j(:,SubSupPtr);
                NewHoriStickMBound = HoriStickM_i(:,BoundarySupPtr)-HoriStickM_j(:,BoundarySupPtr);
                tar = sum(NewHoriStickM ~= 0,2) + sum( NewHoriStickMBound ~= 0,2)==6;
                NewHoriStickM = NewHoriStickM(tar,:);
                NewHoriStickMBound = NewHoriStickMBound*PlanePara(BoundarySupPtr); % column vertor
                NewHoriStickMBound = NewHoriStickMBound(tar);
                NewEstDepHoriStick = EstDepHoriStick(tar);
                NewWeightHoriNeighborStitch = WeightHoriNeighborStitch(tar);

                NewVertStickM = VertStickM_i(:,SubSupPtr)-VertStickM_j(:,SubSupPtr);
                NewVertStickMBound = VertStickM_i(:, BoundarySupPtr)-VertStickM_j(:,BoundarySupPtr);
                tar = sum(NewVertStickM ~= 0,2) + sum(NewVertStickMBound ~= 0,2)==6;
                NewVertStickM = NewVertStickM(tar,:);
                NewVertStickMBound = NewVertStickMBound*PlanePara(BoundarySupPtr); % column vertor
                NewVertStickMBound = NewVertStickMBound(tar);
                NewEstDepVertStick = EstDepVertStick(tar);
                NewWeightVertNeighborStitch = WeightVertNeighborStitch(tar);

                %    try reduce the vertical constrain
                NonVertPtr = setdiff( 1:(3*NuSubSupSize), SubSup2Para( NuSup( (intersect( indexVertical,SubSupPtr ) +1)/3))*3-1 );
                YNoComingBack = YPointer(Sup2Para(SubSup));
                YNoComingBack(SubSup2Para( NuSup( (intersect( indexVertical,SubSupPtr ) +1)/3))) = [];
                YCompMask = zeros(1,3*NuSubSupSize);
                YCompMask(3*(1:NuSubSupSize)-1) = 1;
                YCompMask = YCompMask(NonVertPtr); 
                XCompMask = zeros(1,3*NuSubSupSize);
                XCompMask( SubSup2Para( NuSup( (intersect( indexGroundX,SubSupPtr ) +2)/3))*3-2 ) = 1;
                XCompMask = XCompMask(NonVertPtr);
                ZCompMask = zeros(1,3*NuSubSupSize);
                ZCompMask( SubSup2Para( NuSup( (intersect( indexGroundZ,SubSupPtr ) )/3))*3 ) = 1;
                ZCompMask = ZCompMask(NonVertPtr);
                GroundMask = intersect( find(groundPara), ( SubSupPtr( 3:3:( size(SubSupPtr,1)) )/3 ) ) ;

                % version written by Ashutosh

                tempGroundX = sparse(1:length(XCompMask), 1:length(XCompMask), XCompMask);
                tempGroundX( logical(XCompMask) ) = tempGroundX( logical(XCompMask) )./normPara( GroundMask );
                tempGroundX( sum(tempGroundX,2) == 0,:) = [];
                tempGroundZ = sparse(1:length(ZCompMask), 1:length(ZCompMask), ZCompMask );
                tempGroundZ( logical(ZCompMask) ) = tempGroundZ( logical(ZCompMask) )./normPara( GroundMask );
                tempGroundZ( sum(tempGroundZ,2) == 0,:)= [];

                A = [   sparse(1:length(NewVarM),1:length(NewVarM),1./exp(abs(NewVarM)/BandWith)) * NewPosiMPPCP(:, NonVertPtr);...
                        sparse(1:length(NewCoPEstDepth), 1:length(NewCoPEstDepth), NewCoPEstDepth * Center) * NewCoPM(:, NonVertPtr);...
                        sparse(1:length(NewEstDepHoriStick), 1:length(NewEstDepHoriStick), NewEstDepHoriStick.*NewWeightHoriNeighborStitch) * NewHoriStickM(:, NonVertPtr);...
                        sparse(1:length(NewEstDepVertStick), 1:length(NewEstDepVertStick), NewEstDepVertStick.*NewWeightVertNeighborStitch) * NewVertStickM(:, NonVertPtr);...
                        tempGroundX;...
                        tempGroundZ...
                        ];
                %        +10*norm( ( Para( logical(XCompMask) ))./...
                %                            normPara( GroundMask )', 1)...
                %                 +10*norm( ( Para( logical(ZCompMask)))./...
                %                            normPara( GroundMask )', 1) ...
 
                b = [   1./exp(abs(NewVarM)/BandWith); ...
                        -Center*NewCoPMBound.*NewCoPEstDepth; ...
                        -NewHoriStickMBound.*NewEstDepHoriStick.*NewWeightHoriNeighborStitch;...
                        -NewVertStickMBound.*NewEstDepVertStick.*NewWeightVertNeighborStitch;...
                        sparse(size(tempGroundX,1),1);...
                        sparse(size(tempGroundZ,1),1)...
                        ];

                temp = YCompMask;
                temp(logical(YCompMask)) = YNoComingBack;
                temp = sparse(1:length(temp), 1:length(temp), temp);
                temp( sum(temp,2) ==0,:) = [];
                S = [   temp;...
                        NewRayAllM(:,NonVertPtr);...
                        -NewRayAllM(:,NonVertPtr);...
                        ];
                inq = [ sparse(size(temp,1), 1);...
                        - 1/ClosestDist*ones(size(NewRayAllM,1),1);...
                        1/FarestDist*ones(size(NewRayAllM,1),1);...
                        ];

                Para.ClosestDist = ClosestDist;
                Para.FarestDist = FarestDist;

                % build up ptry and ptrz adapt fot NonVertPtr
                Para.ptry = zeros(size(NewRayAllM,2),1);
                Para.ptry(2:3:size(NewRayAllM,2)) = 1;
                Para.ptry = logical(Para.ptry(NonVertPtr));
                Para.ptrz = zeros(size(NewRayAllM,2),1);
                Para.ptrz(3:3:size(NewRayAllM,2)) = 1;
                Para.ptrz = logical(Para.ptrz(NonVertPtr));
                Para.Dist_Start = size(temp,1)+1;
                [x_ashIterator, alfa, status] = SigmoidLogBarrierSolver(Para, [], [], [], '', [], [], solverVerboseLevel);
                if any(S*x_ashIterator+inq > 0 )
                        disp('Inequality not satisfied');
                        max( S*x_ashIterator+inq)
                elseif status==2
                        fprintf([' Success with alfa=' num2str(alfa)]); 
                end

                Para = x_ashIterator;

                tempPara = zeros(3*NuSubSupSize,1);
                tempPara(NonVertPtr) = Para;
                PlanePara(SubSupPtr) = tempPara;
        end
end

% porject the ray on planes to generate the FitDepth
PlanePara = reshape(PlanePara,3,[]);
FitDepth = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
FitDepth(~maskSkyEroded) = (1./sum(PlanePara(:,Sup2Para(SupEpand(~maskSkyEroded))).*Ray(:,~maskSkyEroded),1))';
FitDepth = reshape(FitDepth,Default.VertYNuDepth,[]); 
% ==========Storage ==============
if Default.Flag.AfterInferenceStorage
        save([ Default.ScratchFolder '/' strrep( Default.filename{1},'.jpg','') '_AInfnew.mat' ], 'FitDepth', 'depthMap', ...
            'Sup', 'SupOri', 'RayOri','Ray','SupNeighborTable','maskSky','maskG','MultiScaleSupTable','WeiM','VarMapRaw');
end
% ===============================
[Position3DFited] = im_cr2w_cr(FitDepth,permute(Ray,[2 3 1]));
Position3DFited(3,:) = -Position3DFited(3,:);
Position3DFited = permute(Position3DFited,[2 3 1]);
RR =permute(Ray,[2 3 1]);
temp = RR(:,:,1:2)./repmat(RR(:,:,3),[1 1 2]);
PositionTex = permute(temp./repmat(cat(3,Default.a_default,Default.b_default),...
                [Default.VertYNuDepth Default.HoriXNuDepth 1])...
                +repmat(cat(3,Default.Ox_default,Default.Oy_default),...
                [Default.VertYNuDepth Default.HoriXNuDepth 1]),[3 1 2]);
PositionTex = permute(PositionTex,[2 3 1]);

% write wrl file
fprintf(['     ' num2str( toc(inferenceTime) ) '\n     : Writing WRL.']);
WrlFacestHroiReduce(Position3DFited, PositionTex, SupOri, Default.filename{1}, Default.filename{1}, ...
Default.OutPutFolder, GridFlag, 0);

system(['gzip -9 -c ' Default.OutPutFolder Default.filename{1} '.wrl > ' ...
        Default.OutPutFolder Default.filename{1} '.wrl.gz']);
copyfile([Default.OutPutFolder Default.filename{1} '.wrl.gz'], ...
        [Default.OutPutFolder Default.filename{1} '.wrl'],'f');
delete([Default.OutPutFolder Default.filename{1} '.wrl.gz']);

return;