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

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

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