source: proiecte/pmake3d/make3d_original/Make3dSingleImageStanford_version0.1/LearningCode/Debug/OldVersion/ReportPlaneParaMRF_Decompv2_old.m @ 37

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

Added original make3d
File size: 40.4 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_Decompv2(Default, step, DepthFolder,...
40          Sup,MedSup,depthMap,VarMap,RayOri, Ray,MedRay,maskSky,maskG,Algo,k, CornerList, OccluList,...
41          MultiScaleSupTable, StraightLineTable, HBrokeBook, VBrokeBook,previoslyStored,...
42          baseline);
43% This function runs the RMF over the plane parameter of each superpixels
44
45if nargin <20
46   baseline = 0;
47end
48
49% initialize parameters
50NOYALMIP = 1;
51Dual = false;
52displayFlag = false;
53RenderVrmlFlag = true;
54StickHori = 5;%0.1; % sticking power in horizontal direction
55StickVert = 10;     % sticking power in vertical direction
56Center = 2; % Co-Planar weight at the Center of each superpixel
57HoriConf = 1; % set the confidant of the learned depth at the middle in Horizontal direction of the image
58VertConf = 0.01; % set the confidant of the learned depth at the top of the image
59BandWith = 1; % Nov29 1 Nov30 0.1 check result change to 0.1 12/1 0.1 lost detail
60mapVert = linspace(VertConf,1,Default.VertYNuDepth); % modeling the gravity prior
61mapHori = [linspace(HoriConf,1,round(Default.HoriXNuDepth/2)) fliplr(linspace(HoriConf,1,Default.HoriXNuDepth-round(Default.HoriXNuDepth/2)))];
62
63% ========set the range of depth that our model in
64ClosestDist = 1;
65% set the FarestDist to very 5 times to median depth
66FarestDist = 1.5*median(depthMap(:)); % tried on university % nogood effect but keep it since usually it the rangelike this   % change to 1.5 for church
67% ================================================
68
69ceiling = 0*Default.VertYNuDepth; % set the position of the ceiling, related to No plane coming back constrain % changed for newchurch
70Name{1} = 'FraWOPri';
71Name{2} = 'FraCoP';
72if isempty(MultiScaleSupTable)
73   Name{3} = 'Var_FraStickCoP';
74else
75   Name{3} = 'Var_FraStickCoPSTasCoP';
76end
77if ~isempty(MultiScaleSupTable)
78   MultiScaleFlag = true;
79   WeiV = 2*ones(1,size(MultiScaleSupTable,2)-1);
80else
81   MultiScaleFlag = false;
82   WeiV = 1;
83end
84WeiV(1,1:2:end) = 6; % emphasize the middle scale three times smaller than large scale
85WeiV =WeiV./sum(WeiV);% normalize if pair of superpixels have same index in all the scale, their weight will be 10
86ShiftStick = -.1;  % between -1 and 0, more means more smoothing.
87ShiftCoP = -.5;  % between -1 and 0, more means more smoothing.
88gravity =true; % if true, apply the HoriConf and VertConf linear scale weight
89CoPST = true; % if true, apply the Straight line prior as the Co-Planar constrain
90ConerImprove = false;
91FractionalDepthError = true;
92
93
94% get rid of the error point and sky point in the depthMap
95% set every depth bigger than FarestDistmeter to FarestDistmeters
96%CleanedDepthMap = (depthMapif ~previoslyStored >80).*medfilt2(depthMap,[4 4])+(depthMap<=80).*depthMap;
97CleanedDepthMap = depthMap;
98%CleanedDepthMap(depthMap>FarestDist) = FarestDist; % don't clean the point >80 sometimes it occlusion
99%disp('Nu of depthMap>FarestDist');
100%sum(sum(depthMap>FarestDist))
101CleanedDepthMap(depthMap>FarestDist) = NaN; % don't clean the point >80 sometimes it occlusion
102Posi3D = im_cr2w_cr(CleanedDepthMap,permute(Ray,[2 3 1]));
103
104if ~previoslyStored
105
106   NewMap = [rand(max(Sup(:)),3); [0 0 0]];
107   % Clean the Sup near sky
108   maskSky = Sup == 0;
109   maskSkyEroded = imerode(maskSky, strel('disk', 4) );
110   SupEpand = ExpandSup2Sky(Sup,maskSkyEroded);
111   NuPatch = Default.HoriXNuDepth*Default.VertYNuDepth-sum(maskSky(:));
112
113   NuSup = setdiff(unique(Sup)',0);
114   NuSup = sort(NuSup);
115   NuSupSize = size(NuSup,2);
116
117   % Sup index and planeParameter index inverse map
118   Sup2Para = sparse(1,max(Sup(:)));
119   Sup2Para(NuSup) = 1:NuSupSize;
120
121   % constructinf the Straight line prior matrix Will be add in the CoPlane matrix
122   NuLine = size(StraightLineTable,2);
123   CoPSTList = [];
124
125   for i = 1:NuLine
126       L = StraightLineTable{i};
127       X = L(1:(end-1))';
128       Y = L(2:end)';
129       if isempty(X)
130          continue;
131       end
132       for j = 1:size(X,1)
133           if X(j)~=Y(j)
134              CoPSTList = [CoPSTList; X(j) Y(j)];
135           end
136       end
137   end
138end
139
140% Generate the Matrix for MRF
141 % ===========================================================================================================================================
142groundThreshold = cos([ zeros(1, Default.VertYNuDepth - ceil(Default.VertYNuDepth/2)+10) linspace(0,15,ceil(Default.VertYNuDepth/2)-10)]*pi/180); 
143  %  v1 15 v2 20 too big v3 20 to ensure non misclassified as ground.
144%  verticalThreshold = cos(linspace(5,55,Default.VertYNuDepth)*pi/180); % give a vector of size 55 in top to down :
145  verticalThreshold = cos([ 5*ones(1,Default.VertYNuDepth - ceil(Default.VertYNuDepth/2)) linspace(5,55,ceil(Default.VertYNuDepth/2))]*pi/180); % give a vector of size 55 in top to down :
146  % 50 means suface norm away from y axis more than 50 degree
147 % ===========================================================================================================================================
148
149PosiM = sparse(0,0);
150VarM = sparse(0,0);
151RayMd = sparse(0,0);
152RayAllOriM = sparse(0,0);
153RayAllM = sparse(0,0);
154RayMtilt = sparse(0,0);
155RayMCent = sparse(0,0);
156DepthInverseMCent = [];
157DepthInverseM = [];
158YPointer = [];
159YPosition = [];
160beta = [];
161EmptyIndex = [];
162for i = NuSup
163%    mask = Sup ==i;
164    mask = SupEpand ==i; % include the Ray that will be use to expand the NonSky
165    RayAllOriM = blkdiag( RayAllOriM, RayOri(:,mask)');
166    RayAllM = blkdiag( RayAllM, Ray(:,mask)');
167    mask = Sup ==i; % Not include the Ray that will be use to expand the NonSky   
168    [yt x] = find(mask);
169    CenterX = round(median(x));
170    CenterY = round(median(yt));
171    YPointer = [YPointer; CenterY >= ceiling];
172    YPosition = [YPosition; CenterY];
173    mask(isnan(CleanedDepthMap)) = false;
174    SupNuPatch(i) = sum(mask(:));
175%    if sum(mask(:)) < 5
176%       EmptyIndex = [EmptyIndex; i];
177%       mask(:) = false;
178%    end
179    % find center point
180    [yt x] = find(mask);
181    CenterX = round(median(x));
182    CenterY = round(median(yt));
183 
184  if ~all(mask(:)==0)
185    if gravity
186      Pa2CenterRatio = median(CleanedDepthMap(mask))./CleanedDepthMap(mask);
187      if sum(mask(:)) > 0
188         RayMtilt = blkdiag(RayMtilt, ...
189             ( Pa2CenterRatio(:,[1 1 1]).*repmat(RayOri(:,CenterY,CenterX)',[ SupNuPatch(i) 1])- RayOri(:,mask)'));
190      else
191         RayMtilt = blkdiag(RayMtilt, RayOri(:,mask)');
192      end
193      RayMCent = blkdiag(RayMCent, RayOri(:,CenterY,CenterX)'*SupNuPatch(i)*mapVert(CenterY)*mapHori(CenterX));
194      PosiM = blkdiag(PosiM,Posi3D(:,mask)');%.*repmat( mapVert(yt)',[1 3]).*repmat( mapHori(x)',[1 3]));
195      VarM = [VarM; VarMap(mask).*(mapVert(yt)').*( mapHori(x)')];
196      RayMd = blkdiag(RayMd,RayOri(:,mask)'.*repmat( mapVert(yt)',[1 3]).*repmat( mapHori(x)',[1 3]));
197      DepthInverseMCent = [DepthInverseMCent; 1./median(CleanedDepthMap(mask)).*SupNuPatch(i)* mapVert(CenterY)'*mapHori(CenterX)];
198      DepthInverseM = [DepthInverseM; 1./CleanedDepthMap(mask).* mapVert(yt)'.*mapHori(x)'];
199    else
200      Pa2CenterRatio = CleanedDepthMap(CenterY,CenterX)./CleanedDepthMap(mask);
201      if sum(mask(:)) > 0
202         RayMtilt = blkdiag(RayMtilt, ...
203             ( Pa2CenterRatio(:,[1 1 1]).*repmat(RayOri(:,CenterY,CenterX)',[ SupNuPatch(i) 1])- RayOri(:,mask)'));
204      else
205         RayMtilt = blkdiag(RayMtilt, RayOri(:,mask)');
206      end
207      RayMCent = blkdiag(RayMCent, RayOri(:,CenterY,CenterX)'*SupNuPatch(i));
208      PosiM = blkdiag(PosiM,Posi3D(:,mask)');
209      VarM = [VarM; VarMap(mask)];
210      RayMd = blkdiag(RayMd,RayOri(:,mask)');
211      DepthInverseMCent = [DepthInverseMCent; 1./median(CleanedDepthMap(mask)).*SupNuPatch(i)];
212      DepthInverseM = [DepthInverseM; 1./CleanedDepthMap(mask)];
213    end
214  else
215     RayMtilt = blkdiag(RayMtilt, RayOri(:,mask)');
216     RayMCent = blkdiag(RayMCent, RayOri(:,mask)');
217     PosiM = blkdiag(PosiM, Posi3D(:,mask)');
218     VarM = [VarM; VarMap(mask)];
219     RayMd = blkdiag(RayMd, RayOri(:,mask)');
220  end
221end
222YPointer(YPointer==0) = -1;
223
224% buliding CoPlane Matrix=========================================================================
225BounaryPHori = conv2(Sup,[1 -1],'same') ~=0;
226BounaryPHori(:,end) = 0;
227BounaryPVert = conv2(Sup,[1; -1],'same') ~=0;
228BounaryPVert(end,:) = 0;
229ClosestNList = [ Sup(find(BounaryPHori==1)) Sup(find(BounaryPHori==1)+Default.VertYNuDepth);...
230                 Sup(find(BounaryPVert==1)) Sup(find(BounaryPVert==1)+1)];
231ClosestNList = sort(ClosestNList,2);
232ClosestNList = unique(ClosestNList,'rows');
233ClosestNList(ClosestNList(:,1) == 0,:) = [];
234BoundaryAll = BounaryPHori + BounaryPHori(:,[1 1:(end-1)])...
235             +BounaryPVert + BounaryPVert([1 1:(end-1)],:);
236BoundaryAll([1 end],:) = 1;
237BoundaryAll(:,[1 end]) = 1;
238NuSTList = 0;
239if CoPST
240   ClosestNList = [ClosestNList; CoPSTList];
241   NuSTList = size(CoPSTList,1);
242end
243NuNei = size(ClosestNList,1);
244CoPM1 = sparse(0,3*NuSupSize);
245CoPM2 = sparse(0,3*NuSupSize);
246CoPEstDepth = sparse(0,0);
247CoPNorM = [];
248WeiCoP = [];
249for i = 1: NuNei
250%  if ~CornerList(i)
251    mask = Sup == ClosestNList(i,1);
252    SizeMaskAll = sum(mask(:));
253    [y x] = find(mask);
254    CenterX = round(median(x));
255    CenterY = round(median(y));
256    y = find(mask(:,CenterX));
257    if ~isempty(y)
258       CenterY = round(median(y));
259    end
260%    CenterX = round(median(x));
261%    CenterY = round(median(yt));
262   
263    temp1 = sparse(1, 3*NuSupSize);
264    temp2 = sparse(1, 3*NuSupSize);
265    temp1(:,(Sup2Para(ClosestNList(i,1))*3-2): Sup2Para(ClosestNList(i,1))*3) = Ray(:,CenterY,CenterX)';
266    temp2(:,(Sup2Para(ClosestNList(i,2))*3-2): Sup2Para(ClosestNList(i,2))*3) = Ray(:,CenterY,CenterX)';
267    %NuNei-NuSTList
268%    i
269    if i < NuNei-NuSTList % only immediate connecting superpixels are weighted using MultiScaleSup
270%       wei = WeiV*10;%*(MultiScaleSupTable(Sup2Para(ClosestNList(i,1)),2:end) == MultiScaleSupTable(Sup2Para(ClosestNList(i,2)),2:end))'; 
271       if MultiScaleFlag
272          vector = (MultiScaleSupTable(Sup2Para(ClosestNList(i,1)),2:end) == MultiScaleSupTable(Sup2Para(ClosestNList(i,2)),2:end));
273          expV = exp(-10*(WeiV*vector' + ShiftCoP) );
274          wei = 1/(1+expV);
275       else
276          wei = 1;
277       end
278    else
279       wei = 1;
280    end
281    CoPM1 = [CoPM1; temp1*wei];
282    CoPM2 = [CoPM2; temp2*wei];
283    tempWeiCoP = [SizeMaskAll];
284    CoPEstDepth = [CoPEstDepth; max(median(CleanedDepthMap(mask)),ClosestDist)];
285   
286    mask = Sup == ClosestNList(i,2);
287    SizeMaskAll = sum(mask(:));
288    [y x] = find(mask);
289    CenterX = round(median(x));
290    CenterY = round(median(y));
291    y = find(mask(:,CenterX));
292    if ~isempty(y)
293       CenterY = round(median(y));
294    end
295%    [yt x] = find(mask);
296%    CenterX = round(median(x));
297%    CenterY = round(median(yt));
298
299    temp1 = sparse(1, 3*NuSupSize);
300    temp2 = sparse(1, 3*NuSupSize);
301    temp1(:,(Sup2Para(ClosestNList(i,1))*3-2): Sup2Para(ClosestNList(i,1))*3) = Ray(:,CenterY,CenterX)';
302    temp2(:,(Sup2Para(ClosestNList(i,2))*3-2): Sup2Para(ClosestNList(i,2))*3) = Ray(:,CenterY,CenterX)';
303    CoPM1 = [CoPM1; temp1*wei];
304    CoPM2 = [CoPM2; temp2*wei];
305    tempWeiCoP = [tempWeiCoP; SizeMaskAll];
306    WeiCoP = [WeiCoP; tempWeiCoP];
307    CoPEstDepth = [CoPEstDepth; max(median(CleanedDepthMap(mask)),ClosestDist)];
308%  end
309end%=========================================================================================================
310
311% find the boundary point that might need to be stick ot each other==========================================
312HoriStickM_i = sparse(0,3*NuSupSize);
313HoriStickM_j = sparse(0,3*NuSupSize);
314HoriStickPointInd = [];
315EstDepHoriStick = [];
316
317SupPixelNeighborList = sparse(NuSupSize, NuSupSize);
318MAX_POINTS_STITCH = inf;    % the actual code will be modified in another copy of the file
319
320for i = find(BounaryPHori==1)'
321    j = i+Default.VertYNuDepth;
322    if Sup(i) == 0 || Sup(j) == 0
323       continue;
324    end
325    if SupPixelNeighborList(Sup(i),Sup(j)) > MAX_POINTS_STITCH      % I.e., increase weight, and do not add to the matrix
326        SupPixelNeighborList(Sup(i),Sup(j)) = SupPixelNeighborList(Sup(i),Sup(j)) + 1;
327        WeightHoriNeighborStitch = SupPixelNeighborList(Sup(i),Sup(j)) / MAX_POINTS_STITCH;     % weight will be distributed between MAX_POINTS_STITCH
328        continue;
329    end
330       
331%  if ~OccluList(sum( ClosestNList == repmat(sort([Sup(i) Sup(j)]), [NuNei  1]),2) == 2)
332%  size(OccluList)
333%  if ~any(sum( OccluList == repmat(sort([Sup(i) Sup(j)]), [size(OccluList,1)  1]),2) == 2)
334    Target(1) = Sup2Para(Sup(i));
335    Target(2) = Sup2Para(Sup(j));
336    rayBoundary(:,1) =  Ray(:,i);
337    rayBoundary(:,2) =  Ray(:,i);
338%    betaTemp = StickHori;%*(DistStickLengthNormWei.^2)*beta(Target(I));
339%    betaTemp = StickHori*WeiV;%*(MultiScaleSupTable(Sup2Para(Sup(i)),2:end) == MultiScaleSupTable(Sup2Para(Sup(j)),2:end))';%*(DistStickLengthNormWei.^2)*beta(Target(I));
340    if MultiScaleFlag
341          vector = (MultiScaleSupTable(Sup2Para(Sup(i)),2:end) == MultiScaleSupTable(Sup2Para(Sup(j)),2:end));
342          expV = exp(-10*(WeiV*vector' + ShiftStick) );
343       betaTemp = StickHori*(0.5+1/(1+expV)); %*(DistStickLengthNormWei.^2)*beta(Target(I));
344       % therr should always be sticking (know don't care about occlusion)
345    else
346       betaTemp = StickHori;
347    end
348    temp = sparse(3,NuSupSize);
349    temp(:,Target(1)) = rayBoundary(:,1);
350    HoriStickM_i = [HoriStickM_i; betaTemp*temp(:)'];
351    temp = sparse(3,NuSupSize);
352    temp(:,Target(2)) = rayBoundary(:,2);
353    HoriStickM_j = [HoriStickM_j; betaTemp*temp(:)'];
354    EstDepHoriStick = [EstDepHoriStick; sqrt(max(CleanedDepthMap(i),ClosestDist)*max(CleanedDepthMap(j),ClosestDist))];
355    HoriStickPointInd = [HoriStickPointInd i ];
356
357end
358
359
360VertStickM_i = sparse(0,3*NuSupSize);
361VertStickM_j = sparse(0,3*NuSupSize);
362VertStickPointInd = [];
363EstDepVertStick = [];
364for i = find(BounaryPVert==1)'
365    j = i+1;
366    if Sup(i) == 0 || Sup(j) == 0
367       continue;
368    end
369%  if ~OccluList(sum( ClosestNList == repmat(sort([Sup(i) Sup(j)]), [NuNei  1]),2) == 2)
370%  if ~any(sum( OccluList == repmat(sort([Sup(i) Sup(j)]), [size(OccluList,1)  1]),2) == 2)
371    Target(1) = Sup2Para(Sup(i));
372    Target(2) = Sup2Para(Sup(j));
373    rayBoundary(:,1) =  Ray(:,i);
374    rayBoundary(:,2) =  Ray(:,i);
375%    betaTemp = StickVert;%*(DistStickLengthNormWei.^2)*beta(Target(I));
376    if MultiScaleFlag
377       vector = (MultiScaleSupTable(Sup2Para(Sup(i)),2:end) == MultiScaleSupTable(Sup2Para(Sup(j)),2:end));
378       expV = exp(-10*(WeiV*vector' + ShiftStick) );
379       betaTemp = StickVert*(0.5+1/(1+expV));
380       % therr should always be sticking (know don't care about occlusion)
381    else
382       betaTemp = StickVert;
383    end
384    temp = sparse(3,NuSupSize);
385    temp(:,Target(1)) = rayBoundary(:,1);
386    VertStickM_i = [VertStickM_i; betaTemp*temp(:)'];
387    temp = sparse(3,NuSupSize);
388    temp(:,Target(2)) = rayBoundary(:,2);
389    VertStickM_j = [VertStickM_j; betaTemp*temp(:)'];
390    EstDepVertStick = [EstDepVertStick; sqrt(max(CleanedDepthMap(i),ClosestDist)*max(CleanedDepthMap(j),ClosestDist))];
391    VertStickPointInd = [VertStickPointInd i ];
392%  else
393%    disp('Occlu');
394%  end
395end
396% ======================================Finish building up matrix=====================hard work======================
397
398
399
400% ================================================================================================================
401depthfile = strrep(Default.filename{k},'img','depth_learned'); %
402
403
404% Start Decompose the image align with superpixels ==================================================================================
405% define the decomposition in X direction only
406
407% ============== parameters for the decomposition problem
408XNuDecompose = 1;
409YNuDecompose = 1;
410% ============ parameters for the decomposition problem
411
412
413 
414TotalRectX = 2*XNuDecompose-1;
415TotalRectY= 2*YNuDecompose-1;
416PlanePara = NaN*ones(3*NuSupSize,1); % setup the lookuptable for the solved plane parameter
417
418opt = sdpsettings('solver','sedumi','cachesolvers',1);
419%     opt = sdpsettings('solver','lpsolve','cachesolvers',1);
420%    opt = sdpsettings('solver','glpk','cachesolvers',1);   
421
422for k = 0:(TotalRectX-1)
423  l = rem(k*2,(TotalRectX));
424  RangeX = (1+ceil(Default.HoriXNuDepth/XNuDecompose)*l/2):...
425            min((1+ceil(Default.HoriXNuDepth/XNuDecompose)*(l/2+1)),Default.HoriXNuDepth);
426  RangeX = ceil(RangeX);
427  for q = 0:(TotalRectY-1)
428    l = rem(q*2,(TotalRectY));
429    RangeY = (1+ceil(Default.VertYNuDepth/YNuDecompose)*l/2):...
430              min((1+ceil(Default.VertYNuDepth/YNuDecompose)*(l/2+1)),Default.VertYNuDepth);
431    RangeY = ceil(RangeY);
432    mask = zeros(size(Sup));
433    mask(RangeY,RangeX) = 1;
434    mask =logical(mask);
435    SubSup = sort(setdiff(reshape( Sup(RangeY,RangeX),[],1),0))';
436    BoundarySup = [];
437    for m = SubSup
438        maskList = ClosestNList(:,1) == m;
439        BoundarySup = [ BoundarySup ClosestNList(maskList,2)'];
440    end
441    BoundarySup = unique(setdiff(BoundarySup,[0 SubSup] ));
442    % chech if BoundarySup non-NaN in PlanePara
443    checkNoNNaN = ~isnan(PlanePara(Sup2Para(BoundarySup)*3));
444    BoundarySup = BoundarySup(checkNoNNaN);
445    TotalSup = sort([SubSup BoundarySup]);
446
447    TotalSupPtr = [ Sup2Para(TotalSup)*3-2;...
448                    Sup2Para(TotalSup)*3-1;...
449                    Sup2Para(TotalSup)*3];
450    TotalSupPtr = TotalSupPtr(:);
451    BoundarySupPtr = [ Sup2Para(BoundarySup)*3-2;...
452                       Sup2Para(BoundarySup)*3-1;...
453                       Sup2Para(BoundarySup)*3];
454    BoundarySupPtr =BoundarySupPtr(:);
455    NuSubSupSize = size(TotalSup,2);
456    TotalSup2Para = sparse(1,max(TotalSup));
457    TotalSup2Para(TotalSup) = 1:NuSubSupSize;
458    BoundarySupPtrSub = [ TotalSup2Para(BoundarySup)*3-2;...
459                          TotalSup2Para(BoundarySup)*3-1;...
460                          TotalSup2Para(BoundarySup)*3];
461    BoundarySupPtrSub =BoundarySupPtrSub(:);
462   
463    % clearn RayAllM PosiM CoPM1 HoriStickM_i VertStickM_i
464    NewRayAllM = RayAllM(:,TotalSupPtr);
465    tar = sum(NewRayAllM ~= 0,2) ==3;
466    NewRayAllM = NewRayAllM(tar,:);
467    NewPosiM = PosiM(:,TotalSupPtr);
468    tar = sum(NewPosiM ~= 0,2) ==3;
469    NewPosiM = NewPosiM(tar,:);
470    NewVarM = VarM(tar);
471    NewCoPM = CoPM1(:,TotalSupPtr) - CoPM2(:,TotalSupPtr);
472    tar = sum(NewCoPM ~= 0,2) ==6;
473    NewCoPM = NewCoPM(tar,:);
474    NewCoPEstDepth = CoPEstDepth(tar);   
475    NewHoriStickM = HoriStickM_i(:,TotalSupPtr)-HoriStickM_j(:,TotalSupPtr);
476    tar = sum(NewHoriStickM ~= 0,2) ==6;
477    NewHoriStickM = NewHoriStickM(tar,:);
478    NewEstDepHoriStick = EstDepHoriStick(tar);
479    NewVertStickM = VertStickM_i(:,TotalSupPtr)-VertStickM_j(:,TotalSupPtr);
480    tar = sum(NewVertStickM ~= 0,2) ==6;
481    NewVertStickM = NewVertStickM(tar,:);
482    NewEstDepVertStick = EstDepVertStick(tar);
483   
484    for i = step
485        ParaPPCP = sdpvar(3*NuSubSupSize,1);
486        F = set(ParaPPCP(3*(1:NuSubSupSize)-1).*YPointer(Sup2Para(TotalSup))<=0)...
487            +set(NewRayAllM*ParaPPCP <=1/ClosestDist)...
488            +set(NewRayAllM*ParaPPCP >=1/FarestDist)...
489            +set(ParaPPCP(BoundarySupPtrSub) == PlanePara(BoundarySupPtr) ); % Special constrain for decomp fix the solved neighbor plane parameter       
490%         +set(RayAllOriM(:,TotalSupPtr)*ParaPPCP >=1/FarestDist)...
491%         +set(RayAllOriM(:,TotalSupPtr)*ParaPPCP <=1/ClosestDist);
492% First fit the plane to find the estimated plane parameters
493% If PosiM contain NaN data the corresponding Plane Parameter will also be NaN
494    if i == 1 % W/O  priors
495%    solvesdp(F,norm( (PosiM*ParaPPCP-ones(size(PosiM,1),1))./(abs(VarM)),1)...
496       solvesdp(F,norm( (PosiM*ParaPPCP-ones(size(PosiM,1),1))./exp(abs(VarM)/BandWith),1)...
497             , opt);
498    elseif i ==2 % Coner
499       solvesdp(F,norm( (PosiM*ParaPPCP-ones(size(PosiM,1),1))./exp(abs(VarM)/BandWith),1)...
500             +Center*norm(((CoPM1 - CoPM2)*ParaPPCP).*CoPEstDepth, 1)...
501             , opt);
502    else % all the priors
503%    sum(sum(isnan(PosiM)))
504       disp('Whole MRF')
505%     solvesdp(F,norm( (PosiM*ParaPPCP-ones(size(PosiM,1),1))./exp(abs(VarM)/BandWith),1)...
506%              +Center*norm(((CoPM1 - CoPM2)*ParaPPCP).*CoPEstDepth, 1)...
507%              +norm(((HoriStickM_i-HoriStickM_j)*ParaPPCP).*EstDepHoriStick,1)...
508%              +norm(((VertStickM_i-VertStickM_j)*ParaPPCP).*EstDepVertStick,1)...
509%              , opt);
510   
511
512% =========  Debugging the code for numerical accuracy  ==========
513
514
515
516% ================================================================
517
518 
519       if NOYALMIP
520          sol = solvesdp(F, norm( (NewPosiM*ParaPPCP-ones(size(NewPosiM,1),1)) ./ exp(abs(NewVarM)/BandWith),1)...
521                        + Center*norm(((NewCoPM)*ParaPPCP).*NewCoPEstDepth, 1)...
522                        + norm(((NewHoriStickM)*ParaPPCP).*NewEstDepHoriStick,1)...
523                        + norm(((NewVertStickM)*ParaPPCP).*NewEstDepVertStick,1), ...
524                        opt);
525       else
526
527       % ============Using Sedumi directly ==================================
528          if Dual
529          % trial for dual form
530
531
532          else
533            % trial for MS_E212 method: Primal form
534            %  D1 = PosiM;
535            %  D2 = (CoPM1 - CoPM2);
536            %  D3 = (HoriStickM_i-HoriStickM_j);
537            %  D4 = (VertStickM_i-VertStickM_j);
538            %  b = -ones(size(PosiM,1),1);
539 
540            %  C1 = RayAllM;
541            %  C2 = -RayAllM
542            %  clear tempSparse;
543            %  e1 = 1/ClosestDist*ones(size(RayAllM,1),1);
544            %  e2 = -1/ FarestDist*ones(size(RayAllM,1),1);
545
546
547            %  e2 = sparse(3*NuSupSize,1);
548            %  C2 = spdiags(tempSparse,0,3*NuSupSize,3*NuSupSize);
549
550            %  t1 = 1./exp(abs(VarM)/BandWith);
551            %  t2 = CoPEstDepth*Center;
552            %  t3 = EstDepHoriStick;
553            %  t4 = EstDepVertStick;
554
555            sizeX = size(PosiM,2);
556            sizeD1 = size(PosiM,1);
557            sizeD2 = size(CoPM1,1);
558            sizeD3 = size(HoriStickM_i,1);
559            sizeD4 = size(VertStickM_i,1);
560            sizeC1 = size(RayAllM,1);
561            sizeC2 = sizeC1;
562            %   sizeC2 = size(spdiags(tempSparse,0,3*NuSupSize,3*NuSupSize),1);
563 
564 
565            A = [ -2*PosiM ... % x
566                  -speye(sizeD1,sizeD1) ... % V_1
567                  sparse(sizeD1,(sizeD2+sizeD3+sizeD4) );...
568                  ...
569                  -2*(CoPM1 - CoPM2) ... % x
570                  sparse(sizeD2,sizeD1) ...
571                  -spdiags(ones(sizeD2,1),0,sizeD2,sizeD2) ... % V_2
572                  sparse(sizeD2, (sizeD3+sizeD4));...
573                  ...
574                  -2*(HoriStickM_i-HoriStickM_j) ... %x
575                  sparse(sizeD3, (sizeD1+sizeD2) ) ...
576                  -spdiags(ones(sizeD3,1),0,sizeD3,sizeD3) ... % V_3
577                  sparse(sizeD3,sizeD4 );...
578                  ...
579                  -2*(VertStickM_i-VertStickM_j) ... %x
580                  sparse(sizeD4, (sizeD1+sizeD2+sizeD3)) ...
581                  -spdiags(ones(sizeD4,1),0,sizeD4,sizeD4); ... % V_4
582                  ...
583                  RayAllM ...
584                  sparse(sizeC1,(sizeD1+sizeD2+sizeD3+sizeD4) ); ...
585                  ...
586                  -RayAllM ...
587                  sparse(sizeC2,(sizeD1+sizeD2+sizeD3+sizeD4) ); ...
588            ]; 
589
590            b = [ -2*ones(size(PosiM,1),1); ...
591                  sparse(sizeD2+sizeD3+sizeD4,1);...
592                  1/ClosestDist*ones(size(RayAllM,1),1);...
593                  -1/FarestDist*ones(size(RayAllM,1),1);...
594                ];
595            H = [ (1./exp(abs(VarM)/BandWith))'*(PosiM)+...
596                  (CoPEstDepth*Center)'*(CoPM1 - CoPM2)+...
597                  (EstDepHoriStick)'*((HoriStickM_i-HoriStickM_j))+...
598                  (EstDepVertStick)'*((VertStickM_i-VertStickM_j)) ...
599                  ...
600                  (1./exp(abs(VarM)/BandWith))' ...
601                  ...
602                  CoPEstDepth'*Center ...
603                  ...
604                  EstDepHoriStick' ...
605                  ...
606                  EstDepVertStick' ...
607                ];
608            lb = [-Inf*ones(sizeX,1); ...
609                  sparse((sizeD1+sizeD2+sizeD3+sizeD4),1)];
610            tempSparse = sparse(3*NuSupSize,1);
611            tempSparse(3*(1:NuSupSize)-1) = YPointer;
612            ub = Inf*ones(sizeX,1);
613            ub(logical(tempSparse)) = 0;
614            ub = [ub; Inf*ones((sizeD1+sizeD2+sizeD3+sizeD4),1)];
615 
616            %   options = optimset('LargeScale','on');
617            %  K.f = sizeX;
618            %  ParaPPCP = J*sedumi(A,b,H,K);
619            %   linOut = linprog(H',A,b,[],[],lb,ub,[],options);
620            [obj,x,duals,stat] = lp_solve(-H',A,b,-1*ones(size(b)),lb,ub);
621             ParaPPCP = x(1:sizeX);
622            %   opt = sdpsettings('solver','sedumi');
623            %   ParaPPCP = sdpvar(sizeX + sizeD1 +sizeD2+sizeD3+sizeD4);
624            %   F = set(A*ParaPPCP <= b) + set(ParaPPCP>=lb);
625            %   ParaPPCP = J*solvesdp(F,H*ParaPPCP,opt);
626          end
627       end
628    end
629
630    ParaPPCP = double(ParaPPCP);
631%     sum(isnan(ParaPPCP))
632    yalmip('clear');
633    PlanePara(TotalSupPtr) = ParaPPCP;
634
635% %    SepPointMeasureHori = 1./(HoriStickM_i*ParaPPCP)-1./(HoriStickM_j*ParaPPCP);
636% %    SepPointMeasureVert = 1./(VertStickM_i*ParaPPCP)-1./(VertStickM_j*ParaPPCP);
637%     ParaPPCP = reshape(ParaPPCP,3,[]);
638%     %any(any(isnan(ParaPPCP)))
639%     % porject the ray on planes to generate the ProjDepth
640%     FitDepthPPCP = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
641%     FitDepthPPCP(~maskSkyEroded & mask) = (1./sum(ParaPPCP(:,TotalSup2Para(SupEpand(~maskSkyEroded & mask))).*Ray(:,~maskSkyEroded & mask),1))';
642%     FitDepthPPCP = reshape(FitDepthPPCP,Default.VertYNuDepth,[]);
643%     % storage for comparison purpose ======================
644% %    depthMap = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
645% %    depthMap(~maskSkyEroded | mask) = (1./sum(ParaPPCP(:,TotalSup2Para(SupEpand(~maskSkyEroded | mask))).*RayOri(:,~maskSkyEroded | mask),1))';
646% %    depthMap = reshape(depthMap,Default.VertYNuDepth,[]);
647% %    if baseline == 0
648% %       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '/']);
649% %       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '/' depthfile '.mat'],'depthMap');
650% %    elseif baseline == 1
651% %       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline/']);
652% %       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline/' depthfile '.mat'],'depthMap');
653% %    else
654% %       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline2/']);
655% %       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline2/' depthfile '.mat'],'depthMap');
656% %    end
657%   
658%     % =====================================================
659%     [Position3DFitedPPCP] = im_cr2w_cr(FitDepthPPCP,permute(Ray,[2 3 1]));
660%     if true %RenderVrmlFlag && i==3
661%          Position3DFitedPPCP(3,:) = -Position3DFitedPPCP(3,:);
662%          Position3DFitedPPCP = permute(Position3DFitedPPCP,[2 3 1]);
663%          RR =permute(Ray,[2 3 1]);
664%          temp = RR(:,:,1:2)./repmat(RR(:,:,3),[1 1 2]);
665%          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]);
666%          PositionTex = permute(PositionTex,[2 3 1]);
667%          WrlFacest(Position3DFitedPPCP,PositionTex,Sup,Default.filename{1},'./')
668%
669% %        [VrmlName] = vrml_test_faceset_goodSkyBoundary(      Default.filename{k}, Position3DFitedPPCP, FitDepthPPCP, permute(Ray,[2 3 1]), [Name{i} DepthFolder 'ExpVar'], ...
670% %                     [], [], 0, maskSkyEroded, maskG, 1, 0, Default.a_default, Default.b_default, Default.Ox_default, Default.Oy_default);
671% %        system(['gzip -9 -c ' Default.ScratchDataFolder '/vrml/' VrmlName ' > ' Default.ScratchDataFolder '/vrml/' VrmlName '.gz']);
672%        %delete([Default.ScratchDataFolder '/vrml/' VrmlName]);
673%     end
674%    save([Default.ScratchDataFolder '/data/PreDepth/FitDepthPPCP' num2str(k) '.mat'],'FitDepthPPCP','SepPointMeasureHori',...
675%      'SepPointMeasureVert','VertStickPointInd','HoriStickPointInd');
676%save([Default.ScratchDataFolder '/data/All.mat']);
677  end
678  end
679end
680% build the whole image
681    PlanePara = reshape(PlanePara,3,[]);
682    %any(any(isnan(PlanePara)))
683    % porject the ray on planes to generate the ProjDepth
684    FitDepthPPCP = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
685    FitDepthPPCP(~maskSkyEroded) = (1./sum(PlanePara(:,Sup2Para(SupEpand(~maskSkyEroded ))).*Ray(:,~maskSkyEroded ),1))';
686    FitDepthPPCP = reshape(FitDepthPPCP,Default.VertYNuDepth,[]);
687    % storage for comparison purpose ======================
688%    depthMap = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
689%    depthMap(~maskSkyEroded | mask) = (1./sum(PlanePara(:,TotalSup2Para(SupEpand(~maskSkyEroded | mask))).*RayOri(:,~maskSkyEroded | mask),1))';
690%    depthMap = reshape(depthMap,Default.VertYNuDepth,[]);
691%    if baseline == 0
692%       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '/']);
693
694
695
696%       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '/' depthfile '.mat'],'depthMap');
697%    elseif baseline == 1
698%       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline/']);
699%       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline/' depthfile '.mat'],'depthMap');
700%    else
701%       system(['mkdir ' Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline2/']);
702%       save([Default.ScratchDataFolder '/' Name{i} '_' DepthFolder '_baseline2/' depthfile '.mat'],'depthMap');
703%    end
704
705    % =====================================================
706    [Position3DFitedPPCP] = im_cr2w_cr(FitDepthPPCP,permute(Ray,[2 3 1]));
707    if false %RenderVrmlFlag && i==3
708         Position3DFitedPPCP(3,:) = -Position3DFitedPPCP(3,:);
709         Position3DFitedPPCP = permute(Position3DFitedPPCP,[2 3 1]);
710         RR =permute(Ray,[2 3 1]);
711         temp = RR(:,:,1:2)./repmat(RR(:,:,3),[1 1 2]);
712         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]);
713         PositionTex = permute(PositionTex,[2 3 1]);
714         WrlFacestHroiReduce(Position3DFitedPPCP,PositionTex,Sup,Default.filename{1},'./')
715    end
716
717%return;
718%==================Finished for one step MRF==========================================================================================================
719
720NoSecondStep = 1;
721if NoSecondStep
722        return;
723end
724
725% ====================================following are 2nd step MRF to give more visually pleasing result=======================================
726;% generating new PosiMPPCP using the new position
727
728% save([Default.ScratchDataFolder '/data/VertShiftVrml.mat' ] );
729%  groundThreshold = cos(5*pi/180);  % make small range
730%  verticalThreshold = cos(50*pi/180);
731  normPara = norms(PlanePara);
732  normalizedPara = PlanePara ./ repmat( normPara, [3 1]);
733  groundPara = abs(normalizedPara(2,:)) >= groundThreshold(YPosition);
734  groundParaInd = find(groundPara);
735  verticalPara = abs(normalizedPara(2,:)) <= verticalThreshold(YPosition); % change to have different range of vertical thre ================
736  verticalParaInd = find(verticalPara);
737
738  indexVertical = find( verticalPara)*3-1;
739  indexGroundX = find( groundPara)*3-2;
740  indexGroundZ = find( groundPara)*3;
741
742  PosiMPPCP = sparse(0,0);
743  VarM2 = sparse(0,0);
744  %  VertVar = 10^(-2);
745  % forming new supporting matrix using new depth and get rid of the support of the vertical plane
746  for i = NuSup
747      mask = Sup == i;
748      if any(verticalParaInd == Sup2Para(i))
749         mask = logical(zeros(size(Sup)));
750      end
751         PosiMPPCP = blkdiag(PosiMPPCP, Posi3D(:,mask)');
752         VarM2 = [VarM2; VarMap(mask)];         
753  end
754
755% Start Decompose image =========================
756  XNuDecompose = 2;
757  YNuDecompose = 1;
758  TotalRectX = 2*XNuDecompose-1;
759  TotalRectY = 2*YNuDecompose-1;
760  PlanePara = NaN*ones(3*NuSupSize,1); % setup the lookuptable for the solved plane parameter
761
762  opt = sdpsettings('solver','sedumi','cachesolvers',1);
763  %     opt = sdpsettings('solver','lpsolve','cachesolvers',1);
764  %    opt = sdpsettings('solver','glpk','cachesolvers',1);   
765
766  for k = 0:(TotalRectX-1)
767    l = rem(k*2,(TotalRectX));
768    RangeX = (1+ceil(Default.HoriXNuDepth/XNuDecompose)*l/2):...
769              min((1+ceil(Default.HoriXNuDepth/XNuDecompose)*(l/2+1)),Default.HoriXNuDepth);
770    RangeX = ceil(RangeX);
771    for q = 0:(TotalRectY-1)
772      l = rem(q*2,(TotalRectY));
773      RangeY = (1+ceil(Default.VertYNuDepth/YNuDecompose)*l/2):...
774                min((1+ceil(Default.VertYNuDepth/YNuDecompose)*(l/2+1)),Default.VertYNuDepth);
775      RangeY = ceil(RangeY);
776      mask = zeros(size(Sup));
777      mask(RangeY,RangeX) = 1;
778      mask =logical(mask);
779      SubSup = sort(setdiff(reshape( Sup(RangeY,RangeX),[],1),0))';
780      BoundarySup = [];
781      for m = SubSup
782          maskList = ClosestNList(:,1) == m;
783          BoundarySup = [ BoundarySup ClosestNList(maskList,2)'];
784      end
785      BoundarySup = unique(setdiff(BoundarySup,[0 SubSup] ));
786
787      % chech if BoundarySup non-NaN in PlanePara
788      checkNoNNaN = ~isnan(PlanePara(Sup2Para(BoundarySup)*3));
789      BoundarySup = BoundarySup(checkNoNNaN);
790      TotalSup = sort([SubSup BoundarySup]);
791
792      TotalSupPtr = [ Sup2Para(TotalSup)*3-2;...
793                      Sup2Para(TotalSup)*3-1;...
794                      Sup2Para(TotalSup)*3];
795      TotalSupPtr = TotalSupPtr(:);
796      SubSupPtr = [ Sup2Para(SubSup)*3-2;...
797                    Sup2Para(SubSup)*3-1;...
798                    Sup2Para(SubSup)*3];
799      SubSupPtr = SubSupPtr(:);
800      BoundarySupPtr = [ Sup2Para(BoundarySup)*3-2;...
801                         Sup2Para(BoundarySup)*3-1;...
802                         Sup2Para(BoundarySup)*3];
803      BoundarySupPtr =BoundarySupPtr(:);
804      NuSubSupSize = size(TotalSup,2);
805      TotalSup2Para = sparse(1,max(TotalSup));
806      TotalSup2Para(TotalSup) = 1:NuSubSupSize;
807      BoundarySupPtrSub = [ TotalSup2Para(BoundarySup)*3-2;...
808                            TotalSup2Para(BoundarySup)*3-1;...
809                            TotalSup2Para(BoundarySup)*3];
810      BoundarySupPtrSub =BoundarySupPtrSub(:);
811      SubSupPtrSub = [ TotalSup2Para(SubSup)*3-2;...
812                       TotalSup2Para(SubSup)*3-1;...
813                       TotalSup2Para(SubSup)*3];
814      SubSupPtrSub =SubSupPtrSub(:);
815   
816      % clearn RayAllM PosiM CoPM1 HoriStickM_i VertStickM_i
817      NewRayAllM = RayAllM(:,TotalSupPtr);
818      tar = sum(NewRayAllM ~= 0,2) ==3;
819      NewRayAllM = NewRayAllM(tar,:);
820      NewPosiMPPCP = PosiMPPCP(:,TotalSupPtr);
821      tar = sum(NewPosiMPPCP ~= 0,2) ==3;
822      NewPosiMPPCP = NewPosiMPPCP(tar,:);
823      NewVarM = VarM2(tar);
824      NewCoPM = CoPM1(:,TotalSupPtr) - CoPM2(:,TotalSupPtr);
825      tar = sum(NewCoPM ~= 0,2) ==6;
826      NewCoPM = NewCoPM(tar,:);
827      NewCoPEstDepth = CoPEstDepth(tar);   
828      NewHoriStickM = HoriStickM_i(:,TotalSupPtr)-HoriStickM_j(:,TotalSupPtr);
829      tar = sum(NewHoriStickM ~= 0,2) ==6;
830      NewHoriStickM = NewHoriStickM(tar,:);
831      NewEstDepHoriStick = EstDepHoriStick(tar);
832      NewVertStickM = VertStickM_i(:,TotalSupPtr)-VertStickM_j(:,TotalSupPtr);
833      tar = sum(NewVertStickM ~= 0,2) ==6;
834      NewVertStickM = NewVertStickM(tar,:);
835      NewEstDepVertStick = EstDepVertStick(tar);
836
837
838      Para = sdpvar(3*NuSubSupSize,1);
839      F = set(Para(3*(1:NuSubSupSize)-1).*YPointer(Sup2Para(TotalSup))<=0)...
840          +set(NewRayAllM*Para <=1/ClosestDist)...
841          +set(NewRayAllM*Para >=1/FarestDist)...
842          +set(Para(BoundarySupPtrSub) == PlanePara(BoundarySupPtr) )...
843          +set( Para( TotalSup2Para( NuSup( (intersect( indexVertical,SubSupPtr ) +1)/3))*3-1) == 0);
844          % Special constrain for decomp fix the solved neighbor plane parameter       
845
846          %      +set(Para(indexGroundX) == 0)...
847          %      +set(Para(indexGroundZ) == 0);
848
849      if FractionalDepthError
850%         size(PosiMPPCP)
851         solvesdp(F,norm( ( NewPosiMPPCP*Para-ones(size(NewPosiMPPCP,1),1))./exp(abs(NewVarM)/BandWith),1)...
852                 +Center*norm(( NewCoPM*Para).*NewCoPEstDepth, 1)...
853                 +norm(( NewHoriStickM*Para).*NewEstDepHoriStick,1)...
854                 +norm(( NewVertStickM*Para).*NewEstDepVertStick,1)...
855                 +10*norm( ( Para( TotalSup2Para( NuSup( (intersect( indexGroundX,SubSupPtr ) +2)/3))*3-2))./...
856                            normPara( intersect( find(groundPara), ( SubSupPtr( 3:3:( size(SubSupPtr,1)) )/3 ) ) )', 1)...
857                 +10*norm( ( Para( TotalSup2Para( NuSup( (intersect( indexGroundZ,SubSupPtr ) )/3))*3))./...
858                            normPara( intersect( find(groundPara), ( SubSupPtr( 3:3:( size(SubSupPtr,1)) )/3 ) ) )', 1) ...
859                 , opt);
860%             sqrt( max(CoPM1*ParaPPCP(:), 1/FarestDist).*max(CoPM2*ParaPPCP(:), 1/FarestDist)), 1)...
861%             +norm(((VertStickM_i-VertStickM_j)*Para)./...
862%             sqrt( max(VertStickM_i*ParaPPCP(:), 1/FarestDist).*max(VertStickM_j*ParaPPCP(:), 1/FarestDist)),1)...
863%   pause;
864    %         +1000*norm( Para(indexVertical)./ normPara( find(verticalPara) )',1) ...
865%            +norm(((HoriStickM_i-HoriStickM_j)*Para)./sqrt((VertStickM_i*ParaPPCP(:)).*(VertStickM_j*ParaPPCP(:))),1)...
866      else % not used anymore
867         solvesdp(F,norm( RayMd*Para - DepthInverseM,1)...
868                 +Center*norm((CoPM1 - CoPM2)*Para,1)...
869                 +norm((VertStickM_i-VertStickM_j)*Para,1)...
870                 +norm((HoriStickM_i-HoriStickM_j)*Para,1)...
871                 +1000*norm( Para(indexVertical)./ normPara( find(verticalPara) )',1) ...
872                 +1000*norm( Para(indexGroundX)./ normPara( find(groundPara) )', 1)  ...
873                 +1000*norm( Para(indexGroundZ)./ normPara( find(groundPara) )', 1) ...
874                 , opt);
875%         +0.01*norm( RayMtilt*ParaPPCP,1)...       
876%         +norm(spdiags(WeiCoP,0,size(CoPM1,1),size(CoPM1,1))*(CoPM1 - CoPM2)*ParaPPCP,1)...
877%         +norm( (CoPM1 - CoPM2)*ParaPPCP,1 )...
878     end
879
880     Para = double(Para);
881     %sum(isnan(Para))
882     yalmip('clear');
883     PlanePara(TotalSupPtr) = Para;
884    end
885  end
886
887  %pause
888  PlanePara = reshape(PlanePara,3,[]);
889  FitDepth = FarestDist*ones(1,Default.VertYNuDepth*Default.HoriXNuDepth);
890  FitDepth(~maskSkyEroded) = (1./sum(PlanePara(:,Sup2Para(SupEpand(~maskSkyEroded))).*Ray(:,~maskSkyEroded),1))';
891  FitDepth = reshape(FitDepth,Default.VertYNuDepth,[]);
892  %sum(isnan(FitDepth(:)))
893  [Position3DFited] = im_cr2w_cr(FitDepth,permute(Ray,[2 3 1]));
894  Position3DFited(3,:) = -Position3DFited(3,:);
895  Position3DFited = permute(Position3DFited,[2 3 1]);
896  RR =permute(Ray,[2 3 1]);
897  temp = RR(:,:,1:2)./repmat(RR(:,:,3),[1 1 2]);
898  PositionTex = permute(temp./repmat(cat(3,Default.a_default,Default.b_default),...
899                [Default.VertYNuDepth Default.HoriXNuDepth 1])...
900                +repmat(cat(3,Default.Ox_default,Default.Oy_default),...
901                [Default.VertYNuDepth Default.HoriXNuDepth 1]),[3 1 2]);
902  PositionTex = permute(PositionTex,[2 3 1]);
903  WrlFacestHroiReduce(Position3DFited,PositionTex,Sup,Default.filename{1},'./')
904
905% save depth Map ++++++++++++++++
906%       depthMap = FitDepth;
907%       system(['mkdir ' Default.ScratchDataFolder '/VNonSupport_' DepthFolder '/']);
908%       save([Default.ScratchDataFolder '/VNonSupport_' DepthFolder '/' depthfile '.mat'],'depthMap');
909% =============================
910% return;
Note: See TracBrowser for help on using the repository browser.