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, T, lamda1, lamda2, inlier, Error] = EstPose( defaultPara, E, x, PriorDepthRatio, PriorR) |
---|
40 | |
---|
41 | % This function estimate the Pose ( R rotation, T translation) from E |
---|
42 | % After finding R and T, it also triangulate the depth(lamda) when |
---|
43 | % T is unit length |
---|
44 | % Input: |
---|
45 | % E - essential matrix (for calibrated camera) |
---|
46 | % x - calibrated matches point in both images |
---|
47 | % |
---|
48 | % Return |
---|
49 | % R - rotation matrix |
---|
50 | % T - translation unit vector |
---|
51 | % lamda1/2 - triangulated depth for image 1 and 2 when T is unit length |
---|
52 | |
---|
53 | LowThre = 0.2; |
---|
54 | [U S V] =svd(E); |
---|
55 | % pause |
---|
56 | Rz_pos = [ [0 -1 0];... |
---|
57 | [1 0 0];... |
---|
58 | [0 0 1]]; |
---|
59 | |
---|
60 | R1 = U*Rz_pos'*V'; |
---|
61 | T1 = U(:,end); |
---|
62 | |
---|
63 | R2 = U*Rz_pos*V'; |
---|
64 | T2 = -T1; |
---|
65 | |
---|
66 | % eight/four possible combination to be the solution. |
---|
67 | % check positive depth constrain |
---|
68 | lamda1 = []; |
---|
69 | lamda2 = []; |
---|
70 | R = []; |
---|
71 | T = []; |
---|
72 | % Building RPool |
---|
73 | count = 1; |
---|
74 | RPoolAll = {R1,R2,-R1,-R2}; |
---|
75 | for j = 1:length(RPoolAll) |
---|
76 | % test UpSide Dwon |
---|
77 | NewZ = RPoolAll{j}*[0; 1; 0]; |
---|
78 | if NewZ(2) > 0 |
---|
79 | if ~DetectImproperRotation(RPoolAll{j}) % test not improperRotation |
---|
80 | RPool{count} = RPoolAll{j}; |
---|
81 | count = count + 1; |
---|
82 | end |
---|
83 | end |
---|
84 | end |
---|
85 | TPool = {T1,T2}; |
---|
86 | |
---|
87 | count = 1; |
---|
88 | for j = 1:length(RPool) |
---|
89 | for k = 1:length(TPool) |
---|
90 | [lamda1Candidate{count}, lamda2Candidate{count} Error{count}] = triangulation( defaultPara, RPool{j}, TPool{k}, x); |
---|
91 | PositiveRatio(count) = sum(lamda1Candidate{count} >0 & lamda2Candidate{count} > 0) / size(lamda1Candidate{count}, 2); |
---|
92 | inlierM{count} = find(lamda1Candidate{count} > 0 & lamda2Candidate{count} > 0); |
---|
93 | count = count +1; |
---|
94 | end |
---|
95 | end |
---|
96 | |
---|
97 | % filter out some obvious bad matches |
---|
98 | IND = find( PositiveRatio > LowThre); |
---|
99 | if length(IND) ~= 1 % multiple choice cases |
---|
100 | if isempty(PriorR) |
---|
101 | |
---|
102 | if ~isempty(PriorDepthRatio) |
---|
103 | % choose the lamdaRatio close to PriorDepthRatio |
---|
104 | |
---|
105 | else |
---|
106 | [C I] = max( PositiveRatio( IND)); |
---|
107 | TempIND = find( PositiveRatio( IND) == C); |
---|
108 | if length(TempIND) > 1 |
---|
109 | MedainErro = median( cell2mat(Error( TempIND)'), 2); |
---|
110 | [Ct It] = min(MedainErro); |
---|
111 | I = IND( TempIND(It)); |
---|
112 | else |
---|
113 | I = IND( I); |
---|
114 | end |
---|
115 | end |
---|
116 | else |
---|
117 | % choose the R closer to PriorR |
---|
118 | [PriorRAxis, q] = Rotation2Q(PriorR); |
---|
119 | count = 1; |
---|
120 | for i = IND |
---|
121 | [RAxis(count,:), q] = Rotation2Q(RPool{ floor( (i-1)/length(TPool))+1}); |
---|
122 | count = count + 1; |
---|
123 | end |
---|
124 | DistRotationAxis = sqrt( sum( ( RAxis(:,1:3) - repmat(PriorRAxis(1,1:3), length(IND), 1)).^2, 2)); |
---|
125 | [Ctemp I] = min(DistRotationAxis); |
---|
126 | I = IND(I); |
---|
127 | end |
---|
128 | |
---|
129 | else |
---|
130 | I = IND; % single choice cases |
---|
131 | end |
---|
132 | |
---|
133 | if ~isempty(I) |
---|
134 | [i j] = ind2sub([ length(TPool) length(RPool)],I); |
---|
135 | R = RPool{j}; |
---|
136 | T = TPool{i}; |
---|
137 | inlier = inlierM{I}; |
---|
138 | lamda1 = lamda1Candidate{I}; |
---|
139 | lamda2 = lamda2Candidate{I}; |
---|
140 | Error = Error{I}; |
---|
141 | else |
---|
142 | R = []; |
---|
143 | T = []; |
---|
144 | inlier = []; |
---|
145 | lamda1 = []; |
---|
146 | lamda2 = []; |
---|
147 | Error = []; |
---|
148 | end |
---|
149 | |
---|
150 | if isempty(R) |
---|
151 | disp('all four solution failed'); |
---|
152 | end |
---|
153 | return; |
---|