[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, 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; |
---|