[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 [R_ground_constrain T_mr D1_modified D2_modified] = MetricReconEstPose(P, R, T, T1, T2, D1, D2, x1, x2, defaultPara); |
---|
| 40 | |
---|
| 41 | % This function reconstruct the R_mr and T_mr from the P( camera matrix) |
---|
| 42 | % which most close to the original estimated R and T |
---|
| 43 | GroundStaticWeight = 50; |
---|
| 44 | ops = sdpsettings('solver','sedumi','verbose',1); |
---|
| 45 | |
---|
| 46 | P = [inv(T1)*P(1:3,:);... |
---|
| 47 | inv(T2)*P(4:6,:)]; |
---|
| 48 | |
---|
| 49 | P1 = P(1:3,:); |
---|
| 50 | P2 = P(4:6,:); |
---|
| 51 | [U S V] = svd(P1); |
---|
| 52 | H_last = V(:,end); |
---|
| 53 | H_4by3 = P1\defaultPara.InrinsicK1; |
---|
| 54 | |
---|
| 55 | K2simple_square = sdpvar(3,1); |
---|
| 56 | F = set(diag(K2simple_square) >=0); |
---|
| 57 | sol = solvesdp(F , norm( P2*H_4by3*H_4by3'*P2' - diag(K2simple_square),'fro'), ops); |
---|
| 58 | K2simple_square = double(K2simple_square); |
---|
| 59 | K2simple = sqrt(K2simple_square); |
---|
| 60 | R_est = diag( 1./K2simple)*P2*H_4by3; |
---|
| 61 | % R_est = P2*H_4by3; |
---|
| 62 | T_est = P2*H_last; |
---|
| 63 | |
---|
| 64 | % Reconstruct Rotation matrix by penalize rotation in other than z axis |
---|
| 65 | R_ground_constrain = sdpvar(3,3); |
---|
| 66 | sol =solvesdp( [], norm( R_ground_constrain - R_est,'fro') + GroundStaticWeight*norm([0 1 0]' - R_ground_constrain*[0 1 0]'), ops); |
---|
| 67 | R_ground_constrain = double( R_ground_constrain); |
---|
| 68 | R_ground_constrain = R_ground_constrain * (R_ground_constrain'*R_ground_constrain)^(-.5); |
---|
| 69 | |
---|
| 70 | % Reconstruct the translation matrix by using Mono-depth infomation |
---|
| 71 | Ray1 = inv(defaultPara.InrinsicK1)*x1; |
---|
| 72 | Ray2 = inv(defaultPara.InrinsicK2)*x2; |
---|
| 73 | T = sdpvar(3,1); |
---|
| 74 | D1_modified = sdpvar(1,length(D1)); |
---|
| 75 | D2_modified = sdpvar(1,length(D2)); |
---|
| 76 | D2scale = sdpvar(1); |
---|
| 77 | sol =solvesdp( [], norm( reshape( (Ray1.*repmat( D1_modified, 3,1))' - ... |
---|
| 78 | ( R_ground_constrain'*( Ray2.*repmat( D2_modified, 3, 1) ) + repmat(T,1,length(D1)) )' ,1,[]),1)+... |
---|
| 79 | norm(D1_modified - D1,1) + norm(D2_modified - D2scale*D2,1), ops); |
---|
| 80 | T_mr = double(T); |
---|
| 81 | D1_modified = double(D1_modified); |
---|
| 82 | D2_modified = double(D2_modified); |
---|
| 83 | D2scale = double(D2scale); |
---|
| 84 | |
---|
| 85 | return; |
---|