[37] | 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 | % */
|
---|
| 39 | function [] = MetricRecon(defaultPara, ImgInfo, Fdir, left, right, Wrlname, appendOpt, RenderFlag) |
---|
| 40 | % This function estimate the orientation and translation of a pair of imaegs |
---|
| 41 | % Always set the left image as the world coordinate |
---|
| 42 | % assumption: |
---|
| 43 | % 1) image center is in the cneter of the pixel coordinate (e. round(2272/2) round(1704/2)) |
---|
| 44 | % 2) No Skew |
---|
| 45 | % 3) Aspect ratio is know |
---|
| 46 | % processure: |
---|
| 47 | % 1) run the ransac matching |
---|
| 48 | % 2) retrive the camera matrix(P) and structure(X) up to projective transformation |
---|
| 49 | % 3) Using the assumption of the camera intrinsic parameter to do linearized self-calibration |
---|
| 50 | % 4) rescale the learned depth of two images |
---|
| 51 | % 5) plane parameter inference of a pair of images |
---|
| 52 | |
---|
| 53 | % Setup parameters |
---|
| 54 | Type = '_RConS'; |
---|
| 55 | ResultFolder = '/afs/cs/group/reconstruction3d/scratch/3DmodelMultipleImage'; |
---|
| 56 | EstimateIntrinsicCameraPara % setup the intrinsic camera parameters |
---|
| 57 | image3dstichingSetupUp% setup for the new image3d stitching functinos |
---|
| 58 | ACalibratedFlag = 1; % set to 1 if using EstimateIntrinsicCameraPara |
---|
| 59 | BCalibratedFlag = 1; % set to 1 if using EstimateIntrinsicCameraPara |
---|
| 60 | UnDoAspecRatioFlag = 0; |
---|
| 61 | ClosestMatchfindFlag = 0; |
---|
| 62 | RenderWrlDirectlyFlag = 1; |
---|
| 63 | GroundStaticFlag = 0; |
---|
| 64 | GroundStaticWeight = 5; |
---|
| 65 | solverVerboseLevel = 0; |
---|
| 66 | Match3DThreshould = 5; % meters |
---|
| 67 | opt = sdpsettings('solve','sedumi','cachesolvers',1,'verbose',solverVerboseLevel); |
---|
| 68 | |
---|
| 69 | AImgPath = [Fdir '/jpg/' left '.jpg']; |
---|
| 70 | BImgPath = [Fdir '/jpg/' right '.jpg']; |
---|
| 71 | OutPutFolder = '/afs/cs/group/reconstruction3d/scratch/3DmodelMultipleImage/'; |
---|
| 72 | taskName = '';%(Not Used) taskname will append to the imagename and form the outputname |
---|
| 73 | Flag.DisplayFlag = 0; |
---|
| 74 | Flag.IntermediateStorage = 0; |
---|
| 75 | Flag.FeaturesOnly = 0; |
---|
| 76 | Flag.NormalizeFlag = 1; |
---|
| 77 | Flag.BeforeInferenceStorage = 0; |
---|
| 78 | Flag.NonInference = 0; |
---|
| 79 | Flag.AfterInferenceStorage = 1; |
---|
| 80 | %ScratchFolder = '/afs/cs/group/reconstruction3d/scratch/IMStorage'; % ScratchFolder |
---|
| 81 | ScratchFolder = '/afs/cs/group/reconstruction3d/scratch/temp'; % ScratchFolder |
---|
| 82 | ParaFolder = '/afs/cs/group/reconstruction3d/scratch/Para/'; % default parameter folder |
---|
| 83 | |
---|
| 84 | |
---|
| 85 | % load images |
---|
| 86 | imgA = imread([ Fdir '/jpg/' left '.jpg']); |
---|
| 87 | AimgCameraParameters = exifread( [ Fdir '/jpg/' left '.jpg']); |
---|
| 88 | [Ay Ax dummy] = size(imgA); |
---|
| 89 | imgB = imread([ Fdir '/jpg/' right '.jpg']); |
---|
| 90 | BimgCameraParameters = exifread( [ Fdir '/jpg/' left '.jpg']); |
---|
| 91 | [By Bx dummy] = size(imgB); |
---|
| 92 | |
---|
| 93 | % copy image to the Outputfolder |
---|
| 94 | system(['cp ' Fdir '/jpg/' left '.jpg ' OutPutFolder]); |
---|
| 95 | system(['cp ' Fdir '/jpg/' right '.jpg ' OutPutFolder]); |
---|
| 96 | |
---|
| 97 | % known assumption |
---|
| 98 | % 1. camera center |
---|
| 99 | if ACalibratedFlag |
---|
| 100 | AUx = Default.Ox; % in pixel coordinate % Default round(Px/2) |
---|
| 101 | AUy = Default.Oy; % in pixel coordinate % Default round(Py/2) |
---|
| 102 | elseif 0 |
---|
| 103 | AUx = 1; |
---|
| 104 | AUy = 1; |
---|
| 105 | else |
---|
| 106 | AUx = round(Ax/2); % in pixel coordinate % Default round(Px/2) |
---|
| 107 | AUy = round(Ay/2); % in pixel coordinate % Default round(Py/2) |
---|
| 108 | end |
---|
| 109 | if BCalibratedFlag |
---|
| 110 | BUx = Default.Ox; % in pixel coordinate % Default round(Px/2) |
---|
| 111 | BUy = Default.Oy; % in pixel coordinate % Default round(Py/2) |
---|
| 112 | elseif 0 |
---|
| 113 | BUx =1; |
---|
| 114 | BUy =1; |
---|
| 115 | else |
---|
| 116 | BUx = round(Bx/2); % in pixel coordinate % Default round(Px/2) |
---|
| 117 | BUy = round(By/2); % in pixel coordinate % Default round(Py/2) |
---|
| 118 | end |
---|
| 119 | |
---|
| 120 | % 2. camera aspect ratio |
---|
| 121 | if ACalibratedFlag |
---|
| 122 | AAspectRatio = Default.fx/Default.fy; % in pixel coordinate % Default round(Px/2) |
---|
| 123 | elseif 0 |
---|
| 124 | AAspectRatio = 1; |
---|
| 125 | else |
---|
| 126 | AAspectRatio = 1; % in pixel coordinate % Default round(Px/2) |
---|
| 127 | end |
---|
| 128 | if BCalibratedFlag |
---|
| 129 | BAspectRatio = Default.fx/Default.fy; % in pixel coordinate % Default round(Px/2) |
---|
| 130 | elseif 0 |
---|
| 131 | BAspectRatio = 1; |
---|
| 132 | else |
---|
| 133 | BAspectRatio = 1; % in pixel coordinate % Default round(Px/2) |
---|
| 134 | end |
---|
| 135 | |
---|
| 136 | % 1) run the matching to get F and match point |
---|
| 137 | if system(['ls ' Fdir '/surf/*.surf']) |
---|
| 138 | cd ./match/ |
---|
| 139 | % system(['./surfFeaturesAndMatchesDir.sh ' Fdir]); |
---|
| 140 | system(['./surfMatch.sh ' Fdir '' left '' right]); |
---|
| 141 | cd .. |
---|
| 142 | end |
---|
| 143 | |
---|
| 144 | I1=imreadbw([Fdir '/pgm/' left '.pgm']); |
---|
| 145 | I2=imreadbw([Fdir '/pgm/' right '.pgm']); |
---|
| 146 | |
---|
| 147 | |
---|
| 148 | [f1, f2, matches] = readSurfMatches(left, right, Fdir, Type, false, true); |
---|
| 149 | % displaySurfMatches(Fdir, left, right, Type, 0, 1); |
---|
| 150 | % ============Load mono info========== |
---|
| 151 | % process imageA |
---|
| 152 | % cd ../LearningCode |
---|
| 153 | % InitialPath |
---|
| 154 | % cd ../multipleImages |
---|
| 155 | if appendOpt |
---|
| 156 | load( [ScratchFolder '/' left '_NonMono.mat']); |
---|
| 157 | else |
---|
| 158 | if system(['ls ' ScratchFolder '/' left '__AInfnew.mat']); |
---|
| 159 | cd ../LearningCode |
---|
| 160 | OneShot3dEfficient(AImgPath, OutPutFolder,... |
---|
| 161 | taskName,...% taskname will append to the imagename and form the outputname |
---|
| 162 | ScratchFolder,... % ScratchFolder |
---|
| 163 | ParaFolder,... |
---|
| 164 | Flag... % All Flags 1) intermediate storage flag |
---|
| 165 | ); |
---|
| 166 | cd ../multipleImages |
---|
| 167 | end |
---|
| 168 | load( [ScratchFolder '/' left '__AInfnew.mat']); |
---|
| 169 | % ================== |
---|
| 170 | ARotation = eye(3); |
---|
| 171 | ATranslation = zeros(3,1); |
---|
| 172 | AHistory=[]; |
---|
| 173 | %AdepthMapRaw = depthMap; |
---|
| 174 | AdepthMap = full(FitDepth); |
---|
| 175 | ARayAll = Ray; |
---|
| 176 | ARayOri = RayOri; |
---|
| 177 | ASup = Sup; |
---|
| 178 | AMedSup = MedSup; |
---|
| 179 | ASupOri = SupOri; |
---|
| 180 | AMedSup = MedSup; |
---|
| 181 | ASupNeighborTable =SupNeighborTable; |
---|
| 182 | AmaskSky = maskSky; |
---|
| 183 | AmaskG = maskG; |
---|
| 184 | AMultiScaleSupTable = MultiScaleSupTable; |
---|
| 185 | Aconstrain.RayMatched = [ ]; |
---|
| 186 | Aconstrain.Depth_modified = [ ]; |
---|
| 187 | Aconstrain.SupMatched = [ ]; |
---|
| 188 | % ================= |
---|
| 189 | end |
---|
| 190 | % process imageB |
---|
| 191 | if system(['ls ' ScratchFolder '/' right '__AInfnew.mat']); |
---|
| 192 | cd ../LearningCode |
---|
| 193 | OneShot3dEfficient(BImgPath, OutPutFolder,... |
---|
| 194 | taskName,...% taskname will append to the imagename and form the outputname |
---|
| 195 | ScratchFolder,... % ScratchFolder |
---|
| 196 | ParaFolder,... |
---|
| 197 | Flag... % All Flags 1) intermediate storage flag |
---|
| 198 | ); |
---|
| 199 | cd ../multipleImages |
---|
| 200 | end |
---|
| 201 | load([ScratchFolder '/' right '__AInfnew.mat']); |
---|
| 202 | % ================== |
---|
| 203 | %BdepthMapRaw = depthMap; |
---|
| 204 | BHistory=[]; |
---|
| 205 | BdepthMap = full(FitDepth); |
---|
| 206 | BRayAll = Ray; |
---|
| 207 | BRayOri = RayOri; |
---|
| 208 | BSup = Sup; |
---|
| 209 | BMedSup = MedSup; |
---|
| 210 | BSupOri = SupOri; |
---|
| 211 | BMedSup = MedSup; |
---|
| 212 | BSupNeighborTable =SupNeighborTable; |
---|
| 213 | BmaskSky = maskSky; |
---|
| 214 | BmaskG = maskG; |
---|
| 215 | BMultiScaleSupTable = MultiScaleSupTable; |
---|
| 216 | % ================= |
---|
| 217 | % robut estimate of matches |
---|
| 218 | IND1 = sub2ind([55 305],max(min(round(f1(2,matches(1,:))/size(I1,1)*55),55),1),max(min(round(f1(1,matches(1,:))/size(I1,2)*305),305),1)); |
---|
| 219 | IND2 = sub2ind([55 305],max(min(round(f2(2,matches(2,:))/size(I1,1)*55),55),1),max(min(round(f2(1,matches(2,:))/size(I1,2)*305),305),1)); |
---|
| 220 | D1 = AdepthMap(IND1); |
---|
| 221 | D2 = BdepthMap(IND2); |
---|
| 222 | [F, newinliers, NewDist, fail]=GeneralRansac(defaultPara, f1, f2, matches, D1, D2); |
---|
| 223 | figure; |
---|
| 224 | plotmatches(I1,I2,f1, f2,matches(:,newinliers), 'Stacking', 'v', 'Interactive', 2); |
---|
| 225 | % [ F, newinliers, fail]=RansacOnPairMatches(defaultPara, f1, f2, matches, I1, I2, D1, D2, 1) |
---|
| 226 | % ================= |
---|
| 227 | % ==================================== |
---|
| 228 | |
---|
| 229 | AMatch = [f1(1,matches(1,newinliers)) ; f1(2,matches(1,newinliers))]; % [x ; y] pixel coordinate (left top as origin) |
---|
| 230 | BMatch = [f2(1,matches(2,newinliers)) ; f2(2,matches(2,newinliers))]; % [x ; y] pixel coordinate (left top as origin) |
---|
| 231 | % BMatch = [f1(1,matches(1,newinliers)) ; f1(2,matches(1,newinliers))]; % [x ; y] pixel coordinate (left top as origin) |
---|
| 232 | % AMatch = [f2(1,matches(2,newinliers)) ; f2(2,matches(2,newinliers))]; % [x ; y] pixel coordinate (left top as origin) |
---|
| 233 | % ================================================================= |
---|
| 234 | |
---|
| 235 | % ==========undo the camera center and the camera aspect ratio if needed ==================== |
---|
| 236 | % to improve the condition of the camera matrix |
---|
| 237 | % translate the coordinate into camera center coordinate |
---|
| 238 | AMatchCenter = [ [AMatch(1,:) - AUx];... |
---|
| 239 | [Ay + 1 - AMatch(2,:) - AUy] ]; % [x ; y] pixel coordinate (center as origin) |
---|
| 240 | BMatchCenter = [ [BMatch(1,:) - BUx];... |
---|
| 241 | [By + 1 - BMatch(2,:) - BUy] ]; % [x ; y] pixel coordinate (center as origin) |
---|
| 242 | if UnDoAspecRatioFlag |
---|
| 243 | AScale = [ Ax; Ay]; % important |
---|
| 244 | BScale = [ Bx; By]; % important |
---|
| 245 | else |
---|
| 246 | AScale = [ Ax; Ax]; % important |
---|
| 247 | BScale = [ Bx; Bx]; % important |
---|
| 248 | end |
---|
| 249 | % scaling to improve condition |
---|
| 250 | AMatchCenter_normalized = AMatchCenter./repmat(AScale,1,size( AMatchCenter,2)); |
---|
| 251 | BMatchCenter_normalized = BMatchCenter./repmat(BScale,1,size( BMatchCenter,2)); |
---|
| 252 | % ================================================================= |
---|
| 253 | Ray = permute(Ray, [ 2 3 1]); |
---|
| 254 | [APositionAll] = im_cr2w_cr( AdepthMap,Ray); |
---|
| 255 | [Dy Dx] = size(AdepthMap); |
---|
| 256 | AMatchDepthRes = [ ( AMatch(1,:)-0.5)/Ax*Dx+0.5 ;... |
---|
| 257 | ( AMatch(2,:) -0.5)/Ay*Dy+0.5]; % [x ; y] in Dy Dx Resolution |
---|
| 258 | % the closet approximate of the match points to the depthMap index |
---|
| 259 | Aindex = min(max( round( AMatchDepthRes(1,:))-1, 0), Dx)*Dy+ min(max( round( AMatchDepthRes(2,:)), 0), Dy); |
---|
| 260 | ADepthMatch = AdepthMap( Aindex); |
---|
| 261 | ASampleImCoordYSmall = (( Dy+1-AMatchDepthRes(2,:))-0.5)/Dy - Default.Oy_default; |
---|
| 262 | ASampleImCoordXSmall = (AMatchDepthRes(1,:) -0.5)/Dx - Default.Ox_default; |
---|
| 263 | ARayMatch = permute( RayImPosition( ASampleImCoordYSmall, ASampleImCoordXSmall,... |
---|
| 264 | Default.a_default, Default.b_default, ... |
---|
| 265 | Default.Ox_default,Default.Oy_default), [3 2 1]); %[ 3 horiXSizeLowREs VertYSizeLowREs] |
---|
| 266 | APositionMatch = [ARayMatch'.*repmat( ADepthMatch', 1, 3)]; |
---|
| 267 | |
---|
| 268 | [BPositionAll] = im_cr2w_cr( BdepthMap,Ray); |
---|
| 269 | BMatchDepthRes = [ ( BMatch(1,:)-0.5)/Bx*Dx+0.5 ; ( BMatch(2,:)-0.5)/By*Dy+0.5]; % [x ; y] |
---|
| 270 | Bindex = min(max( round( BMatchDepthRes(1,:))-1, 0), Dx)*Dy+ min(max( round( BMatchDepthRes(2,:)), 0), Dy); |
---|
| 271 | BDepthMatch = BdepthMap( Bindex); |
---|
| 272 | BSampleImCoordYSmall = (( Dy+1-BMatchDepthRes(2,:))-0.5)/Dy - Default.Oy_default; |
---|
| 273 | BSampleImCoordXSmall = ( BMatchDepthRes(1,:) -0.5)/Dx - Default.Ox_default; |
---|
| 274 | BRayMatch = permute( RayImPosition( BSampleImCoordYSmall, BSampleImCoordXSmall,... |
---|
| 275 | Default.a_default, Default.b_default, ... |
---|
| 276 | Default.Ox_default,Default.Oy_default), [3 2 1]); %[ 3 horiXSizeLowREs VertYSizeLowREs] |
---|
| 277 | BPositionMatch = (BRayMatch').*repmat( BDepthMatch', 1, 3); |
---|
| 278 | |
---|
| 279 | % 2) retrive the camera matrix(P) and structure(X) up to projective transformation |
---|
| 280 | [ P, X, OutlinerCameraCoord] = ProjectionFactorization( [Ax; Ay]./AScale, [Bx; By]./BScale, ... |
---|
| 281 | cat( 3, [ AMatchCenter_normalized(1,:); AMatchCenter_normalized(2,:); ones(1, size(AMatchCenter_normalized ,2)) ],... |
---|
| 282 | [ BMatchCenter(1,:); BMatchCenter(2,:); ones(1, size(AMatchCenter_normalized ,2))]),... |
---|
| 283 | cat( 3, APositionMatch(:,3)', BPositionMatch(:,3)')); |
---|
| 284 | |
---|
| 285 | % remove outliners |
---|
| 286 | Aindex(OutlinerCameraCoord) = []; |
---|
| 287 | ADepthMatch(OutlinerCameraCoord) = []; |
---|
| 288 | ARayMatch(:,OutlinerCameraCoord) = []; |
---|
| 289 | APositionMatch(OutlinerCameraCoord,:) = []; |
---|
| 290 | ASampleImCoordYSmall(OutlinerCameraCoord) = []; |
---|
| 291 | ASampleImCoordXSmall(OutlinerCameraCoord) = []; |
---|
| 292 | Bindex(OutlinerCameraCoord) = []; |
---|
| 293 | BDepthMatch(OutlinerCameraCoord) = []; |
---|
| 294 | BRayMatch(:,OutlinerCameraCoord) = []; |
---|
| 295 | BPositionMatch(OutlinerCameraCoord,:) = []; |
---|
| 296 | BSampleImCoordYSmall(OutlinerCameraCoord) = []; |
---|
| 297 | BSampleImCoordXSmall(OutlinerCameraCoord) = []; |
---|
| 298 | |
---|
| 299 | % 3) Using the assumption of the camera intrinsic parameter to do linearized self-calibration |
---|
| 300 | [ U S V] = svd(P(1:3,:)); |
---|
| 301 | % ============ simple try ================================ |
---|
| 302 | P1 = P(1:3,:); |
---|
| 303 | P2 = P(4:6,:); |
---|
| 304 | Ksimple = diag( [ Default.fx/AScale(1) Default.fy/AScale(2) 1]); |
---|
| 305 | H_a_simple = P1\[Ksimple]; % the upper 4x3 part of the projective transformation matrix |
---|
| 306 | [ U S V] = svd(P1); |
---|
| 307 | H_b_simple = V(:,4); % the last 4x1 column vector |
---|
| 308 | K2simple_square = sdpvar(3,1); |
---|
| 309 | F = set(diag(K2simple_square) >=0); |
---|
| 310 | sol = solvesdp(F , norm( P2*H_a_simple*H_a_simple'*P2' - diag(K2simple_square),'fro'), opt); |
---|
| 311 | K2simple_square = double(K2simple_square); |
---|
| 312 | K2simple = sqrt(K2simple_square); |
---|
| 313 | R2_est = diag( 1./K2simple)*P2*H_a_simple; |
---|
| 314 | if GroundStaticFlag |
---|
| 315 | disp('Ground Static'); |
---|
| 316 | R2_ground_constrain = sdpvar(3,3); |
---|
| 317 | sol =solvesdp( [], norm( R2_ground_constrain - R2_est,'fro') + GroundStaticWeight*norm([0 1 0]' - R2_ground_constrain*[0 1 0]'), opt); |
---|
| 318 | R2_ground_constrain = double( R2_ground_constrain); |
---|
| 319 | disp('[0; 1; 0] - R2_ground_constrain*[0; 1; 0]'); |
---|
| 320 | norm([0 1 0]' - R2_ground_constrain*[0 1 0]') |
---|
| 321 | R2 = R2_ground_constrain * (R2_ground_constrain'*R2_ground_constrain)^(-.5); |
---|
| 322 | else |
---|
| 323 | R2 = R2_est*(R2_est'*R2_est)^(-0.5); |
---|
| 324 | end |
---|
| 325 | % check if norm([0 1 0]' - R2*[0 1 0]'); |
---|
| 326 | disp('[0; 1; 0] - R2*[0; 1; 0]'); |
---|
| 327 | norm([0 1 0]' - R2*[0 1 0]') |
---|
| 328 | % pause; |
---|
| 329 | |
---|
| 330 | % set image B's groud and vertical vector; |
---|
| 331 | % BRotation = R2'*ARotation'; |
---|
| 332 | BRotation = ARotation*R2'; |
---|
| 333 | %BDirectionFromReference = R2*eye(3); |
---|
| 334 | ADirectionFromReference = ARotation'*eye(3); |
---|
| 335 | BDirectionFromReference = BRotation'*eye(3); |
---|
| 336 | |
---|
| 337 | T_simple_before_R = R2'*inv(diag(sqrt(K2simple_square)))*P2*H_b_simple; |
---|
| 338 | yalmip('clear'); |
---|
| 339 | % after calibration |
---|
| 340 | A.a_default = Default.TrainHoriXSize/Default.fx; |
---|
| 341 | A.b_default = Default.TrainVerYSize/Default.fy; |
---|
| 342 | B.a_default = A.a_default;%1704/(K2simple(1)*AScale(1)/K2simple(3)); |
---|
| 343 | B.b_default = A.b_default;%2272/(K2simple(2)*AScale(2)/K2simple(3)); |
---|
| 344 | ARay_after_calibration = permute( RayImPosition( ASampleImCoordYSmall, ASampleImCoordXSmall,... |
---|
| 345 | A.a_default, A.b_default), [3 2 1]); %[ 3 horiXSizeLowREs VertYSizeLowREs] |
---|
| 346 | BRay_after_calibration = permute( RayImPosition( BSampleImCoordYSmall, BSampleImCoordXSmall,... |
---|
| 347 | B.a_default, B.b_default), [3 2 1]); %[ 3 horiXSizeLowREs VertYSizeLowREs] |
---|
| 348 | % solve for the translation scaling factor |
---|
| 349 | T = sdpvar(3,1); |
---|
| 350 | ADepth_modified = sdpvar(1,length(ADepthMatch)); |
---|
| 351 | BDepth_modified = sdpvar(1,length(BDepthMatch)); |
---|
| 352 | BDepthScale = sdpvar(1); |
---|
| 353 | sol =solvesdp( [], norm( reshape( (ARay_after_calibration.*repmat( ADepth_modified, 3,1))' - ... |
---|
| 354 | ( R2'*( BRay_after_calibration.*repmat( BDepth_modified, 3, 1) ) + repmat(T,1,length(ADepthMatch)) )' ,1,[]),1)+... |
---|
| 355 | norm(ADepth_modified - ADepthMatch,1) + norm(BDepth_modified - BDepthScale*BDepthMatch,1), opt); |
---|
| 356 | T_simple_before_R = double(T); |
---|
| 357 | ADepth_modified = double(ADepth_modified); |
---|
| 358 | BDepth_modified = double(BDepth_modified); |
---|
| 359 | BDepthScale = double(BDepthScale); |
---|
| 360 | ADepthScale = 1; |
---|
| 361 | |
---|
| 362 | % clean the 3D matches if the |AMatchPosition - BMatchPosition | > Match3DThreshould ================= |
---|
| 363 | DistMatchError = ARay_after_calibration.*repmat( ADepth_modified, 3,1) - ... |
---|
| 364 | ( R2'*( BRay_after_calibration.*repmat( BDepth_modified, 3, 1) ) + repmat(T_simple_before_R,1,length(ADepthMatch)) ); |
---|
| 365 | DistMatchError = norms(DistMatchError); |
---|
| 366 | OutlinerMark = DistMatchError > Match3DThreshould; |
---|
| 367 | |
---|
| 368 | Aindex(OutlinerMark) = []; |
---|
| 369 | ADepthMatch(OutlinerMark) = []; |
---|
| 370 | ARayMatch(:,OutlinerMark) = []; |
---|
| 371 | APositionMatch(OutlinerMark,:) = []; |
---|
| 372 | ASampleImCoordYSmall(OutlinerMark) = []; |
---|
| 373 | ASampleImCoordXSmall(OutlinerMark) = []; |
---|
| 374 | Bindex(OutlinerMark) = []; |
---|
| 375 | BDepthMatch(OutlinerMark) = []; |
---|
| 376 | BRayMatch(:,OutlinerMark) = []; |
---|
| 377 | BPositionMatch(OutlinerMark,:) = []; |
---|
| 378 | BSampleImCoordYSmall(OutlinerMark) = []; |
---|
| 379 | BSampleImCoordXSmall(OutlinerMark) = []; |
---|
| 380 | ARay_after_calibration(:, OutlinerMark) = []; |
---|
| 381 | BRay_after_calibration(:, OutlinerMark) = []; |
---|
| 382 | ADepth_modified( OutlinerMark) = []; |
---|
| 383 | BDepth_modified( OutlinerMark) = []; |
---|
| 384 | |
---|
| 385 | % =================================================================================================== |
---|
| 386 | if ClosestMatchfindFlag |
---|
| 387 | % solve the opt depth for matches of image A and image B |
---|
| 388 | opt = sdpsettings('solver','sedumi'); |
---|
| 389 | AClosestDepth = sdpvar( 1,length(ADepthMatch)); |
---|
| 390 | BClosestDepth = sdpvar( 1,length(BDepthMatch)); |
---|
| 391 | F = set(AClosestDepth >=0) + set(BClosestDepth >=0); |
---|
| 392 | sol = solvesdp( F, norm( reshape( (ARay_after_calibration.*repmat( AClosestDepth, 3,1)) - ... |
---|
| 393 | (R2'*(BRay_after_calibration.*repmat( BClosestDepth, 3,1)) + repmat(T_simple_before_R, 1, length(ADepthMatch))) , 1,[]), 1), opt); |
---|
| 394 | AClosestDepth = double(AClosestDepth); |
---|
| 395 | BClosestDepth = double(BClosestDepth); |
---|
| 396 | |
---|
| 397 | |
---|
| 398 | % check if the ray really match well ============================= |
---|
| 399 | figure(50); title('Closest point Match Point, one opt with BScale'); |
---|
| 400 | AClosestMatchPosition = ARay_after_calibration.*repmat( AClosestDepth, 3,1); |
---|
| 401 | BClosestMatchPosition = R2'*(BRay_after_calibration.*repmat( BClosestDepth, 3,1)) + repmat(T_simple_before_R, 1, length(ADepthMatch)); |
---|
| 402 | scatter3(AClosestMatchPosition(1,:)', AClosestMatchPosition(3,:)', AClosestMatchPosition(2,:)', 0.5*ones(1,size( AClosestMatchPosition,2))); |
---|
| 403 | hold on; |
---|
| 404 | scatter3(BClosestMatchPosition(1,:)', BClosestMatchPosition(3,:)', BClosestMatchPosition(2,:)', 0.5*ones(1,size( BClosestMatchPosition,2))); |
---|
| 405 | line( [ AClosestMatchPosition(1,:); BClosestMatchPosition(1,:)], ... |
---|
| 406 | [ AClosestMatchPosition(3,:); BClosestMatchPosition(3,:)], ... |
---|
| 407 | [ AClosestMatchPosition(2,:); BClosestMatchPosition(2,:)]); |
---|
| 408 | % ================================================================ |
---|
| 409 | end |
---|
| 410 | % check if the ray really match well ============================= |
---|
| 411 | figure(52); title('Match Point, one opt with BScale'); |
---|
| 412 | ADepth_modifiedPosition = ARay_after_calibration.*repmat( ADepth_modified, 3,1); |
---|
| 413 | BDepth_modifiedPosition = R2'*(BRay_after_calibration.*repmat( BDepth_modified, 3,1)) + repmat(T_simple_before_R, 1, length(ADepthMatch)); |
---|
| 414 | scatter3(ADepth_modifiedPosition(1,:)', ADepth_modifiedPosition(3,:)', ADepth_modifiedPosition(2,:)', 0.5*ones(1,size( ADepth_modifiedPosition,2))); |
---|
| 415 | hold on; |
---|
| 416 | scatter3(BDepth_modifiedPosition(1,:)', BDepth_modifiedPosition(3,:)', BDepth_modifiedPosition(2,:)', 0.5*ones(1,size( BDepth_modifiedPosition,2))); |
---|
| 417 | line( [ ADepth_modifiedPosition(1,:); BDepth_modifiedPosition(1,:)], ... |
---|
| 418 | [ ADepth_modifiedPosition(3,:); BDepth_modifiedPosition(3,:)], ... |
---|
| 419 | [ ADepth_modifiedPosition(2,:); BDepth_modifiedPosition(2,:)]); |
---|
| 420 | % ================================================================ |
---|
| 421 | |
---|
| 422 | BTranslation = ATranslation + ARotation*T_simple_before_R; |
---|
| 423 | % =========not used anymore since scaling is included in match point jointly opt |
---|
| 424 | % % solve the scale for depth A and depth B |
---|
| 425 | % ADepthScale = sdpvar(1); |
---|
| 426 | % BDepthScale = sdpvar(1); |
---|
| 427 | % sol =solvesdp( [], norm( ADepth_modified - ADepthScale*ADepthMatch, 1)+... |
---|
| 428 | % norm( BDepth_modified - BDepthScale*BDepthMatch, 1),opt); |
---|
| 429 | % ADepthScale = double(ADepthScale); |
---|
| 430 | % BDepthScale = double(BDepthScale); |
---|
| 431 | % %ADepth_normalized = ADepth*ADepthScale; |
---|
| 432 | % %BDepth_normalized = BDepthMatch*BDepthScale; |
---|
| 433 | % ========================================================================= |
---|
| 434 | |
---|
| 435 | if RenderWrlDirectlyFlag % only a reality check to see if the rotation and translation is reasonable |
---|
| 436 | % rendering the joint wrl |
---|
| 437 | temp = Ray(:,:,1:2)./repmat(Ray(:,:,3),[1 1 2]); |
---|
| 438 | 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]); |
---|
| 439 | PositionTex = permute(PositionTex,[2 3 1]); |
---|
| 440 | % if SeperateOptFlag |
---|
| 441 | AWrlPosition = ARotation*APositionAll(:,:)*ADepthScale+... |
---|
| 442 | repmat( ATranslation, 1, size(APositionAll(:,:), 2)); |
---|
| 443 | BWrlPosition = BRotation*BPositionAll(:,:)*BDepthScale+... |
---|
| 444 | repmat( BTranslation, 1, size(BPositionAll(:,:),2)); |
---|
| 445 | AWrlPosition = reshape(AWrlPosition,3,55,[]); |
---|
| 446 | AWrlPosition(3,:) = - AWrlPosition(3,:); |
---|
| 447 | AWrlPosition = permute(AWrlPosition,[2 3 1]); |
---|
| 448 | BWrlPosition = reshape(BWrlPosition,3,55,[]); |
---|
| 449 | BWrlPosition(3,:) = - BWrlPosition(3,:); |
---|
| 450 | BWrlPosition = permute(BWrlPosition,[ 2 3 1]); |
---|
| 451 | WrlFacestHroiReduce(AWrlPosition,PositionTex,ASup,left,[ Wrlname '_' left '-' right '_OldMulti'], [ResultFolder '/'], 0,0); |
---|
| 452 | WrlFacestHroiReduce(BWrlPosition,PositionTex,BSup,right,[ Wrlname '_' left '-' right '_OldMulti'], [ResultFolder '/'], 0,1); |
---|
| 453 | system(['cp ' Fdir '/jpg/' left '.jpg ' ResultFolder '/' left '.jpg']); |
---|
| 454 | system(['cp ' Fdir '/jpg/' right '.jpg ' ResultFolder '/' right '.jpg']); |
---|
| 455 | disp('Finish model simple rotation and translation, first reality check'); |
---|
| 456 | % pause; |
---|
| 457 | end |
---|
| 458 | |
---|
| 459 | % =========estimating ground level using ground in imgA and imgB on image A coordinate |
---|
| 460 | % cleaning Groundmask for A |
---|
| 461 | RangePercent = 100; |
---|
| 462 | AY_median = median( APositionAll(2,AmaskG)); |
---|
| 463 | ADistant2Ay_median = (APositionAll(2,AmaskG) - AY_median); |
---|
| 464 | ANumber_YMedian = round( sum(APositionAll(2,AmaskG)<AY_median)*RangePercent/100); |
---|
| 465 | [Avalue AIndexSort] = sort(ADistant2Ay_median); |
---|
| 466 | AYmedia_mark = AIndexSort(1:ANumber_YMedian); |
---|
| 467 | AGround_mark = zeros(Dy, Dx); |
---|
| 468 | temp = zeros(sum(AmaskG(:)) ,1); |
---|
| 469 | temp(AYmedia_mark) = 1; |
---|
| 470 | AGround_mark(AmaskG) = temp; |
---|
| 471 | AGround_mark = logical(AGround_mark); |
---|
| 472 | % finishing cleaning Groundmask for A |
---|
| 473 | |
---|
| 474 | % cleaning Groundmask for B |
---|
| 475 | BWrlPosition = R2'*BPositionAll(:,:)*BDepthScale+repmat( T_simple_before_R, 1, size(BPositionAll(:,:),2)); |
---|
| 476 | BWrlPosition = reshape(BWrlPosition,3,55,[]); |
---|
| 477 | BY_median = median( BWrlPosition(2,BmaskG)); |
---|
| 478 | BDistant2By_median = (BWrlPosition(2,BmaskG) - BY_median); |
---|
| 479 | BNumber_YMedian = round( sum(BWrlPosition(2,BmaskG)<BY_median)*RangePercent/100); |
---|
| 480 | [Bvalue BIndexSort] = sort(BDistant2By_median); |
---|
| 481 | BYmedia_mark = BIndexSort(1:BNumber_YMedian); |
---|
| 482 | BGround_mark = zeros(Dy, Dx); |
---|
| 483 | temp = zeros(sum(BmaskG(:)) ,1); |
---|
| 484 | temp(BYmedia_mark) = 1; |
---|
| 485 | BGround_mark(BmaskG) = temp; |
---|
| 486 | BGround_mark = logical( BGround_mark); |
---|
| 487 | % finishing cleaning Groundmask for B |
---|
| 488 | |
---|
| 489 | % find the jointly median of the ground of image AB in Y direction |
---|
| 490 | if ~appendOpt |
---|
| 491 | GroundY0inACoor = median([ APositionAll(2,AGround_mark) BWrlPosition(2,BGround_mark)]); |
---|
| 492 | end |
---|
| 493 | % ===================================== |
---|
| 494 | |
---|
| 495 | % given the AClosestDepth and BClosestDepth do the seperate inference again output the ADepth_normalized and BDepth_normalized |
---|
| 496 | % imgA |
---|
| 497 | AappendOpt = appendOpt; |
---|
| 498 | ASupMatched = ASup(Aindex)'; |
---|
| 499 | mask = ASupMatched == 0; |
---|
| 500 | ASupMatched(mask)=[]; |
---|
| 501 | ARayMatched = ARay_after_calibration'; |
---|
| 502 | ARayMatched(mask,:) = []; |
---|
| 503 | ADepth_modified(:,mask) = []; |
---|
| 504 | % AClosestDepth(:,mask) = []; |
---|
| 505 | Default.OutPutFolder = OutPutFolder; |
---|
| 506 | Default.ScratchFolder = ScratchFolder; |
---|
| 507 | Default.filename{1} = left; |
---|
| 508 | Default.Wrlname{1} = Wrlname; |
---|
| 509 | Default.Flag.AfterInferenceStorage = 0; |
---|
| 510 | % if ~appendOpt |
---|
| 511 | if RenderFlag |
---|
| 512 | Default.RenderFlag = 1; |
---|
| 513 | else |
---|
| 514 | Default.RenderFlag = 0; |
---|
| 515 | end |
---|
| 516 | [APosition_normalized ADepth_normalized] = ReportPlaneParaMRF_Conditioned_trianglate2( Default, ARotation, ATranslation, AappendOpt, ... |
---|
| 517 | [ ARayMatched; Aconstrain.RayMatched],... |
---|
| 518 | [ ADepth_modified Aconstrain.Depth_modified]',... |
---|
| 519 | [ ASupMatched; Aconstrain.SupMatched],... |
---|
| 520 | [ ], [ ], [ ], [],... |
---|
| 521 | ASup, ASupOri, AMedSup, AdepthMap*ADepthScale, zeros(size(AdepthMap)), ARayOri, ARayAll, ... |
---|
| 522 | ASupNeighborTable, [], AmaskSky, AmaskG,... |
---|
| 523 | 'cvx_allL1Norm',1,... |
---|
| 524 | [], [], AMultiScaleSupTable, [], [], [], false, 0, ADirectionFromReference, GroundY0inACoor); |
---|
| 525 | % else |
---|
| 526 | % ADepth_normalized = AdepthMap; |
---|
| 527 | % end |
---|
| 528 | % [APosition_normalized ADepth_normalized] = ReportPlaneParaMRF_Conditioned_trianglate( Default, ARotation, ATranslation, AappendOpt, ... |
---|
| 529 | % [ ], [ ], [ ], ... |
---|
| 530 | % [ ], [ ], [ ], [],... |
---|
| 531 | % ASup, ASupOri, AMedSup, AdepthMap*ADepthScale, ones(size(AdepthMap)), ARayOri, ARayAll, ... |
---|
| 532 | % ASupNeighborTable, [], AmaskSky, AmaskG,... |
---|
| 533 | % 'cvx_allL1Norm',1,... |
---|
| 534 | % [], [], AMultiScaleSupTable, [], [], [], false, 0, [], GroundY0inACoor); |
---|
| 535 | % imgB |
---|
| 536 | % BRotation = R2'*ARotation'; |
---|
| 537 | % BTranslation = ATranslation + ARotation*T_simple_before_R; |
---|
| 538 | BappendOpt = 1; |
---|
| 539 | BSupMatched = BSup(Bindex)'; |
---|
| 540 | mask = BSupMatched == 0; |
---|
| 541 | BSupMatched(mask)=[]; |
---|
| 542 | BRayMatched = BRay_after_calibration'; |
---|
| 543 | BRayMatched(mask,:) = []; |
---|
| 544 | BDepth_modified(:,mask) = []; |
---|
| 545 | % BClosestDepth(:,mask) = []; |
---|
| 546 | Default.OutPutFolder = OutPutFolder; |
---|
| 547 | Default.ScratchFolder = ScratchFolder; |
---|
| 548 | Default.filename{1} = right; |
---|
| 549 | Default.Wrlname{1} = Wrlname; |
---|
| 550 | Default.Flag.AfterInferenceStorage = 0; |
---|
| 551 | |
---|
| 552 | Default.RenderFlag = 0; |
---|
| 553 | [BPosition_normalized BDepth_normalized] = ... |
---|
| 554 | ReportPlaneParaMRF_Conditioned_trianglate2( Default, BRotation, BTranslation, BappendOpt, ... |
---|
| 555 | [ BRayMatched ], [ BDepth_modified' ], [ BSupMatched], ... |
---|
| 556 | [ ], [ ], [ ], [],... |
---|
| 557 | BSup, BSupOri, BMedSup, BdepthMap*BDepthScale, zeros(size(BdepthMap)), BRayOri, BRayAll, ... |
---|
| 558 | BSupNeighborTable, [], BmaskSky, BmaskG,... |
---|
| 559 | 'cvx_allL1Norm',1,... |
---|
| 560 | [], [], BMultiScaleSupTable, [], [], [], false, 0, BDirectionFromReference, GroundY0inACoor); |
---|
| 561 | % [BPosition_normalized BDepth_normalized] = ... |
---|
| 562 | % ReportPlaneParaMRF_Conditioned_trianglate( Default, BRotation, BTranslation, BappendOpt, ... |
---|
| 563 | % [ ], [ ], [ ], ... |
---|
| 564 | % [ ], [ ], [ ], [],... |
---|
| 565 | % BSup, BSupOri, BMedSup, BdepthMap*BDepthScale, ones(size(BdepthMap)), BRayOri, BRayAll, ... |
---|
| 566 | % BSupNeighborTable, [], BmaskSky, BmaskG,... |
---|
| 567 | % 'cvx_allL1Norm',1,... |
---|
| 568 | % [], [], BMultiScaleSupTable, [], [], [], false, 0, BDirectionFromReference, GroundY0inACoor); |
---|
| 569 | % |
---|
| 570 | AMatchedPosition_after_normalized = (ARay_after_calibration').*repmat( ADepth_normalized(Aindex)', 1, 3); |
---|
| 571 | BMatchedPosition_after_normalized = (BRay_after_calibration').*repmat( BDepth_normalized(Bindex)', 1, 3); |
---|
| 572 | |
---|
| 573 | % ============= save constrain Rotation Translation and combining history =============== |
---|
| 574 | %ImgA |
---|
| 575 | AHistory{end+1}= right; |
---|
| 576 | if isempty(intersect(AHistory, right)) |
---|
| 577 | Aconstrain.RayMatched = [ Aconstrain.RayMatched; ARayMatched ]; |
---|
| 578 | Aconstrain.Depth_modified = [ Aconstrain.Depth_modified ADepth_modified ]; |
---|
| 579 | Aconstrain.SupMatched = [ Aconstrain.SupMatched; ASupMatched ]; |
---|
| 580 | end |
---|
| 581 | AdepthMap = ADepth_normalized; |
---|
| 582 | save([ ScratchFolder '/' left '_NonMono.mat' ], 'AdepthMap', 'ARotation', 'ATranslation', 'AHistory', 'Aconstrain',... |
---|
| 583 | 'GroundY0inACoor',... |
---|
| 584 | 'ASup', 'ASupOri', 'AMedSup', 'ARayOri','ARayAll','ASupNeighborTable','AmaskSky','AmaskG','AMultiScaleSupTable'); |
---|
| 585 | |
---|
| 586 | %ImgB |
---|
| 587 | ARotation = BRotation; |
---|
| 588 | ATranslation = BTranslation; |
---|
| 589 | AHistory{1}= 'left'; |
---|
| 590 | AdepthMap = BdepthMap; |
---|
| 591 | ARayAll = BRayAll; |
---|
| 592 | ARayOri = BRayOri; |
---|
| 593 | ASup = BSup; |
---|
| 594 | ASupOri = BSupOri; |
---|
| 595 | AMedSup = BMedSup; |
---|
| 596 | ASupNeighborTable = BSupNeighborTable; |
---|
| 597 | AmaskSky = BmaskSky; |
---|
| 598 | AmaskG = BmaskG; |
---|
| 599 | AMultiScaleSupTable = BMultiScaleSupTable; |
---|
| 600 | Aconstrain.RayMatched = [ BRayMatched ]; |
---|
| 601 | Aconstrain.Depth_modified = [ BDepth_modified ]; |
---|
| 602 | Aconstrain.SupMatched = [ BSupMatched ]; |
---|
| 603 | AdepthMap = BDepth_normalized; |
---|
| 604 | save([ ScratchFolder '/' right '_NonMono.mat' ], 'AdepthMap', 'ARotation', 'ATranslation', 'AHistory', 'Aconstrain',... |
---|
| 605 | 'GroundY0inACoor',... |
---|
| 606 | 'ASup', 'ASupOri', 'AMedSup', 'ARayOri','ARayAll','ASupNeighborTable','AmaskSky','AmaskG','AMultiScaleSupTable'); |
---|
| 607 | |
---|
| 608 | |
---|
| 609 | % ======================================================================================= |
---|
| 610 | % generate the 3D model jointly |
---|
| 611 | % Aaxis = [ zeros(1,3); [0 0 10]; zeros(1,3); [0 10 0]]; |
---|
| 612 | % BCenter = T_simple_before_R; |
---|
| 613 | % Baxis = [ BCenter'; (R2'*[0 0 10]'+BCenter)'; BCenter'; (R2'*[0 10 0]'+BCenter)']; |
---|
| 614 | % DisplayPairPointsCloud(APositionAll(:,:)', (R2'*BPositionAll(:,:)+repmat( T_simple_before_R, 1, size(BPositionAll(:,:),2)))',... |
---|
| 615 | % Aaxis, Baxis, 300 ); |
---|
| 616 | % DisplayPairPointsCloud(APositionAll(:,:)'*ADepthScale, (R2'*BPositionAll(:,:)*BDepthScale+repmat( T_simple_before_R, 1, size(BPositionAll(:,:),2)))',... |
---|
| 617 | % Aaxis, Baxis, 400 ); |
---|
| 618 | |
---|
| 619 | |
---|
| 620 | |
---|
| 621 | %Psimple = Pnew*Hsimple; |
---|
| 622 | %Xsimple = inv(Hsimple)*Xnew; |
---|
| 623 | %DisplayMetricReconstruction( Psimple,Xsimple); |
---|
| 624 | %DisplayMetricReconstruction( Psimple,Xsimple); |
---|
| 625 | return; |
---|
| 626 | % ======================================================== |
---|