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

Last change on this file since 37 was 37, checked in by (none), 14 years ago

Added original make3d
File size: 54.9 KB
Line 
1% *  This code was used in the following articles:
2% *  [1] Learning 3-D Scene Structure from a Single Still Image,
3% *      Ashutosh Saxena, Min Sun, Andrew Y. Ng,
4% *      In ICCV workshop on 3D Representation for Recognition (3dRR-07), 2007.
5% *      (best paper)
6% *  [2] 3-D Reconstruction from Sparse Views using Monocular Vision,
7% *      Ashutosh Saxena, Min Sun, Andrew Y. Ng,
8% *      In ICCV workshop on Virtual Representations and Modeling
9% *      of Large-scale environments (VRML), 2007.
10% *  [3] 3-D Depth Reconstruction from a Single Still Image,
11% *      Ashutosh Saxena, Sung H. Chung, Andrew Y. Ng.
12% *      International Journal of Computer Vision (IJCV), Aug 2007.
13% *  [6] Learning Depth from Single Monocular Images,
14% *      Ashutosh Saxena, Sung H. Chung, Andrew Y. Ng.
15% *      In Neural Information Processing Systems (NIPS) 18, 2005.
16% *
17% *  These articles are available at:
18% *  http://make3d.stanford.edu/publications
19% *
20% *  We request that you cite the papers [1], [3] and [6] in any of
21% *  your reports that uses this code.
22% *  Further, if you use the code in image3dstiching/ (multiple image version),
23% *  then please cite [2].
24% * 
25% *  If you use the code in third_party/, then PLEASE CITE and follow the
26% *  LICENSE OF THE CORRESPONDING THIRD PARTY CODE.
27% *
28% *  Finally, this code is for non-commercial use only.  For further
29% *  information and to obtain a copy of the license, see
30% *
31% *  http://make3d.stanford.edu/publications/code
32% *
33% *  Also, the software distributed under the License is distributed on an
34% * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
35% *  express or implied.   See the License for the specific language governing
36% *  permissions and limitations under the License.
37% *
38% */
39function  ReportPlaneParaMRF_Conditioned(Default, step, DepthFolder,...
40          Sup, SupOri, MedSup,depthMap,VarMapRaw,RayOri, Ray, SupNeighborTable,...
41          MedRay,maskSky,maskG,Algo,k, CornerList, OccluList,...
42          MultiScaleSupTable, StraightLineTable, HBrokeBook, VBrokeBook,previoslyStored,...
43          baseline);
44% This function runs the RMF over the plane parameter of each superpixels
45
46% step = nothing
47% Sup = Superpixel, in 2-d matrix.
48% SupOri = original superpixel before cleaning
49% MedSup = median size superpixel
50% RayOri = Rays without stiching to the boundary.
51% Ray = rays after stiching
52% SupNeigborTabl = 2d sparse matrix
53% Algo = not used
54% k = not used
55% CornerList = not used
56% OccluList = not used
57% MultiScaleSupTable = multiple scale segmentation, used to define the weights betwenn the stiching terms.
58% StraightLineTable = straight line stiching, not used anymore.
59% HBrokeBook, VBrokeBook = occlusion, not used
60% baseline = using only tyhe bias term
61
62if nargin <20
63   baseline = 0;
64end
65solverVerboseLevel = 0;
66FlagRemoveVerticalSupport = 1;
67ExtractRelationInfo = 1;
68VarMap = zeros( size(VarMapRaw));
69% ============== parameters for the decomposition problem
70% Optimal parameters for current code:
71% For one machine: Sedumi: (1,1)        Lpsolve:(3,1)
72% Multiple machines: Sedumi: (4,1)      LPsolve:(3,1)
73% lpsolve running time is 22 seconds for (4,2) arrangement; but numerical
74% accuracy needs to be resolved first.
75XNuDecompose = 1; %3 is stable     
76YNuDecompose = 1;
77% ============ parameters for the decomposition problem
78
79inferenceTime = tic;
80
81fprintf(['\n     : Building Matrices....       ']);
82
83% initialize parameters
84GridFlag = 0;
85NOYALMIP = 0;   %1;
86NEW_OPTIMIZATION = 1;
87
88NoSecondStep = 0;
89displayFlag = false;
90RenderVrmlFlag = true;
91PlaneParaSegmentationFlag = false;
92Catsh1stOptFlag = false; % 3.8 added new storage flag
93
94scale = 1;
95StickHori = 5;  %0.1; % sticking power in horizontal direction
96StickVert = 10;     % sticking power in vertical direction
97Center = 2; % Co-Planar weight at the Center of each superpixel
98HoriConf = 1; % set the confidant of the learned depth at the middle in Horizontal direction of the image
99VertConf = 0.01; % set the confidant of the learned depth at the top of the image
100BandWith = 1; % Nov29 1 Nov30 0.1 check result change to 0.1 12/1 0.1 lost detail
101mapVert = linspace(VertConf,1,Default.VertYNuDepth); % modeling the gravity prior
102mapHori = [linspace(HoriConf,1,round(Default.HoriXNuDepth/2)) fliplr(linspace(HoriConf,1,Default.HoriXNuDepth-round(Default.HoriXNuDepth/2)))];
103
104% ========set the range of depth that our model in
105ClosestDist = 1;
106% set the FarestDist to very 5 times to median depth
107FarestDist = 1.5*median(depthMap(:)); % tried on university % nogood effect but keep it since usually it the rangelike this   % change to 1.5 for church       
108% ERROR --- use more intelligent FarestDistance
109
110% ================================================
111
112ceiling = 0*Default.VertYNuDepth; % set the position of the ceiling, related to No plane coming back constrain % changed for newchurch
113
114% The hand-tuned 14 dimensional vector.
115if ~isempty(MultiScaleSupTable)
116   MultiScaleFlag = true;       % multiple segmentaition hypothesis
117   WeiV = 2*ones(1,size(MultiScaleSupTable,2)-1);
118else
119   MultiScaleFlag = false;
120   WeiV = 1;
121end
122WeiV(1,1:2:end) = 6; % emphasize the middle scale three times smaller than large scale
123WeiV = WeiV./sum(WeiV);% normalize if pair of superpixels have same index in all the scale, their weight will be 10
124ShiftStick = -.1;  % between -1 and 0, more means more smoothing.
125ShiftCoP = -.5;  % between -1 and 0, more means more smoothing.
126gravity =true; % if true, apply the HoriConf and VertConf linear scale weight
127CoPST = true; % if true, apply the Straight line prior as the Co-Planar constrain
128
129%ConerImprove = false;
130%FractionalDepthError = true;
131
132
133% get rid of the error point and sky point in the depthMap
134% set every depth bigger than FarestDistmeter to FarestDistmeters
135%CleanedDepthMap = (depthMapif ~previoslyStored >80).*medfilt2(depthMap,[4 4])+(depthMap<=80).*depthMap;
136CleanedDepthMap = depthMap;
137%CleanedDepthMap(depthMap>FarestDist) = FarestDist; % don't clean the point >80 sometimes it occlusion
138%disp('Nu of depthMap>FarestDist');
139%sum(sum(depthMap>FarestDist))
140CleanedDepthMap(depthMap>FarestDist) = NaN; % don't clean the point >80 sometimes it occlusion
141
142Posi3D = im_cr2w_cr(CleanedDepthMap,permute(RayOri,[2 3 1]));
143% given the depth and ray as input, calculate the 3-d coordinate at each point.
144
145if ~previoslyStored
146
147   %NewMap = [rand(max(Sup(:)),3); [0 0 0]];
148   
149   % Clean the Sup near sky
150   maskSky = Sup == 0;
151   maskSkyEroded = imerode(maskSky, strel('disk', 4) );
152   SupEpand = ExpandSup2Sky(Sup,maskSkyEroded);
153   NuPatch = Default.HoriXNuDepth*Default.VertYNuDepth-sum(maskSky(:));
154
155   NuSup = setdiff(unique(Sup)',0);
156   NuSup = sort(NuSup);
157   NuSupSize = size(NuSup,2);
158
159   % Sup index and planeParameter index inverse map
160   Sup2Para = sparse(1,max(Sup(:)));
161   Sup2Para(NuSup) = 1:NuSupSize;
162
163   % constructinf the Straight line prior matrix Will be add in the CoPlane matrix
164   NuLine = size(StraightLineTable,2);
165   CoPSTList = [];
166
167        %effectively not running the straight line constraint here.
168   for i = 1:NuLine
169       L = StraightLineTable{i};
170       X = L(1:(end-1))';
171       Y = L(2:end)';
172       if isempty(X)
173          continue;
174       end
175       for j = 1:size(X,1)
176           if X(j)~=Y(j)
177              CoPSTList = [CoPSTList; X(j) Y(j)];
178           end
179       end
180   end
181end
182
183% Generate the Matrix for MRF
184 % ===========================================================================================================================================
185% If you go up in the vertical direction in the image, the weights change in the vertical direction.
186groundThreshold = cos([ zeros(1, Default.VertYNuDepth - ceil(Default.VertYNuDepth/2)+10) ...
187                                linspace(0,15,ceil(Default.VertYNuDepth/2)-10)]*pi/180);
188    %  v1 15 v2 20 too big v3 20 to ensure non misclassified as ground.
189    %  verticalThreshold = cos(linspace(5,55,Default.VertYNuDepth)*pi/180); % give a vector of size 55 in top to down :
190verticalThreshold = cos([ 5*ones(1,Default.VertYNuDepth - ceil(Default.VertYNuDepth/2)) ...
191                                linspace(5,55,ceil(Default.VertYNuDepth/2))]*pi/180);
192        % give a vector of size 55 in top to down :
193        % 50 means suface norm away from y axis more than 50 degree
194 % ===========================================================================================================================================
195
196PosiM = sparse(0,0);
197VarM = sparse(0,0);
198RayMd = sparse(0,0);    % Not used
199RayAllOriM = sparse(0,0);
200RayAllM = sparse(0,0);
201RayMtilt = sparse(0,0); % Not used
202RayMCent = sparse(0,0); % Not used
203DepthInverseMCent = []; % Not used
204DepthInverseM = [];     % Not used
205YPointer = [];
206YPosition = [];
207beta = [];
208EmptyIndex = [];
209
210for i = NuSup
211%    mask = Sup ==i;
212    mask = SupEpand ==i; % include the Ray that will be use to expand the NonSky
213    RayAllOriM = blkdiag( RayAllOriM, RayOri(:,mask)');
214    RayAllM = blkdiag( RayAllM, Ray(:,mask)');
215    mask = Sup ==i; % Not include the Ray that will be use to expand the NonSky   
216    [yt x] = find(mask);
217    CenterX = round(median(x));
218    CenterY = round(median(yt));
219    YPointer = [YPointer; CenterY >= ceiling];
220    YPosition = [YPosition; CenterY];
221    mask(isnan(CleanedDepthMap)) = false;
222    SupNuPatch(i) = sum(mask(:));
223   
224        % find center point
225    [yt x] = find(mask);
226    CenterX = round(median(x));
227    CenterY = round(median(yt));
228 
229  if ~all(mask(:)==0)
230    if gravity
231      if any(CleanedDepthMap(mask) <=0)
232         CleanedDepthMap(mask)
233%         pause
234      end
235      Pa2CenterRatio = median(CleanedDepthMap(mask))./CleanedDepthMap(mask);
236      if sum(mask(:)) > 0
237         RayMtilt = blkdiag(RayMtilt, ...
238                                ( Pa2CenterRatio(:,[1 1 1]).*repmat(RayOri(:,CenterY,CenterX)', ...
239                                                [ SupNuPatch(i) 1])- RayOri(:,mask)'));
240      else
241         RayMtilt = blkdiag(RayMtilt, RayOri(:,mask)');
242      end
243      RayMCent = blkdiag(RayMCent, RayOri(:,CenterY,CenterX)'*SupNuPatch(i)*mapVert(CenterY)*mapHori(CenterX));
244      PosiM = blkdiag(PosiM,Posi3D(:,mask)');%.*repmat( mapVert(yt)',[1 3]).*repmat( mapHori(x)',[1 3]));
245      VarM = [VarM; VarMap(mask).*(mapVert(yt)').*( mapHori(x)')];
246      RayMd = blkdiag(RayMd,RayOri(:,mask)'.*repmat( mapVert(yt)',[1 3]).*repmat( mapHori(x)',[1 3]));
247      DepthInverseMCent = [DepthInverseMCent; 1./median(CleanedDepthMap(mask)).*SupNuPatch(i)* mapVert(CenterY)'*mapHori(CenterX)];
248      DepthInverseM = [DepthInverseM; 1./CleanedDepthMap(mask).* mapVert(yt)'.*mapHori(x)'];
249    else
250      Pa2CenterRatio = CleanedDepthMap(CenterY,CenterX)./CleanedDepthMap(mask);
251      if sum(mask(:)) > 0
252         RayMtilt = blkdiag(RayMtilt, ...
253             ( Pa2CenterRatio(:,[1 1 1]).*repmat(RayOri(:,CenterY,CenterX)',[ SupNuPatch(i) 1])- RayOri(:,mask)'));
254      else
255         RayMtilt = blkdiag(RayMtilt, RayOri(:,mask)');
256      end
257      RayMCent = blkdiag(RayMCent, RayOri(:,CenterY,CenterX)'*SupNuPatch(i));
258      PosiM = blkdiag(PosiM,Posi3D(:,mask)');
259      VarM = [VarM; VarMap(mask)];
260      RayMd = blkdiag(RayMd,RayOri(:,mask)');
261      DepthInverseMCent = [DepthInverseMCent; 1./median(CleanedDepthMap(mask)).*SupNuPatch(i)];
262      DepthInverseM = [DepthInverseM; 1./CleanedDepthMap(mask)];
263    end
264  else
265     RayMtilt = blkdiag(RayMtilt, RayOri(:,mask)');
266     RayMCent = blkdiag(RayMCent, RayOri(:,mask)');
267     PosiM = blkdiag(PosiM, Posi3D(:,mask)');
268     VarM = [VarM; VarMap(mask)];
269     RayMd = blkdiag(RayMd, RayOri(:,mask)');
270  end
271end
272YPointer(YPointer==0) = -1;
273
274% buliding CoPlane Matrix=========================================================================
275
276NuSTList = 0;
277if CoPST
278   NuSTList = size(CoPSTList,1);
279   if ~isempty(CoPSTList)
280      [V H] = size(SupNeighborTable);
281      SupNeighborTable( CoPSTList(:,1)*V + CoPSTList(:,2)) = 1;
282      SupNeighborTable( CoPSTList(:,2)*V + CoPSTList(:,1)) = 1;
283%   ClosestNList = [ClosestNList; CoPSTList];
284   end
285end
286CoPM1 = sparse(0,3*NuSupSize);
287CoPM2 = sparse(0,3*NuSupSize);
288CoPEstDepth = sparse(0,0);
289CoPNorM = [];
290WeiCoP = [];
291if ExtractRelationInfo == 1
292        WeiM = sparse(max(NuSup),max(NuSup));
293end
294
295for i = NuSup
296    Neighbor = find( SupNeighborTable(i,:) ~=0);
297    Neighbor = Neighbor( Neighbor> i);
298    for j = Neighbor
299        %  if ~CornerList(i)
300        mask = Sup == i;
301        SizeMaskAll = sum(mask(:));
302        [y x] = find(mask);
303        CenterX = round(median(x));
304        CenterY = round(median(y));
305        y = find(mask(:,CenterX));
306        if ~isempty(y)
307           CenterY = round(median(y));
308        end
309   
310        temp1 = sparse(1, 3*NuSupSize);
311        temp2 = sparse(1, 3*NuSupSize);
312        temp1(:,(Sup2Para( i)*3-2): Sup2Para( i)*3) = ...
313                                                   RayOri(:,CenterY,CenterX)';
314        temp2(:,(Sup2Para( j)*3-2): Sup2Para( j)*3) = ...
315                                                   RayOri(:,CenterY,CenterX)';
316    %NuNei-NuSTList
317
318%        if i < NuNei-NuSTList % only immediate connecting superpixels are weighted using MultiScaleSup
319%       wei = WeiV*10;%*(MultiScaleSupTable(Sup2Para(ClosestNList(i,1)),2:end) == MultiScaleSupTable(Sup2Para(ClosestNList(i,2)),2:end))'; 
320           if MultiScaleFlag
321              vector = (MultiScaleSupTable(Sup2Para( i),2:end) == MultiScaleSupTable(Sup2Para( j),2:end));
322              expV = exp(-10*(WeiV*vector' + ShiftCoP) );
323              wei = 1/(1+expV);
324           else
325              wei = 1;
326           end
327           if ExtractRelationInfo == 1;
328                WeiM(i,j) = wei;
329           end
330%        else
331%           wei = 1;
332%        end
333        oneRay1 = temp1*wei;
334        oneRay2 = temp2*wei;
335    %CoPM1 = [CoPM1; temp1*wei];
336    %CoPM2 = [CoPM2; temp2*wei];
337
338   
339        tempWeiCoP = [SizeMaskAll];
340        CoPEstDepth = [CoPEstDepth; max(median(CleanedDepthMap(mask)),ClosestDist)];
341   
342        mask = Sup == j;
343        SizeMaskAll = sum(mask(:));
344        [y x] = find(mask);
345        CenterX = round(median(x));
346        CenterY = round(median(y));
347        y = find(mask(:,CenterX));
348        if ~isempty(y)
349           CenterY = round(median(y));
350        end
351
352        temp1 = sparse(1, 3*NuSupSize);
353        temp2 = sparse(1, 3*NuSupSize);
354        temp1(:,(Sup2Para( i)*3-2): Sup2Para( i)*3) = ...
355                                                            RayOri(:,CenterY,CenterX)';
356        temp2(:,(Sup2Para( j)*3-2): Sup2Para( j)*3) = ...
357                                                            RayOri(:,CenterY,CenterX)';
358   
359    % Instead of having separate L-1 terms for symmetric co-planar constraint; do the following:
360    % If the penaly was ||.||_2^2 + ||.||_2^2; then the co-efficients are
361    % some kind of average of two rays.  For one norm; we take its average.
362    % (do not divide by 2 because the penalty should be double.
363   
364       CoPM1 = [CoPM1; temp1*wei + oneRay1 ];
365       CoPM2 = [CoPM2; temp2*wei + oneRay2 ];
366   
367       tempWeiCoP = [tempWeiCoP; SizeMaskAll];
368       WeiCoP = [WeiCoP; tempWeiCoP];
369       CoPEstDepth = [CoPEstDepth; max(median(CleanedDepthMap(mask)),ClosestDist)];
370  end
371end%=========================================================================================================
372
373%== find the boundary point that might need to be stick ot each other==========================================
374HoriStickM_i = sparse(0,3*NuSupSize);
375HoriStickM_j = sparse(0,3*NuSupSize);
376HoriStickPointInd = [];
377EstDepHoriStick = [];
378
379MAX_POINTS_STITCH_HORI = 2;    % the actual code will be modified in another copy of the file
380MIN_POINTS_STITCH = 2;      % ERROR:  not used.
381
382% ================================================================
383% NOTE: The actual algorithm should be picking precisely 2 points which are
384% FARTHEST away from the candidate set of neighbors.  This algorithm
385% will ALWAYS work and produce no surprising results.
386
387% In some cases, one may experiment with picking only 1 point when the 2
388% points are too close ---- this will make the algorithm faster; but might
389% produce surprising (e.g. a triangle sticking out) sometimes.
390% An ideal algorithm will reduce the number of points by checking for loops
391% passing through 3 or less superpixels through this matrix; and removing
392% them such that the smallest loop passes through 4 superpixels. (see EE263
393% for a quick algorithm to do this -- involves product of matrices.
394% ==================================================================
395
396DIST_STICHING_THRESHOLD_HORI = 0.4;   
397DIST_STICHING_THRESHOLD_HORI_ONLYCOL = -0.5;    % effectively not used,
398
399SupPixelNeighborList = sparse( max(Sup(:)), max(Sup(:)) );
400SupPixelParsedList = sparse( max(Sup(:)), max(Sup(:)) );
401recordAdded1 = sparse( max(Sup(:)), max(Sup(:)) );
402recordAdded2 = sparse( max(Sup(:)), max(Sup(:)) );
403addedIndexList = [ ];
404
405BounaryPHori = conv2(Sup,[1 -1],'same') ~=0;
406BounaryPHori(:,end) = 0;
407BounaryPVert = conv2(Sup,[1; -1],'same') ~=0;
408BounaryPVert(end,:) = 0;
409
410%    boundariesHoriIndex = find(BounaryPHori==1)';
411% pre-select the boundary in order with the NuSup order
412for l = NuSup
413    mask = Sup == l;
414    boundariesHoriIndex = find(BounaryPHori==1 & mask)';
415    for i = boundariesHoriIndex
416        j = i+Default.VertYNuDepth;
417        if Sup(i) == 0 || Sup(j) == 0
418           continue;
419        end
420        SupPixelParsedList(Sup(i),Sup(j)) = SupPixelParsedList(Sup(i),Sup(j)) + 1;
421   
422        if SupPixelNeighborList(Sup(i),Sup(j)) == 0
423           recordAdded1(Sup(i),Sup(j)) = i;     
424        elseif SupPixelNeighborList(Sup(i),Sup(j)) >= MAX_POINTS_STITCH_HORI
425           continue;
426        elseif SupPixelNeighborList(Sup(i),Sup(j)) == 1  % inside this remove the close stiching terms
427           rowN = ceil(i/55);      colN = rem(i,55);
428           rowN_older = ceil( recordAdded1(Sup(i),Sup(j)) / 55);
429           colN_older = rem( recordAdded1(Sup(i),Sup(j)), 55);
430           if abs(rowN - rowN_older) + (55/305)*abs(colN - colN_older) > DIST_STICHING_THRESHOLD_HORI && ...
431              abs(colN - colN_older) > DIST_STICHING_THRESHOLD_HORI_ONLYCOL
432              recordAdded2(Sup(i),Sup(j)) = i;
433           else
434              continue;
435           end
436        elseif SupPixelNeighborList(Sup(i),Sup(j)) == 2     %Assuming MAX_POINTS_STITCH = 3
437           rowN = ceil(i/55);      colN = rem(i,55);
438           rowN_older1 = ceil( recordAdded1(Sup(i),Sup(j)) / 55);
439           colN_older1 = rem( recordAdded1(Sup(i),Sup(j)), 55);
440           rowN_older2 = ceil( recordAdded2(Sup(i),Sup(j)) / 55);
441           colN_older2 = rem( recordAdded2(Sup(i),Sup(j)), 55);
442       
443           if abs(rowN - rowN_older1) + (55/305)*abs(colN - colN_older1) > DIST_STICHING_THRESHOLD_HORI && ...
444              abs(rowN - rowN_older2) + (55/305)*abs(colN - colN_older2) > DIST_STICHING_THRESHOLD_HORI
445              ;
446           else
447              continue;
448           end   
449        end
450
451        % If you come here, it means you are probably adding it.
452        SupPixelNeighborList(Sup(i),Sup(j)) = SupPixelNeighborList(Sup(i),Sup(j)) + 1;
453        addedIndexList = [addedIndexList i];
454    end
455end
456
457WeightHoriNeighborStitch = [ ];
458
459for i = addedIndexList
460    j = i+Default.VertYNuDepth;
461   
462    WeightHoriNeighborStitch = [WeightHoriNeighborStitch;  SupPixelParsedList(Sup(i),Sup(j)) / ...
463                                            SupPixelNeighborList(Sup(i),Sup(j)) ];
464
465    Target(1) = Sup2Para(Sup(i));
466    Target(2) = Sup2Para(Sup(j));
467    rayBoundary(:,1) =  RayOri(:,i);
468    rayBoundary(:,2) =  RayOri(:,i);
469%    betaTemp = StickHori;%*(DistStickLengthNormWei.^2)*beta(Target(I));
470%    betaTemp = StickHori*WeiV;%*(MultiScaleSupTable(Sup2Para(Sup(i)),2:end) == MultiScaleSupTable(Sup2Para(Sup(j)),2:end))';%*(DistStickLengthNormWei.^2)*beta(Target(I));
471    if MultiScaleFlag
472          vector = (MultiScaleSupTable(Sup2Para(Sup(i)),2:end) == MultiScaleSupTable(Sup2Para(Sup(j)),2:end));
473          expV = exp(-10*(WeiV*vector' + ShiftStick) );
474       betaTemp = StickHori*(0.5+1/(1+expV)); %*(DistStickLengthNormWei.^2)*beta(Target(I));
475       % therr should always be sticking (know don't care about occlusion)
476    else
477       betaTemp = StickHori;
478    end
479    temp = sparse(3,NuSupSize);
480    temp(:,Target(1)) = rayBoundary(:,1);
481    HoriStickM_i = [HoriStickM_i; betaTemp*temp(:)'];
482    temp = sparse(3,NuSupSize);
483    temp(:,Target(2)) = rayBoundary(:,2);
484    HoriStickM_j = [HoriStickM_j; betaTemp*temp(:)'];
485    EstDepHoriStick = [EstDepHoriStick; sqrt(max(CleanedDepthMap(i),ClosestDist)*max(CleanedDepthMap(j),ClosestDist))];
486    HoriStickPointInd = [HoriStickPointInd i ];
487
488end
489% ==============================================
490
491
492
493% ======== finding the unucessary stiching points in Vertical direction ====
494VertStickM_i = sparse(0,3*NuSupSize);
495VertStickM_j = sparse(0,3*NuSupSize);
496VertStickPointInd = [];
497EstDepVertStick = [];
498
499MAX_POINTS_STITCH_VERT = 4;     %3
500DIST_STICHING_THRESHOLD_VERT = 0.1;     %0.3
501DIST_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.
502
503SupPixelNeighborList = sparse( max(Sup(:)), max(Sup(:)) );
504SupPixelParsedList = sparse( max(Sup(:)), max(Sup(:)) );
505recordAdded1 = sparse( max(Sup(:)), max(Sup(:)) );
506recordAdded2 = sparse( max(Sup(:)), max(Sup(:)) );
507addedIndexList = [ ];
508
509% pre-select the boundary in order with the NuSup order
510for l = NuSup
511    mask = Sup == l;
512%    for i = find(BounaryPVert==1)'
513    for i = find(BounaryPVert==1 & mask)'
514        j = i+1;
515        if Sup(i) == 0 || Sup(j) == 0
516           continue;
517        end
518        SupPixelParsedList(Sup(i),Sup(j)) = SupPixelParsedList(Sup(i),Sup(j)) + 1;
519   
520        if SupPixelNeighborList(Sup(i),Sup(j)) == 0
521           recordAdded1(Sup(i),Sup(j)) = i;
522        elseif SupPixelNeighborList(Sup(i),Sup(j)) >= MAX_POINTS_STITCH_VERT
523           continue;
524        elseif SupPixelNeighborList(Sup(i),Sup(j)) == 1  % inside this remove the close stiching terms
525           rowN = ceil(i/55);      colN = rem(i,55);
526           rowN_older = ceil( recordAdded1(Sup(i),Sup(j)) / 55);
527           colN_older = rem( recordAdded1(Sup(i),Sup(j)), 55);
528           if abs(rowN - rowN_older) + (55/305)*abs(colN - colN_older) > DIST_STICHING_THRESHOLD_VERT && ...
529                 abs(colN - colN_older) > DIST_STICHING_THRESHOLD_VERT_ONLYCOL
530              recordAdded2(Sup(i),Sup(j)) = i;
531           else
532              continue;
533           end
534        elseif SupPixelNeighborList(Sup(i),Sup(j)) == 2     %Assuming MAX_POINTS_STITCH = 3
535           rowN = ceil(i/55);      colN = rem(i,55);
536           rowN_older1 = ceil( recordAdded1(Sup(i),Sup(j)) / 55);
537           colN_older1 = rem( recordAdded1(Sup(i),Sup(j)), 55);
538           rowN_older2 = ceil( recordAdded2(Sup(i),Sup(j)) / 55);
539           colN_older2 = rem( recordAdded2(Sup(i),Sup(j)), 55);
540       
541           if abs(rowN - rowN_older1) + (55/305)*abs(colN - colN_older1) > DIST_STICHING_THRESHOLD_VERT && ...
542              abs(rowN - rowN_older2) + (55/305)*abs(colN - colN_older2) > DIST_STICHING_THRESHOLD_VERT
543              ;
544           else
545              continue;
546           end       
547        end
548
549        % If you come here, it means you are probably adding it.
550        SupPixelNeighborList(Sup(i),Sup(j)) = SupPixelNeighborList(Sup(i),Sup(j)) + 1;
551        addedIndexList = [addedIndexList i];
552    end
553end
554
555
556WeightVertNeighborStitch = [ ];
557
558for i = addedIndexList
559    j = i+1;
560   
561     WeightVertNeighborStitch = [WeightVertNeighborStitch;  SupPixelParsedList(Sup(i),Sup(j)) / ...
562                                            SupPixelNeighborList(Sup(i),Sup(j)) ];
563
564    Target(1) = Sup2Para(Sup(i));
565    Target(2) = Sup2Para(Sup(j));
566    rayBoundary(:,1) =  RayOri(:,i);
567    rayBoundary(:,2) =  RayOri(:,i);
568    if MultiScaleFlag
569       vector = (MultiScaleSupTable(Sup2Para(Sup(i)),2:end) == MultiScaleSupTable(Sup2Para(Sup(j)),2:end));
570       expV = exp(-10*(WeiV*vector' + ShiftStick) );
571       betaTemp = StickVert*(0.5+1/(1+expV));
572       % therr should always be sticking (know don't care about occlusion)
573    else
574       betaTemp = StickVert;
575    end
576    temp = sparse(3,NuSupSize);
577    temp(:,Target(1)) = rayBoundary(:,1);
578    VertStickM_i = [VertStickM_i; betaTemp*temp(:)'];
579    temp = sparse(3,NuSupSize);
580    temp(:,Target(2)) = rayBoundary(:,2);
581    VertStickM_j = [VertStickM_j; betaTemp*temp(:)'];
582    EstDepVertStick = [EstDepVertStick; sqrt(max(CleanedDepthMap(i),ClosestDist)*max(CleanedDepthMap(j),ClosestDist))];
583    VertStickPointInd = [VertStickPointInd i ];
584
585end
586% ====finished finding the unucessary stiching points in Vertical direction ===
587
588
589% ======================================Finish building up matrix=====================hard work======================
590
591
592
593% ================================================================================================================
594depthfile = strrep(Default.filename{k},'img','depth_learned'); %
595
596
597% Start Decompose the image align with superpixels ==================================================================================
598% define the decomposition in X direction only
599
600
601 
602TotalRectX = 2*XNuDecompose-1;
603TotalRectY= 2*YNuDecompose-1;
604PlanePara = NaN*ones(3*NuSupSize,1); % setup the lookuptable for the solved plane parameter
605
606 opt = sdpsettings('solver','sedumi','cachesolvers',1,'sedumi.eps',1e-6,'verbose',solverVerboseLevel);
607%     opt = sdpsettings('solver','lpsolve','cachesolvers',1,'verbose',5);
608%     opt = sdpsettings('solver','lpsolve','cachesolvers',1,'showprogress',1, 'verbose',4);
609%    opt = sdpsettings('solver','glpk','cachesolvers',1, 'usex0', 1);   
610%    opt = sdpsettings('solver','glpk', 'usex0', 1);   
611    %opt = sdpsettings('solver','linprog','cachesolvers',1);   
612%    opt = sdpsettings('solver','bpmpd');
613   
614for k = 0:(TotalRectX-1)
615  l = rem(k*2,(TotalRectX));
616  RangeX = (1+ceil(Default.HoriXNuDepth/XNuDecompose)*l/2):...
617            min((1+ceil(Default.HoriXNuDepth/XNuDecompose)*(l/2+1)),Default.HoriXNuDepth);
618  RangeX = ceil(RangeX);
619  for q = 0:(TotalRectY-1)
620    l = rem(q*2,(TotalRectY));
621    RangeY = (1+ceil(Default.VertYNuDepth/YNuDecompose)*l/2):...
622              min((1+ceil(Default.VertYNuDepth/YNuDecompose)*(l/2+1)),Default.VertYNuDepth);
623    RangeY = ceil(RangeY);
624    mask = zeros(size(Sup));
625    mask(RangeY,RangeX) = 1;
626    mask =logical(mask);
627    SubSup = sort(setdiff(unique( reshape( Sup(RangeY,RangeX),1,[])),0));
628    BoundarySup = [];
629   
630        if any(SubSup <=0)
631       SubSup(SubSup<=0)
632    end
633    BoundarySup = find(sum(SupNeighborTable(SubSup,:), 1) ~=0);
634    BoundarySup = unique(setdiff(BoundarySup,[0 SubSup] ));
635   
636        % chech if BoundarySup non-NaN in PlanePara
637    checkNoNNaN = ~isnan(PlanePara(Sup2Para(BoundarySup)*3));
638    BoundarySup = BoundarySup(checkNoNNaN);
639    TotalSup = sort([SubSup BoundarySup]);
640
641    SubSupPtr = [ Sup2Para(SubSup)*3-2;...
642                    Sup2Para(SubSup)*3-1;...
643                    Sup2Para(SubSup)*3];
644    SubSupPtr = SubSupPtr(:);
645    BoundarySupPtr = [ Sup2Para(BoundarySup)*3-2;...
646                       Sup2Para(BoundarySup)*3-1;...
647                       Sup2Para(BoundarySup)*3];
648    BoundarySupPtr = BoundarySupPtr(:);
649    NuSubSupSize = size(SubSup,2);
650   
651    % clearn RayAllM PosiM CoPM1 HoriStickM_i VertStickM_i
652    NewRayAllM = RayAllM(:,SubSupPtr);
653    tar = sum(NewRayAllM ~= 0,2) == 3;
654    NewRayAllM = NewRayAllM(tar,:);
655    NewPosiM = PosiM(:,SubSupPtr);
656    tar = sum(NewPosiM ~= 0,2) == 3;
657    NewPosiM = NewPosiM(tar,:);
658    NewVarM = VarM(tar);
659   
660    NewCoPM = CoPM1(:,SubSupPtr) - CoPM2(:,SubSupPtr);
661    NewCoPMBound = CoPM1(:,BoundarySupPtr) - CoPM2(:,BoundarySupPtr);
662   
663        tar = sum(NewCoPM ~= 0,2) + sum(NewCoPMBound ~= 0,2)==6;
664    NewCoPMBound = NewCoPMBound*PlanePara(BoundarySupPtr);
665    NewCoPM = NewCoPM(tar,:);
666    NewCoPMBound = NewCoPMBound(tar);
667    NewCoPEstDepth = CoPEstDepth(tar);   
668
669    NewHoriStickM = HoriStickM_i(:,SubSupPtr)-HoriStickM_j(:,SubSupPtr);
670    NewHoriStickMBound = HoriStickM_i(:,BoundarySupPtr)-HoriStickM_j(:,BoundarySupPtr);
671   
672        tar = sum(NewHoriStickM ~= 0,2)+  sum(NewHoriStickMBound ~= 0,2) ==6;
673    NewHoriStickM = NewHoriStickM(tar,:);   
674    BoundarySupPtr = [ Sup2Para(BoundarySup)*3-2;...
675                       Sup2Para(BoundarySup)*3-1;...
676                       Sup2Para(BoundarySup)*3];
677    BoundarySupPtr = BoundarySupPtr(:);
678    NuSubSupSize = size(SubSup,2);
679   
680    % clearn RayAllM PosiM CoPM1 HoriStickM_i VertStickM_i
681    NewRayAllM = RayAllM(:,SubSupPtr);
682    tar = sum(NewRayAllM ~= 0,2) == 3;
683    NewRayAllM = NewRayAllM(tar,:);
684    NewPosiM = PosiM(:,SubSupPtr);
685    tar = sum(NewPosiM ~= 0,2) == 3;
686    NewPosiM = NewPosiM(tar,:);
687    NewVarM = VarM(tar);
688   
689    NewCoPM = CoPM1(:,SubSupPtr) - CoPM2(:,SubSupPtr);
690    NewCoPMBound = CoPM1(:,BoundarySupPtr) - CoPM2(:,BoundarySupPtr);
691   
692        tar = sum(NewCoPM ~= 0,2) + sum(NewCoPMBound ~= 0,2)==6;
693    NewCoPMBound = NewCoPMBound*PlanePara(BoundarySupPtr);
694    NewCoPM = NewCoPM(tar,:);
695    NewCoPMBound = NewCoPMBound(tar);
696    NewCoPEstDepth = CoPEstDepth(tar);   
697
698    NewHoriStickM = HoriStickM_i(:,SubSupPtr)-HoriStickM_j(:,SubSupPtr);
699    NewHoriStickMBound = HoriStickM_i(:,BoundarySupPtr)-HoriStickM_j(:,BoundarySupPtr);
700   
701        tar = sum(NewHoriStickM ~= 0,2)+  sum(NewHoriStickMBound ~= 0,2) ==6;
702    NewHoriStickM = NewHoriStickM(tar,:);   
703    NewEstDepHoriStick = EstDepHoriStick(tar);
704    NewHoriStickMBound = NewHoriStickMBound*PlanePara(BoundarySupPtr);
705    NewHoriStickMBound = NewHoriStickMBound(tar);
706        NewWeightHoriNeighborStitch = WeightHoriNeighborStitch(tar);
707
708    NewVertStickM = VertStickM_i(:,SubSupPtr)-VertStickM_j(:,SubSupPtr);
709    NewVertStickMBound = VertStickM_i(:,BoundarySupPtr)-VertStickM_j(:,BoundarySupPtr);
710   
711        tar = sum(NewVertStickM ~= 0,2) + sum(NewVertStickMBound ~= 0,2)==6;
712    NewVertStickM = NewVertStickM(tar,:);
713    NewEstDepVertStick = EstDepVertStick(tar);
714    NewVertStickMBound = NewVertStickMBound*PlanePara(BoundarySupPtr);
715    NewVertStickMBound = NewVertStickMBound(tar);
716        NewWeightVertNeighborStitch = WeightVertNeighborStitch(tar);
717   
718 
719    for i = step
720        ParaPPCP = sdpvar(3*NuSubSupSize,1);
721        F = set(ParaPPCP(3*(1:NuSubSupSize)-1).*YPointer(Sup2Para(SubSup))<=0)...
722            +set(NewRayAllM*ParaPPCP <=1/ClosestDist)...
723            +set(NewRayAllM*ParaPPCP >=1/FarestDist);
724    if i == 1 % W/O  priors
725       solvesdp(F,norm( (PosiM*ParaPPCP-ones(size(PosiM,1),1))./exp(abs(VarM)/BandWith),1)...
726             , opt);
727    elseif i ==2 % Coner
728       solvesdp(F,norm( (PosiM*ParaPPCP-ones(size(PosiM,1),1))./exp(abs(VarM)/BandWith),1)...
729             +Center*norm(((CoPM1 - CoPM2)*ParaPPCP).*CoPEstDepth, 1)...
730             , opt);
731    else % all the priors
732   
733
734% =========  Debugging the code for numerical accuracy  ==========
735
736
737
738% ================================================================
739
740        WeightsSelfTerm = 1 ./ exp(abs(NewVarM)/BandWith);
741   
742   
743    %============ solver specific options ======
744    if strcmp(opt.solver, 'glpk')
745        disp('Using x0 for GPLK');
746        ParaPPCP = NewPosiM \ ones(size(NewPosiM,1),1);
747    end
748
749       if NOYALMIP
750   fprintf(['     ' num2str( toc(inferenceTime) ) '\n     : In 1st level Optimization, using ' ...
751                                        opt.solver '.  ' ...
752            '(' num2str(k+1) '/' num2str((TotalRectX-1)+1) ',' num2str(l+1) '/' num2str((TotalRectY-1)+1) ')']);
753%          sol = solvesdp([], norm( (NewPosiM*ParaPPCP-ones(size(NewPosiM,1),1)) .* WeightsSelfTerm,1)...
754        fprintf([ '    Starting Sedumi at: ' num2str( toc(inferenceTime) ) '\n ']);
755          sol = solvesdp(F, norm( (NewPosiM*ParaPPCP-ones(size(NewPosiM,1),1)) .* WeightsSelfTerm,1)...
756                        + Center*norm(((NewCoPM)*ParaPPCP + NewCoPMBound).*NewCoPEstDepth, 1)...
757                        + norm(((NewHoriStickM)*ParaPPCP + NewHoriStickMBound).*...
758                                 NewEstDepHoriStick.*NewWeightHoriNeighborStitch,1)...
759                        + norm(((NewVertStickM)*ParaPPCP + NewVertStickMBound).*...
760                                 NewEstDepVertStick.*NewWeightVertNeighborStitch,1) ...
761                        , opt);
762        fprintf([ '    Finished Sedumi at: ' num2str( toc(inferenceTime) ) '\n ' ]);
763%opt
764%mean(abs(ParaPPCP(:))) 
765        x_sedumi = double(ParaPPCP);
766        %     sum(isnan(ParaPPCP))
767        yalmip('clear');
768end
769     
770        if NEW_OPTIMIZATION
771   fprintf(['     ' num2str( toc(inferenceTime) ) '\n     : In 1st level Optimization, using new solver.' ...
772            '(' num2str(k+1) '/' num2str((TotalRectX-1)+1) ',' num2str(l+1) '/' num2str((TotalRectY-1)+1) ')']);
773
774        global A b S inq
775
776        A = [ sparse(1:length(WeightsSelfTerm),1:length(WeightsSelfTerm),WeightsSelfTerm ) * NewPosiM;...
777          sparse(1:length(NewCoPEstDepth),1:length(NewCoPEstDepth),NewCoPEstDepth*Center ) * NewCoPM;...
778          sparse(1:length(NewEstDepHoriStick), 1:length(NewEstDepHoriStick), NewEstDepHoriStick.*NewWeightHoriNeighborStitch) * NewHoriStickM;...
779          sparse(1:length(NewEstDepVertStick), 1:length(NewEstDepVertStick), NewEstDepVertStick.*NewWeightVertNeighborStitch) * NewVertStickM;...
780            ];
781     
782        b = [ ones(size(NewPosiM,1),1) .* WeightsSelfTerm;...
783         -NewCoPMBound.*NewCoPEstDepth*Center;...
784         -NewHoriStickMBound.*NewEstDepHoriStick.*NewWeightHoriNeighborStitch;...
785         -NewVertStickMBound.*NewEstDepVertStick.*NewWeightVertNeighborStitch];
786         
787        %norm(NewCoPMBound.*NewCoPEstDepth*Center)
788
789        temp = zeros(1, NuSubSupSize*3);
790        temp(3*(1:NuSubSupSize)-1) = YPointer(Sup2Para(SubSup));
791        temp = sparse(1:length(temp), 1:length(temp), temp);
792        temp( sum(temp,2) ==0,:) = [];
793        S = [temp;...
794                NewRayAllM;...
795                -NewRayAllM];
796           inq = [ sparse(size(temp,1), 1);...
797                 - 1/ClosestDist*ones(size(NewRayAllM,1),1);...
798                 1/FarestDist*ones(size(NewRayAllM,1),1)];
799        Para.ClosestDist = ClosestDist;
800        Para.FarestDist = FarestDist;
801    Para.ptry = [];
802    Para.ptrz = [];
803    Para.Dist_Start = [];
804       
805%       fprintf([ '    Starting new solver at: ' num2str( toc(inferenceTime) ) '\n ']);
806                [x_ashIterator, alfa, status, history, T_nt_hist] = ...
807                                SigmoidLogBarrierSolver( Para, [], [], [], '', [], [], solverVerboseLevel);
808        if any(S*x_ashIterator+inq > 0 )
809                disp('Inequality not satisfied');
810                max( S*x_ashIterator+inq)
811        elseif status == 2
812                fprintf([' Success with alfa=' num2str(alfa)]);
813        end
814%       fprintf([ '    Finished new solver at: ' num2str( toc(inferenceTime) ) '\n ' ]);
815
816        ParaPPCP = x_ashIterator;
817     
818        end
819    end
820
821%       norm(x_ashIterator)
822%       norm(x_sedumi)
823%       norm(x_ashIterator-x_sedumi)
824    PlanePara(SubSupPtr) = ParaPPCP;
825
826% %    SepPointMeasureHori = 1./(HoriStickM_i*ParaPPCP)-1./(HoriStickM_j*ParaPPCP);
827% %    SepPointMeasureVert = 1./(VertStickM_i*ParaPPCP)-1./(VertStickM_j*ParaPPCP);
828%     ParaPPCP = reshape(ParaPPCP,3,[]);
829%     %any(any(isnan(ParaPPCP)))
830%     % porject the ray on planes to generate the ProjDepth
831%     FitDepthPPCP = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
832%     FitDepthPPCP(~maskSkyEroded & mask) = (1./sum(ParaPPCP(:,TotalSup2Para(SupEpand(~maskSkyEroded & mask))).*Ray(:,~maskSkyEroded & mask),1))';
833%     FitDepthPPCP = reshape(FitDepthPPCP,Default.VertYNuDepth,[]);
834%     % storage for comparison purpose ======================
835% %    depthMap = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
836% %    depthMap(~maskSkyEroded | mask) = (1./sum(ParaPPCP(:,TotalSup2Para(SupEpand(~maskSkyEroded | mask))).*RayOri(:,~maskSkyEroded | mask),1))';
837% %    depthMap = reshape(depthMap,Default.VertYNuDepth,[]);
838% %    if baseline == 0
839% %       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '/']);
840% %       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '/' depthfile '.mat'],'depthMap');
841% %    elseif baseline == 1
842% %       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline/']);
843% %       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline/' depthfile '.mat'],'depthMap');
844% %    else
845% %       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline2/']);
846% %       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline2/' depthfile '.mat'],'depthMap');
847% %    end
848%   
849%     % =====================================================
850%     [Position3DFitedPPCP] = im_cr2w_cr(FitDepthPPCP,permute(Ray,[2 3 1]));
851%     if true %RenderVrmlFlag && i==3
852%          Position3DFitedPPCP(3,:) = -Position3DFitedPPCP(3,:);
853%          Position3DFitedPPCP = permute(Position3DFitedPPCP,[2 3 1]);
854%          RR =permute(Ray,[2 3 1]);
855%          temp = RR(:,:,1:2)./repmat(RR(:,:,3),[1 1 2]);
856%          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]);
857%          PositionTex = permute(PositionTex,[2 3 1]);
858%          WrlFacest(Position3DFitedPPCP,PositionTex,Sup,Default.filename{1},'./')
859%
860% %        [VrmlName] = vrml_test_faceset_goodSkyBoundary(      Default.filename{k}, Position3DFitedPPCP, FitDepthPPCP, permute(Ray,[2 3 1]), [Name{i} DepthFolder 'ExpVar'], ...
861% %                     [], [], 0, maskSkyEroded, maskG, 1, 0, Default.a_default, Default.b_default, Default.Ox_default, Default.Oy_default);
862% %        system(['gzip -9 -c ' Default.ScratchDataFolder '/vrml/' VrmlName ' > ' Default.ScratchDataFolder '/vrml/' VrmlName '.gz']);
863%        %delete([Default.ScratchDataFolder '/vrml/' VrmlName]);
864%     end
865%    save([Default.ScratchDataFolder '/data/PreDepth/FitDepthPPCP' num2str(k) '.mat'],'FitDepthPPCP','SepPointMeasureHori',...
866%      'SepPointMeasureVert','VertStickPointInd','HoriStickPointInd');
867%save([Default.ScratchDataFolder '/data/All.mat']);
868  end
869  end
870end
871% build the whole image
872    PlanePara = reshape(PlanePara,3,[]);
873    %any(any(isnan(PlanePara)))
874    % porject the ray on planes to generate the ProjDepth
875    FitDepthPPCP = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
876    FitDepthPPCP(~maskSkyEroded) = (1./sum(PlanePara(:,Sup2Para(SupEpand(~maskSkyEroded ))).*Ray(:,~maskSkyEroded ),1))';
877    FitDepthPPCP = reshape(FitDepthPPCP,Default.VertYNuDepth,[]);
878    % storage for comparison purpose ======================
879%    depthMap = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
880%    depthMap(~maskSkyEroded | mask) = (1./sum(PlanePara(:,TotalSup2Para(SupEpand(~maskSkyEroded | mask))).*RayOri(:,~maskSkyEroded | mask),1))';
881%    depthMap = reshape(depthMap,Default.VertYNuDepth,[]);
882%    if baseline == 0
883%       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '/']);
884
885
886
887%       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '/' depthfile '.mat'],'depthMap');
888%    elseif baseline == 1
889%       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline/']);
890%       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline/' depthfile '.mat'],'depthMap');
891%    else
892%       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline2/']);
893%       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline2/' depthfile '.mat'],'depthMap');
894%    end
895
896    % =====================================================
897    [Position3DFitedPPCP] = im_cr2w_cr(FitDepthPPCP,permute(Ray,[2 3 1]));
898    if NoSecondStep %RenderVrmlFlag && i==3
899         Position3DFitedPPCP(3,:) = -Position3DFitedPPCP(3,:);
900         Position3DFitedPPCP = permute(Position3DFitedPPCP,[2 3 1]);
901         RR =permute(Ray,[2 3 1]);
902         temp = RR(:,:,1:2)./repmat(RR(:,:,3),[1 1 2]);
903         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]);
904         PositionTex = permute(PositionTex,[2 3 1])
905         disp('First level Wrl');
906         WrlFacestHroiReduce(Position3DFitedPPCP,PositionTex,SupOri, [ Default.filename{1} '1st'],[ Default.filename{1} '1st'], ...
907                             Default.OutPutFolder, GridFlag, 0);
908         system(['gzip -9 -c ' Default.OutPutFolder Default.filename{1} '1st.wrl > ' ...
909                Default.OutPutFolder Default.filename{1} '1st.wrl.gz']);
910         %system(['cp '  Default.OutPutFolder Default.filename{1} '1st.wrl.gz ' ...
911         %       Default.OutPutFolder Default.filename{1} '1st.wrl']);
912         copyfile([ Default.OutPutFolder Default.filename{1} '1st.wrl.gz '], ...
913                [ Default.OutPutFolder Default.filename{1} '1st.wrl'],'f');
914         delete([Default.OutPutFolder Default.filename{1} '1st.wrl.gz']);
915    end
916   
917         if Catsh1stOptFlag
918            disp([Default.ScratchFolder '/Catsh1stOpt_' Default.filename{1} '.mat']);
919            save([Default.ScratchFolder '/Catsh1stOpt_' Default.filename{1} '.mat'],'NewPosiM','WeightsSelfTerm',...
920                       'Center','NewCoPM','NewCoPMBound','NewCoPEstDepth',...
921                       'NewHoriStickM','NewHoriStickMBound','NewEstDepHoriStick','NewWeightHoriNeighborStitch',...
922                       'NewVertStickM','NewVertStickMBound','NewEstDepVertStick','NewWeightVertNeighborStitch',...
923                       'PlanePara',...
924                       'NuSubSupSize','YPointer','SubSup','ClosestDist','FarestDist','NewRayAllM',...
925                       'PositionTex','SupOri','Ray','FarestDist','Sup2Para','SupEpand','maskSkyEroded');
926            return;
927         end
928
929%return;
930%==================Finished for one step MRF==========================================================================================================
931
932if NoSecondStep
933        return;
934end
935
936
937
938
939
940
941
942
943
944% ====================================following are 2nd step MRF to give more visually pleasing result=======================================
945;% generating new PosiMPPCP using the new losition
946
947% save([Default.ScratchDataFolder '/data/VertShiftVrml.mat' ] );
948%  groundThreshold = cos(5*pi/180);  % make small range
949%  verticalThreshold = cos(50*pi/180);
950  normPara = norms(PlanePara);
951  normalizedPara = PlanePara ./ repmat( normPara, [3 1]);
952  groundPara = abs(normalizedPara(2,:)) >= groundThreshold(YPosition);
953  groundParaInd = find(groundPara);
954  verticalPara = abs(normalizedPara(2,:)) <= verticalThreshold(YPosition); % change to have different range of vertical thre ================
955  verticalParaInd = find(verticalPara);
956
957  indexVertical = find( verticalPara)*3-1;
958  indexGroundX = find( groundPara)*3-2;
959  indexGroundZ = find( groundPara)*3;
960
961  PosiMPPCP = sparse(0,0);
962  VarM2 = sparse(0,0);
963   
964        % forming new supporting matrix using new depth and get rid of the support of the vertical plane
965  for i = NuSup
966      mask = Sup == i;
967      if any(verticalParaInd == Sup2Para(i)) & FlagRemoveVerticalSupport % add by Min Aug 14th
968         mask = logical(zeros(size(Sup)));
969      end
970         PosiMPPCP = blkdiag(PosiMPPCP, Position3DFitedPPCP(:,mask)');
971         VarM2 = [VarM2; VarMap(mask)];
972         if length( find(isnan(PosiMPPCP)) )
973              disp('PosiMPPCP is NaN');
974         end
975  end
976
977% Start Decompose image =========================
978  TotalRectX = 2*XNuDecompose-1;
979  TotalRectY = 2*YNuDecompose-1;
980  PlanePara = NaN*ones(3*NuSupSize,1); % setup the lookuptable for the solved plane parameter
981
982  opt = sdpsettings('solver','sedumi','cachesolvers',1, 'verbose', solverVerboseLevel);
983  %     opt = sdpsettings('solver','lpsolve','cachesolvers',1);
984  %    opt = sdpsettings('solver','glpk','cachesolvers',1);   
985
986  for k = 0:(TotalRectX-1)
987    l = rem(k*2,(TotalRectX));
988    RangeX = (1+ceil(Default.HoriXNuDepth/XNuDecompose)*l/2):...
989              min((1+ceil(Default.HoriXNuDepth/XNuDecompose)*(l/2+1)),Default.HoriXNuDepth);
990    RangeX = ceil(RangeX);
991    for q = 0:(TotalRectY-1)
992      l = rem(q*2,(TotalRectY));
993      RangeY = (1+ceil(Default.VertYNuDepth/YNuDecompose)*l/2):...
994                min((1+ceil(Default.VertYNuDepth/YNuDecompose)*(l/2+1)),Default.VertYNuDepth);
995      RangeY = ceil(RangeY);
996      mask = zeros(size(Sup));
997      mask(RangeY,RangeX) = 1;
998      mask =logical(mask);
999      SubSup = sort(setdiff( unique( reshape( Sup(RangeY,RangeX),1,[])),0));
1000      BoundarySup = [];
1001      BoundarySup = find(sum(SupNeighborTable(SubSup,:), 1) ~=0);
1002      BoundarySup = unique(setdiff(BoundarySup,[0 SubSup] ));
1003
1004      % chech if BoundarySup non-NaN in PlanePara
1005      checkNoNNaN = ~isnan(PlanePara(Sup2Para(BoundarySup)*3));
1006      BoundarySup = BoundarySup(checkNoNNaN);
1007      TotalSup = sort([SubSup BoundarySup]);
1008
1009      SubSupPtr = [ Sup2Para(SubSup)*3-2;...
1010                    Sup2Para(SubSup)*3-1;...
1011                    Sup2Para(SubSup)*3];
1012      SubSupPtr = SubSupPtr(:);
1013      BoundarySupPtr = [ Sup2Para(BoundarySup)*3-2;...
1014                         Sup2Para(BoundarySup)*3-1;...
1015                         Sup2Para(BoundarySup)*3];
1016      BoundarySupPtr =BoundarySupPtr(:);
1017      NuSubSupSize = size(SubSup,2);
1018      SubSup2Para = sparse(1,max(SubSup));
1019      SubSup2Para(SubSup) = 1:NuSubSupSize;
1020   
1021      % clearn RayAllM PosiM CoPM1 HoriStickM_i VertStickM_i
1022      NewRayAllM = RayAllM(:,SubSupPtr);
1023      tar = sum(NewRayAllM ~= 0,2) ==3;
1024      NewRayAllM = NewRayAllM(tar,:);
1025
1026      NewPosiMPPCP = PosiMPPCP(:,SubSupPtr);
1027      tar = sum(NewPosiMPPCP ~= 0,2) ==3;
1028      NewPosiMPPCP = NewPosiMPPCP(tar,:);
1029      NewVarM = VarM2(tar);
1030
1031      NewCoPM = CoPM1(:,SubSupPtr) - CoPM2(:,SubSupPtr);
1032      NewCoPMBound = CoPM1(:,BoundarySupPtr) - CoPM2(:,BoundarySupPtr);
1033      tar = sum( NewCoPM ~= 0,2) + sum( NewCoPMBound ~= 0,2) ==6;
1034      NewCoPM = NewCoPM(tar,:);
1035      NewCoPMBound = NewCoPMBound*PlanePara(BoundarySupPtr); % column vertor
1036      NewCoPMBound = NewCoPMBound(tar);
1037      NewCoPEstDepth = CoPEstDepth(tar);   
1038
1039      NewHoriStickM = HoriStickM_i(:,SubSupPtr)-HoriStickM_j(:,SubSupPtr);
1040      NewHoriStickMBound = HoriStickM_i(:,BoundarySupPtr)-HoriStickM_j(:,BoundarySupPtr);
1041      tar = sum(NewHoriStickM ~= 0,2) + sum( NewHoriStickMBound ~= 0,2)==6;
1042      NewHoriStickM = NewHoriStickM(tar,:);
1043      NewHoriStickMBound = NewHoriStickMBound*PlanePara(BoundarySupPtr); % column vertor
1044      NewHoriStickMBound = NewHoriStickMBound(tar);
1045      NewEstDepHoriStick = EstDepHoriStick(tar);
1046         NewWeightHoriNeighborStitch = WeightHoriNeighborStitch(tar);
1047
1048      NewVertStickM = VertStickM_i(:,SubSupPtr)-VertStickM_j(:,SubSupPtr);
1049      NewVertStickMBound = VertStickM_i(:, BoundarySupPtr)-VertStickM_j(:,BoundarySupPtr);
1050      tar = sum(NewVertStickM ~= 0,2) + sum(NewVertStickMBound ~= 0,2)==6;
1051      NewVertStickM = NewVertStickM(tar,:);
1052      NewVertStickMBound = NewVertStickMBound*PlanePara(BoundarySupPtr); % column vertor
1053      NewVertStickMBound = NewVertStickMBound(tar);
1054      NewEstDepVertStick = EstDepVertStick(tar);
1055         NewWeightVertNeighborStitch = WeightVertNeighborStitch(tar);
1056
1057%    try reduce the vertical constrain
1058     NonVertPtr = setdiff( 1:(3*NuSubSupSize), SubSup2Para( NuSup( (intersect( indexVertical,SubSupPtr ) +1)/3))*3-1 );
1059     YNoComingBack = YPointer(Sup2Para(SubSup));
1060     YNoComingBack(SubSup2Para( NuSup( (intersect( indexVertical,SubSupPtr ) +1)/3))) = [];
1061     YCompMask = zeros(1,3*NuSubSupSize);
1062     YCompMask(3*(1:NuSubSupSize)-1) = 1;
1063     YCompMask = YCompMask(NonVertPtr);
1064     XCompMask = zeros(1,3*NuSubSupSize);
1065     XCompMask( SubSup2Para( NuSup( (intersect( indexGroundX,SubSupPtr ) +2)/3))*3-2 ) = 1;
1066     XCompMask = XCompMask(NonVertPtr);
1067     ZCompMask = zeros(1,3*NuSubSupSize);
1068     ZCompMask( SubSup2Para( NuSup( (intersect( indexGroundZ,SubSupPtr ) )/3))*3 ) = 1;
1069     ZCompMask = ZCompMask(NonVertPtr);
1070     GroundMask = intersect( find(groundPara), ( SubSupPtr( 3:3:( size(SubSupPtr,1)) )/3 ) ) ;
1071
1072     if NOYALMIP
1073      Para = sdpvar( size(NonVertPtr',1),1);
1074      F = set(Para( logical(YCompMask)).*YNoComingBack<=0)...
1075          +set(NewRayAllM(:, NonVertPtr)*Para <=1/ClosestDist)...
1076          +set(NewRayAllM(:, NonVertPtr)*Para >=1/FarestDist);
1077
1078    fprintf([ '     ' num2str( toc(inferenceTime) ) '\n     : In 2nd level Optimization, using ' opt.solver '.  ' ...
1079                     '(' num2str(k+1) '/' num2str((TotalRectX-1)+1) ',' num2str(l+1) '/' num2str((TotalRectY-1)+1) ')']);
1080       
1081    sol=solvesdp(F,norm( ( NewPosiMPPCP(:, NonVertPtr)*Para-ones(size(NewPosiMPPCP,1),1))./exp(abs(NewVarM)/BandWith),1)...
1082                 +Center*norm(( NewCoPM(:, NonVertPtr)*Para + NewCoPMBound).*NewCoPEstDepth, 1)...
1083                 +norm(( NewHoriStickM(:, NonVertPtr)*Para + NewHoriStickMBound).*...
1084                         NewEstDepHoriStick.*NewWeightHoriNeighborStitch,1)...
1085                 +norm(( NewVertStickM(:, NonVertPtr)*Para + NewVertStickMBound).*...
1086                         NewEstDepVertStick.*NewWeightVertNeighborStitch,1)...
1087                 +10*norm( ( Para( logical(XCompMask) ))./...
1088                            normPara( GroundMask )', 1)...
1089                 +10*norm( ( Para( logical(ZCompMask)))./...
1090                            normPara( GroundMask )', 1) ...
1091                 , opt);
1092
1093     Para = double(Para);
1094     %sum(isnan(Para))
1095     yalmip('clear');
1096     tempPara = zeros(3*NuSubSupSize,1);
1097     tempPara(NonVertPtr) = Para;
1098     PlanePara(SubSupPtr) = tempPara;
1099     
1100     elseif NEW_OPTIMIZATION
1101        % version written by Ashutosh
1102       
1103       tempGroundX = sparse(1:length(XCompMask), 1:length(XCompMask), XCompMask);
1104        tempGroundX( logical(XCompMask) ) = tempGroundX( logical(XCompMask) )./normPara( GroundMask );
1105        tempGroundX( sum(tempGroundX,2) == 0,:) = [];
1106        tempGroundZ = sparse(1:length(ZCompMask), 1:length(ZCompMask), ZCompMask );
1107        tempGroundZ( logical(ZCompMask) ) = tempGroundZ( logical(ZCompMask) )./normPara( GroundMask );
1108        tempGroundZ( sum(tempGroundZ,2) == 0,:)= [];
1109   
1110        A = [   sparse(1:length(NewVarM),1:length(NewVarM),1./exp(abs(NewVarM)/BandWith)) * NewPosiMPPCP(:, NonVertPtr);...
1111            sparse(1:length(NewCoPEstDepth), 1:length(NewCoPEstDepth), NewCoPEstDepth * Center) * NewCoPM(:, NonVertPtr);...
1112            sparse(1:length(NewEstDepHoriStick), 1:length(NewEstDepHoriStick), NewEstDepHoriStick.*NewWeightHoriNeighborStitch) * NewHoriStickM(:, NonVertPtr);...
1113            sparse(1:length(NewEstDepVertStick), 1:length(NewEstDepVertStick), NewEstDepVertStick.*NewWeightVertNeighborStitch) * NewVertStickM(:, NonVertPtr);...
1114            tempGroundX;...
1115            tempGroundZ...
1116            %        +10*norm( ( Para( logical(XCompMask) ))./...
1117%                            normPara( GroundMask )', 1)...
1118%                 +10*norm( ( Para( logical(ZCompMask)))./...
1119%                            normPara( GroundMask )', 1) ...
1120            ];
1121         
1122    %whos A
1123        b = [   1./exp(abs(NewVarM)/BandWith); ...
1124                 -Center*NewCoPMBound.*NewCoPEstDepth; ...
1125                 -NewHoriStickMBound.*NewEstDepHoriStick.*NewWeightHoriNeighborStitch;...
1126                 -NewVertStickMBound.*NewEstDepVertStick.*NewWeightVertNeighborStitch;...
1127                 sparse(size(tempGroundX,1),1);...
1128                 sparse(size(tempGroundZ,1),1)...
1129                ];
1130     
1131        temp = YCompMask;
1132        temp(logical(YCompMask)) = YNoComingBack;
1133        temp = sparse(1:length(temp), 1:length(temp), temp);
1134        temp( sum(temp,2) ==0,:) = [];
1135        S = [   temp;...
1136                NewRayAllM(:,NonVertPtr);...
1137                -NewRayAllM(:,NonVertPtr);...
1138                ];
1139        inq = [ sparse(size(temp,1), 1);...
1140                 - 1/ClosestDist*ones(size(NewRayAllM,1),1);...
1141                 1/FarestDist*ones(size(NewRayAllM,1),1);...
1142                ];
1143
1144        Para.ClosestDist = ClosestDist;
1145        Para.FarestDist = FarestDist;
1146
1147        % build up ptry and ptrz adapt fot NonVertPtr
1148        Para.ptry = zeros(size(NewRayAllM,2),1);
1149        Para.ptry(2:3:size(NewRayAllM,2)) = 1;
1150        Para.ptry = logical(Para.ptry(NonVertPtr));
1151        Para.ptrz = zeros(size(NewRayAllM,2),1);
1152        Para.ptrz(3:3:size(NewRayAllM,2)) = 1;
1153        Para.ptrz = logical(Para.ptrz(NonVertPtr));
1154       
1155        Para.Dist_Start = size(temp,1)+1;
1156
1157%       xsedumi = sdpvar( size(A,2), 1);
1158%       opt = sdpsettings('solver','sedumi','cachesolvers',1, 'verbose', 0);
1159%       F = set(S*xsedumi+inq<=0);
1160%       fprintf('Solving second step using Sedumi in A,b,S,inq form');
1161%       sol = solvesdp(F, norm(A*xsedumi-b, 1),opt);
1162%       Para = double(xsedumi);
1163%   yalmip('clear');
1164
1165%    fprintf([ '    Starting new solver at: ' num2str( toc(inferenceTime) ) '\n ']);
1166        [x_ashIterator, alfa, status] = SigmoidLogBarrierSolver(Para, [], [], [], '', [], [], solverVerboseLevel);
1167        if any(S*x_ashIterator+inq > 0 )
1168                disp('Inequality not satisfied');
1169                max( S*x_ashIterator+inq)
1170        elseif status==2
1171                fprintf([' Success with alfa=' num2str(alfa)]);
1172        end
1173%       fprintf([ '    Finished new solver at: ' num2str( toc(inferenceTime) ) '\n ' ]);
1174
1175        Para = x_ashIterator;
1176
1177
1178     tempPara = zeros(3*NuSubSupSize,1);
1179     tempPara(NonVertPtr) = Para;
1180     PlanePara(SubSupPtr) = tempPara;
1181     
1182       
1183        end     
1184
1185
1186    end
1187  end
1188
1189  %pause
1190  PlanePara = reshape(PlanePara,3,[]);
1191  FitDepth = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
1192  FitDepth(~maskSkyEroded) = (1./sum(PlanePara(:,Sup2Para(SupEpand(~maskSkyEroded))).*Ray(:,~maskSkyEroded),1))';
1193  FitDepth = reshape(FitDepth,Default.VertYNuDepth,[]);
1194 
1195  % ==========Storage ==============
1196  if Default.Flag.AfterInferenceStorage
1197     save([ Default.ScratchFolder '/' strrep( Default.filename{1},'.jpg','') '_AInfnew.mat' ], 'FitDepth', 'depthMap', ...
1198            'Sup', 'SupOri', 'MedSup', 'RayOri','Ray','SupNeighborTable','maskSky','maskG','MultiScaleSupTable','WeiM','VarMapRaw');
1199  end
1200  % ===============================
1201  %sum(isnan(FitDepth(:)))
1202  [Position3DFited] = im_cr2w_cr(FitDepth,permute(Ray,[2 3 1]));
1203  Position3DFited(3,:) = -Position3DFited(3,:);
1204  Position3DFited = permute(Position3DFited,[2 3 1]);
1205  RR =permute(Ray,[2 3 1]);
1206  temp = RR(:,:,1:2)./repmat(RR(:,:,3),[1 1 2]);
1207  PositionTex = permute(temp./repmat(cat(3,Default.a_default,Default.b_default),...
1208                [Default.VertYNuDepth Default.HoriXNuDepth 1])...
1209                +repmat(cat(3,Default.Ox_default,Default.Oy_default),...
1210                [Default.VertYNuDepth Default.HoriXNuDepth 1]),[3 1 2]);
1211  PositionTex = permute(PositionTex,[2 3 1]);
1212
1213
1214        if true % Min changed back to true from Aug 14th
1215  fprintf(['     ' num2str( toc(inferenceTime) ) '\n     : Writing WRL.']);
1216  WrlFacestHroiReduce(Position3DFited, PositionTex, SupOri, Default.filename{1}, Default.filename{1}, ...
1217      Default.OutPutFolder, GridFlag, 0);
1218 
1219  system(['gzip -9 -c ' Default.OutPutFolder Default.filename{1} '.wrl > ' ...
1220         Default.OutPutFolder Default.filename{1} '.wrl.gz']);
1221  %system(['cp '  Default.OutPutFolder Default.filename{1} '.wrl.gz ' ...
1222  %        Default.OutPutFolder Default.filename{1} '.wrl']);
1223  copyfile([Default.OutPutFolder Default.filename{1} '.wrl.gz'], ...
1224         [Default.OutPutFolder Default.filename{1} '.wrl'],'f');
1225  delete([Default.OutPutFolder Default.filename{1} '.wrl.gz']);
1226        end
1227    % Trial for segmentaion of the plane parameters
1228    if PlaneParaSegmentationFlag
1229       TempSup = Sup;%imresize(Sup,[330 610]);
1230       TempSup(TempSup==0) = max(TempSup(:))+1;
1231       TempSup2Para = Sup2Para;
1232       TempSup2Para(end+1) = size(PlanePara,2);
1233       TempPlanePara = PlanePara;
1234       TempPlanePara(:,end+1) = 0;
1235       PlaneParaPics = TempPlanePara(:,TempSup2Para( TempSup));
1236       PlaneParaPics = PlaneParaPics./repmat( 2*max(abs(PlaneParaPics),[],2), 1, size(PlaneParaPics,2));
1237       PlaneParaPics2 = permute( reshape( PlaneParaPics, 3, 55, []), [2 3 1])+0.5;
1238       MergedPlaneParaPics = segmentImgOpt( Default.sigm*scale, Default.k*scale, Default.minp*10*scale, PlaneParaPics, '', 0) + 1;
1239       if Default.Flag.DisplayFlag
1240          figure(400);
1241          imaegsc( PlaneParaPics);
1242          imaegsc( MergedPlaneParaPics);
1243       end
1244       save('/afs/cs/group/reconstruction3d/scratch/testE/PlaneParaPics2.mat','PlaneParaPics2','MergedPlaneParaPics');
1245    end
1246% save depth Map ++++++++++++++++
1247%       depthMap = FitDepth;
1248%       system(['mkdir ' Default.ScratchDataFolder '/VNonSupport_' DepthFolder '/']);
1249%       save([Default.ScratchDataFolder '/VNonSupport_' DepthFolder '/' depthfile '.mat'],'depthMap');
1250% =============================
1251% return;
Note: See TracBrowser for help on using the repository browser.