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

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

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