[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 [ Rc1_2, Rc2_1, ConS1_2, ConS2_1, RoughConS1_2, RoughConS2_1]=CalMatchSearchRegin(defaultPara, R, T, I1, I2, x1, x2, D1, D2, FlagRotate, FlagDisp); |
---|
| 40 | |
---|
| 41 | % This function Calculate the Constrain of SurfMatch search space give |
---|
| 42 | % Estimated Rotation and Translation matrix and depth |
---|
| 43 | % in the reference camera coordinate |
---|
| 44 | |
---|
| 45 | % initialize Parameter |
---|
| 46 | NegativeDepthTolerence = defaultPara.NegativeDepthTolerence; |
---|
| 47 | MaxRatio =defaultPara.MaxRatio;%300; |
---|
| 48 | MinRatio = 1/MaxRatio; |
---|
| 49 | R1_2 = R(1:3,:); |
---|
| 50 | R2_1 = R(4:6,:); |
---|
| 51 | T1_2 = T(1:3,:); |
---|
| 52 | T2_1 = T(4:6,:); |
---|
| 53 | K1 = size(x1,2); |
---|
| 54 | K2 = size(x2,2); |
---|
| 55 | x1 = [x1; ones(1,K1)]; |
---|
| 56 | x2 = [x2; ones(1,K2)]; |
---|
| 57 | %D1 = D(1,:); |
---|
| 58 | %D2 = D(2,:); |
---|
| 59 | |
---|
| 60 | % I1 project on I2 ==========================================% use zeros depth constraon to restrict the searching area more |
---|
| 61 | [V H] = size(I2); |
---|
| 62 | MaxD1 = min(D1*MaxRatio,defaultPara.FarestDist); |
---|
| 63 | MinD1 = max(D1*MinRatio,defaultPara.Closestdist); |
---|
| 64 | % calculate the prespective depth gives 0 prejective depth in camera 2 |
---|
| 65 | x1_2R_z = R1_2(3,:)*inv(defaultPara.InrinsicK1)*(x1); % it's possible that x1_2R_z is zero |
---|
| 66 | mark = x1_2R_z ~=0; |
---|
| 67 | Depth_zero_ca2 = zeros(1, length(x1_2R_z)); |
---|
| 68 | Depth_one_ca2 = zeros(1, length(x1_2R_z)); |
---|
| 69 | Depth_zero_ca2(mark) = T1_2(3)./x1_2R_z(mark); % 0 (zero depth in camera 2 coordinate) = A*depth + T(3); solve depth |
---|
| 70 | Depth_one_ca2(mark) = (1-T1_2(3))./x1_2R_z(mark); % 1 (zero depth in camera 2 coordinate) = A*depth + T(3); solve depth |
---|
| 71 | |
---|
| 72 | % use zeros depth constraon to restrict the searching area more |
---|
| 73 | mark0_1 = Depth_zero_ca2 >= Depth_one_ca2; |
---|
| 74 | mark1_0 = ~mark0_1; |
---|
| 75 | |
---|
| 76 | tempDepth_one_ca2 = Depth_one_ca2(mark0_1); |
---|
| 77 | tempDepth_zero_ca2 = Depth_zero_ca2(mark0_1); |
---|
| 78 | tempMax = MaxD1(mark0_1); |
---|
| 79 | tempMin = MinD1(mark0_1); |
---|
| 80 | markMaxBigerOneDepthCa2 = tempMax > tempDepth_one_ca2; |
---|
| 81 | markMinBigerOneDepthCa2 = tempMin > tempDepth_one_ca2; % markMinBigerOneDepthCa2 : is one kind of outlier |
---|
| 82 | tempMax(markMaxBigerOneDepthCa2) = tempDepth_one_ca2(markMaxBigerOneDepthCa2); |
---|
| 83 | tempMax(markMinBigerOneDepthCa2) = tempDepth_zero_ca2(markMinBigerOneDepthCa2);%NaN; % indicator of outliers |
---|
| 84 | tempMin(markMinBigerOneDepthCa2) = tempDepth_one_ca2(markMinBigerOneDepthCa2); % NaN indicator of outliers |
---|
| 85 | MaxD1(mark0_1) = tempMax; |
---|
| 86 | MinD1(mark0_1) = tempMin; |
---|
| 87 | MaxD1(mark) = min(D1(mark)*MaxRatio,defaultPara.FarestDist);%Min529 new |
---|
| 88 | MinD1(mark) = max(D1(mark)*MinRatio,defaultPara.Closestdist);%Min529 new |
---|
| 89 | |
---|
| 90 | |
---|
| 91 | tempDepth_one_ca2 = Depth_one_ca2(mark1_0); |
---|
| 92 | tempDepth_zero_ca2 = Depth_zero_ca2(mark1_0); |
---|
| 93 | tempMax = MaxD1(mark1_0); |
---|
| 94 | tempMin = MinD1(mark1_0); |
---|
| 95 | markMaxBigerOneDepthCa2 = tempMax < tempDepth_one_ca2; % markMaxBigerOneDepthCa2 : is one kind of outlier |
---|
| 96 | markMinBigerOneDepthCa2 = tempMin < tempDepth_one_ca2; |
---|
| 97 | tempMax(markMaxBigerOneDepthCa2) = tempDepth_one_ca2( markMaxBigerOneDepthCa2);%NaN; |
---|
| 98 | tempMin(markMinBigerOneDepthCa2) = tempDepth_one_ca2( markMinBigerOneDepthCa2); |
---|
| 99 | tempMin(markMaxBigerOneDepthCa2) = tempDepth_zero_ca2( markMaxBigerOneDepthCa2);%NaN; |
---|
| 100 | MaxD1(mark1_0) = tempMax; |
---|
| 101 | MinD1(mark1_0) = tempMin; |
---|
| 102 | |
---|
| 103 | % calculate the projection position |
---|
| 104 | x1CaMax3D = inv(defaultPara.InrinsicK1)*(x1.*repmat(MaxD1,3,1)); % 3-D position in camera 1 coordinate (3 by n) |
---|
| 105 | x1CaMin3D = inv(defaultPara.InrinsicK1)*(x1.*repmat(MinD1,3,1)); % 3-D position in camera 1 coordinate (3 by n) |
---|
| 106 | x1CaMaxHomo = [ x1CaMax3D; ones(1,K1)]; % into homogenous coordinate (4 by n) |
---|
| 107 | x1CaMinHomo = [ x1CaMin3D; ones(1,K1)]; % into homogenous coordinate (4 by n) |
---|
| 108 | x1_2Max3D = [R1_2 T1_2]*x1CaMaxHomo; % 3-D position in camera 2 coordinate (3 by n) |
---|
| 109 | x1_2MaxHomo = defaultPara.InrinsicK2*x1_2Max3D; % image homo coordinate in camera2 (3 by n) |
---|
| 110 | x1_2Max = [ x1_2MaxHomo(1,:)./x1_2MaxHomo(3,:); x1_2MaxHomo(2,:)./x1_2MaxHomo(3,:)]; % image coordinate (2 by n) |
---|
| 111 | x1_2Min3D = [R1_2 T1_2]*x1CaMinHomo; % 3-D position in camera 2 coordinate (3 by n) |
---|
| 112 | x1_2MinHomo = defaultPara.InrinsicK2*x1_2Min3D; % image homo coordinate in camera2 (3 by n) |
---|
| 113 | x1_2Min = [ x1_2MinHomo(1,:)./x1_2MinHomo(3,:); x1_2MinHomo(2,:)./x1_2MinHomo(3,:)]; % image coordinate (2 by n) |
---|
| 114 | % x1_2Max(mark0_1) = x1_2Max(mark0_1) + (x1_2Max(mark0_1) - x1_2Min(mark0_1))*NegativeDepthTolerence;%Min529 |
---|
| 115 | % x1_2Min(mark1_0) = x1_2Min(mark1_0) + (x1_2Min(mark1_0) - x1_2Max(mark1_0))*NegativeDepthTolerence;%Min529 |
---|
| 116 | x1_2Max = x1_2Max + (x1_2Max - x1_2Min)*NegativeDepthTolerence;%Min529 |
---|
| 117 | x1_2Min = x1_2Min + (x1_2Min - x1_2Max)*NegativeDepthTolerence;%Min529 |
---|
| 118 | |
---|
| 119 | % Define Constrain (simple rectangle) |
---|
| 120 | if FlagRotate |
---|
| 121 | tRAN = x1_2Max - x1_2Min; |
---|
| 122 | Ptr = tRAN(1,:) < 0; |
---|
| 123 | %theta_z = -atan(tRAN(2,:)./tRAN(1,:)); |
---|
| 124 | theta_z = atan(tRAN(2,:)./tRAN(1,:)); |
---|
| 125 | theta_z(Ptr) = theta_z(Ptr)+pi; |
---|
| 126 | Rc1_2 = [cos(-theta_z); -sin(-theta_z); sin(-theta_z); cos(-theta_z)]; |
---|
| 127 | ConS1_2(1:2,:) = x1_2Min; |
---|
| 128 | ConS1_2(3,:) = sum( Rc1_2(1:2,:).*tRAN,1);% also may check sum( Rc1_2(3:4,:).*tRAN,1) close up to zeros to verify correct or not |
---|
| 129 | % Re1_2 = sum( Rc1_2(3:4,:).*tRAN,1) |
---|
| 130 | |
---|
| 131 | ConS1_2(4,:) = defaultPara.VertVar*max(H,V); |
---|
| 132 | % [x_origon; y_origin; x_bound, y_bound(defaultPara.VertVar*max(H,V))] |
---|
| 133 | |
---|
| 134 | % generate plot point |
---|
| 135 | tempConS1_2 = [ ConS1_2(3,:); ConS1_2(4,:); ... |
---|
| 136 | zeros(1,size(ConS1_2,2)); ConS1_2(4,:);... |
---|
| 137 | zeros(1,size(ConS1_2,2)); -ConS1_2(4,:);... |
---|
| 138 | ConS1_2(3,:); -ConS1_2(4,:)]; |
---|
| 139 | %Rc1_2_transpose = [cos(theta_z); sin(theta_z); -sin(theta_z); cos(theta_z)]; |
---|
| 140 | Rc1_2_transpose = [cos(theta_z); -sin(theta_z); sin(theta_z); cos(theta_z)]; |
---|
| 141 | tempConS1_2(1:2,:) = [sum( Rc1_2_transpose(1:2,:).*tempConS1_2(1:2,:), 1); ... |
---|
| 142 | sum( Rc1_2_transpose(3:4,:).*tempConS1_2(1:2,:), 1)]; |
---|
| 143 | tempConS1_2(3:4,:) = [sum( Rc1_2_transpose(1:2,:).*tempConS1_2(3:4,:), 1); ... |
---|
| 144 | sum( Rc1_2_transpose(3:4,:).*tempConS1_2(3:4,:), 1)]; |
---|
| 145 | tempConS1_2(5:6,:) = [sum( Rc1_2_transpose(1:2,:).*tempConS1_2(5:6,:), 1); ... |
---|
| 146 | sum( Rc1_2_transpose(3:4,:).*tempConS1_2(5:6,:), 1)]; |
---|
| 147 | tempConS1_2(7:8,:) = [sum( Rc1_2_transpose(1:2,:).*tempConS1_2(7:8,:), 1); ... |
---|
| 148 | sum( Rc1_2_transpose(3:4,:).*tempConS1_2(7:8,:), 1)]; |
---|
| 149 | tempConS1_2 = tempConS1_2 + repmat( x1_2Min, 4, 1); |
---|
| 150 | RoughConS1_2 = [ min( tempConS1_2([ 1 3 5 7],:), [], 1); max( tempConS1_2([ 1 3 5 7],:), [], 1);... |
---|
| 151 | min( tempConS1_2([ 2 4 6 8],:), [], 1); max( tempConS1_2([ 2 4 6 8],:), [], 1)]; |
---|
| 152 | % tempConS1_2([1 3 5 7],:) = min(max(tempConS1_2([1 3 5 7],:), 1), H); |
---|
| 153 | % tempConS1_2([1 3 5 7]+1,:) = min(max(tempConS1_2([1 3 5 7]+1,:), 1), V); |
---|
| 154 | else |
---|
| 155 | Rc1_2 =[]; |
---|
| 156 | ConS1_2(1,:) = min([ x1_2Max(1,:)+defaultPara.VertVar*H; x1_2Max(1,:)-defaultPara.VertVar*H;... |
---|
| 157 | x1_2Min(1,:)+defaultPara.VertVar*H; x1_2Min(1,:)-defaultPara.VertVar*H ],[],1); |
---|
| 158 | ConS1_2(1,:) = min( max( round(ConS1_2(1,:)), 1), H); |
---|
| 159 | ConS1_2(2,:) = max([ x1_2Max(1,:)+defaultPara.VertVar*H; x1_2Max(1,:)-defaultPara.VertVar*H;... |
---|
| 160 | x1_2Min(1,:)+defaultPara.VertVar*H; x1_2Min(1,:)-defaultPara.VertVar*H ],[],1); |
---|
| 161 | ConS1_2(2,:) = min( max( round(ConS1_2(2,:)), 1), H); |
---|
| 162 | ConS1_2(3,:) = min([ x1_2Max(2,:)+defaultPara.VertVar*V; x1_2Max(2,:)-defaultPara.VertVar*V;... |
---|
| 163 | x1_2Min(2,:)+defaultPara.VertVar*V; x1_2Min(2,:)-defaultPara.VertVar*V ],[],1); |
---|
| 164 | ConS1_2(3,:) = min( max( round(ConS1_2(3,:)), 1), V); |
---|
| 165 | ConS1_2(4,:) = max([ x1_2Max(2,:)+defaultPara.VertVar*V; x1_2Max(2,:)-defaultPara.VertVar*V;... |
---|
| 166 | x1_2Min(2,:)+defaultPara.VertVar*V; x1_2Min(2,:)-defaultPara.VertVar*V ],[],1); |
---|
| 167 | ConS1_2(4,:) = min( max( round(ConS1_2(4,:)), 1), V); |
---|
| 168 | end |
---|
| 169 | |
---|
| 170 | % =========================================================== |
---|
| 171 | |
---|
| 172 | [V H] = size(I1); |
---|
| 173 | % I2 project on I1 |
---|
| 174 | MaxD2 = D2*MaxRatio; |
---|
| 175 | MinD2 = D2*MinRatio; |
---|
| 176 | % calculate the prespective depth gives 0 prejective depth in camera 2 |
---|
| 177 | x2_1R_z = R2_1(3,:)*inv(defaultPara.InrinsicK2)*(x2);% it's possible that x1_2R_z is zero |
---|
| 178 | mark = x2_1R_z ~=0; |
---|
| 179 | Depth_zero_ca2 = zeros(1, length(x2_1R_z)); |
---|
| 180 | Depth_one_ca2 = zeros(1, length(x2_1R_z)); |
---|
| 181 | Depth_zero_ca2(mark) = T2_1(3)./x2_1R_z(mark); |
---|
| 182 | Depth_one_ca2(mark) = (1-T2_1(3))./x2_1R_z(mark); |
---|
| 183 | |
---|
| 184 | % use zeros depth constraon to restrict the searching area more |
---|
| 185 | mark0_1 = Depth_zero_ca2 >= Depth_one_ca2; |
---|
| 186 | mark1_0 = ~mark0_1; |
---|
| 187 | |
---|
| 188 | tempDepth_one_ca2 = Depth_one_ca2(mark0_1); |
---|
| 189 | tempDepth_zero_ca2 = Depth_zero_ca2(mark0_1); |
---|
| 190 | tempMax = MaxD2(mark0_1); |
---|
| 191 | tempMin = MinD2(mark0_1); |
---|
| 192 | markMaxBigerOneDepthCa2 = tempMax > tempDepth_one_ca2; |
---|
| 193 | markMinBigerOneDepthCa2 = tempMin > tempDepth_one_ca2; % markMinBigerOneDepthCa2 : is one kind of outlier |
---|
| 194 | tempMax(markMaxBigerOneDepthCa2) = tempDepth_one_ca2(markMaxBigerOneDepthCa2); |
---|
| 195 | tempMax(markMinBigerOneDepthCa2) = tempDepth_zero_ca2(markMinBigerOneDepthCa2);%NaN; % indicator of outliers |
---|
| 196 | tempMin(markMinBigerOneDepthCa2) = tempDepth_one_ca2(markMinBigerOneDepthCa2);%NaN; % indicator of outliers |
---|
| 197 | MaxD2(mark0_1) = tempMax; |
---|
| 198 | MinD2(mark0_1) = tempMin; |
---|
| 199 | MaxD2(mark) = min(D2(mark)*MaxRatio,defaultPara.FarestDist);%Min529 new |
---|
| 200 | MinD2(mark) = max(D2(mark)*MinRatio,defaultPara.Closestdist);%Min529 new |
---|
| 201 | |
---|
| 202 | tempDepth_one_ca2 = Depth_one_ca2(mark1_0); |
---|
| 203 | tempDepth_zero_ca2 = Depth_zero_ca2(mark1_0); |
---|
| 204 | tempMax = MaxD2(mark1_0); |
---|
| 205 | tempMin = MinD2(mark1_0); |
---|
| 206 | markMaxBigerOneDepthCa2 = tempMax < tempDepth_one_ca2; % markMaxBigerOneDepthCa2 : is one kind of outlier |
---|
| 207 | markMinBigerOneDepthCa2 = tempMin < tempDepth_one_ca2; |
---|
| 208 | tempMax(markMaxBigerOneDepthCa2) = tempDepth_one_ca2( markMaxBigerOneDepthCa2);%NaN; |
---|
| 209 | tempMin(markMinBigerOneDepthCa2) = tempDepth_one_ca2( markMinBigerOneDepthCa2); |
---|
| 210 | tempMin(markMaxBigerOneDepthCa2) = tempDepth_zero_ca2( markMaxBigerOneDepthCa2);%NaN; |
---|
| 211 | MaxD2(mark1_0) = tempMax; |
---|
| 212 | MinD2(mark1_0) = tempMin; |
---|
| 213 | if any( MaxD2< MinD2) |
---|
| 214 | disp('error') |
---|
| 215 | end |
---|
| 216 | % calculate the projection position |
---|
| 217 | x2CaMax3D = inv(defaultPara.InrinsicK2)*(x2.*repmat(MaxD2,3,1)); % 3-D position in camera 2 coordinate (3 by n) |
---|
| 218 | x2CaMin3D = inv(defaultPara.InrinsicK2)*(x2.*repmat(MinD2,3,1)); % 3-D position in camera 2 coordinate (3 by n) |
---|
| 219 | x2CaMaxHomo = [ x2CaMax3D; ones(1,K2)]; % into homogenous coordinate (4 by n) |
---|
| 220 | x2CaMinHomo = [ x2CaMin3D; ones(1,K2)]; % into homogenous coordinate (4 by n) |
---|
| 221 | x2_1Max3D = [R2_1 T2_1]*x2CaMaxHomo; % 3-D position in camera 1 coordinate (3 by n) |
---|
| 222 | x2_1MaxHomo = defaultPara.InrinsicK1*x2_1Max3D; % image homo coordinate in camera1 (3 by n) |
---|
| 223 | x2_1Max = [ x2_1MaxHomo(1,:)./x2_1MaxHomo(3,:); x2_1MaxHomo(2,:)./x2_1MaxHomo(3,:)]; % image coordinate (2 by n) |
---|
| 224 | x2_1Min3D = [R2_1 T2_1]*x2CaMinHomo; % 3-D position in camera 1 coordinate (3 by n) |
---|
| 225 | x2_1MinHomo = defaultPara.InrinsicK1*x2_1Min3D; % image homo coordinate in camera1 (3 by n) |
---|
| 226 | x2_1Min = [ x2_1MinHomo(1,:)./x2_1MinHomo(3,:); x2_1MinHomo(2,:)./x2_1MinHomo(3,:)]; % image coordinate (2 by n) |
---|
| 227 | % x2_1Max(mark0_1) = x2_1Max(mark0_1) + (x2_1Max(mark0_1) - x2_1Min(mark0_1))*NegativeDepthTolerence;%Min529 |
---|
| 228 | % x2_1Min(mark1_0) = x2_1Min(mark1_0) + (x2_1Min(mark1_0) - x2_1Max(mark1_0))*NegativeDepthTolerence;%Min529 |
---|
| 229 | x2_1Max = x2_1Max + (x2_1Max - x2_1Min)*NegativeDepthTolerence;%Min529 |
---|
| 230 | x2_1Min = x2_1Min + (x2_1Min - x2_1Max)*NegativeDepthTolerence;%Min529 |
---|
| 231 | % Define Constrain (simple rectangle) |
---|
| 232 | if FlagRotate |
---|
| 233 | tRAN = x2_1Max - x2_1Min; |
---|
| 234 | Ptr = tRAN(1,:) < 0; |
---|
| 235 | %theta_z = -atan(tRAN(2,:)./tRAN(1,:)); |
---|
| 236 | theta_z = atan(tRAN(2,:)./tRAN(1,:)); |
---|
| 237 | theta_z(Ptr) = theta_z(Ptr)+pi; |
---|
| 238 | Rc2_1 = [cos(-theta_z); -sin(-theta_z); sin(-theta_z); cos(-theta_z)]; |
---|
| 239 | ConS2_1(1:2,:) = x2_1Min; |
---|
| 240 | ConS2_1(3,:) = [sum( Rc2_1(1:2,:).*tRAN,1)]; |
---|
| 241 | % Re2_1 = sum( Rc2_1(3:4,:).*tRAN,1) |
---|
| 242 | |
---|
| 243 | ConS2_1(4,:) = defaultPara.VertVar*max(H,V); |
---|
| 244 | % [x_origon; y_origin; x_bound, y_bound(defaultPara.VertVar*max(H,V))] |
---|
| 245 | |
---|
| 246 | % generate plot point |
---|
| 247 | tempConS2_1 = [ ConS2_1(3,:); ConS2_1(4,:); ... |
---|
| 248 | zeros(1,size(ConS2_1,2)); ConS2_1(4,:);... |
---|
| 249 | zeros(1,size(ConS2_1,2)); -ConS2_1(4,:);... |
---|
| 250 | ConS2_1(3,:); -ConS2_1(4,:)]; |
---|
| 251 | % Rc2_1_transpose = [cos(theta_z); -sin(theta_z); sin(theta_z); cos(theta_z)]; |
---|
| 252 | Rc2_1_transpose = [cos(theta_z); -sin(theta_z); sin(theta_z); cos(theta_z)]; |
---|
| 253 | tempConS2_1(1:2,:) = [sum( Rc2_1_transpose(1:2,:).*tempConS2_1(1:2,:), 1); ... |
---|
| 254 | sum( Rc2_1_transpose(3:4,:).*tempConS2_1(1:2,:), 1)]; |
---|
| 255 | tempConS2_1(3:4,:) = [sum( Rc2_1_transpose(1:2,:).*tempConS2_1(3:4,:), 1); ... |
---|
| 256 | sum( Rc2_1_transpose(3:4,:).*tempConS2_1(3:4,:), 1)]; |
---|
| 257 | tempConS2_1(5:6,:) = [sum( Rc2_1_transpose(1:2,:).*tempConS2_1(5:6,:), 1); ... |
---|
| 258 | sum( Rc2_1_transpose(3:4,:).*tempConS2_1(5:6,:), 1)]; |
---|
| 259 | tempConS2_1(7:8,:) = [sum( Rc2_1_transpose(1:2,:).*tempConS2_1(7:8,:), 1); ... |
---|
| 260 | sum( Rc2_1_transpose(3:4,:).*tempConS2_1(7:8,:), 1)]; |
---|
| 261 | tempConS2_1 = tempConS2_1 + repmat( x2_1Min, 4, 1); |
---|
| 262 | RoughConS2_1 = [ min( tempConS2_1([ 1 3 5 7],:), [], 1); max( tempConS2_1([ 1 3 5 7],:), [], 1);... |
---|
| 263 | min( tempConS2_1([ 2 4 6 8],:), [], 1); max( tempConS2_1([ 2 4 6 8],:), [], 1)]; |
---|
| 264 | % tempConS2_1([1 3 5 7],:) = min(max(tempConS2_1([1 3 5 7],:), 1), H); |
---|
| 265 | % tempConS2_1([1 3 5 7]+1,:) = min(max(tempConS2_1([1 3 5 7]+1,:), 1), V); |
---|
| 266 | else |
---|
| 267 | Rc2_1 = []; |
---|
| 268 | ConS2_1(1,:) = min([ x2_1Max(1,:)+defaultPara.VertVar*H; x2_1Max(1,:)-defaultPara.VertVar*H;... |
---|
| 269 | x2_1Min(1,:)+defaultPara.VertVar*H; x2_1Min(1,:)-defaultPara.VertVar*H ],[],1); |
---|
| 270 | ConS2_1(1,:) = min( max( round(ConS2_1(1,:)), 1), H); |
---|
| 271 | ConS2_1(2,:) = max([ x2_1Max(1,:)+defaultPara.VertVar*H; x2_1Max(1,:)-defaultPara.VertVar*H;... |
---|
| 272 | x2_1Min(1,:)+defaultPara.VertVar*H; x2_1Min(1,:)-defaultPara.VertVar*H ],[],1); |
---|
| 273 | ConS2_1(2,:) = min( max( round(ConS2_1(2,:)), 1), H); |
---|
| 274 | ConS2_1(3,:) = min([ x2_1Max(2,:)+defaultPara.VertVar*V; x2_1Max(2,:)-defaultPara.VertVar*V;... |
---|
| 275 | x2_1Min(2,:)+defaultPara.VertVar*V; x2_1Min(2,:)-defaultPara.VertVar*V ],[],1); |
---|
| 276 | ConS2_1(3,:) = min( max( round(ConS2_1(3,:)), 1), V); |
---|
| 277 | ConS2_1(4,:) = max([ x2_1Max(2,:)+defaultPara.VertVar*V; x2_1Max(2,:)-defaultPara.VertVar*V;... |
---|
| 278 | x2_1Min(2,:)+defaultPara.VertVar*V; x2_1Min(2,:)-defaultPara.VertVar*V ],[],1); |
---|
| 279 | ConS2_1(4,:) = min( max( round(ConS2_1(4,:)), 1), V); |
---|
| 280 | end |
---|
| 281 | |
---|
| 282 | % ========================================================== |
---|
| 283 | if FlagDisp |
---|
| 284 | T1_2_hat = [[0 -T1_2(3) T1_2(2)];... |
---|
| 285 | [T1_2(3) 0 -T1_2(1)];... |
---|
| 286 | [-T1_2(2) T1_2(1) 0]]; |
---|
| 287 | F = inv(defaultPara.InrinsicK2)'*T1_2_hat*R1_2*inv(defaultPara.InrinsicK1); |
---|
| 288 | if FlagRotate |
---|
| 289 | figure; |
---|
| 290 | dispMatchSearchRegin(I1, I2, x1, x2, tempConS1_2, tempConS2_1, F, ... |
---|
| 291 | x1_2Max, MaxD1, x1_2Min, MinD1, ... |
---|
| 292 | x2_1Max, MaxD2, x2_1Min, MinD2, ... |
---|
| 293 | FlagRotate, 'Stacking', 'v', 'Interactive', 0); |
---|
| 294 | else |
---|
| 295 | figure; |
---|
| 296 | dispMatchSearchRegin(I1, I2, x1, x2, ConS1_2, ConS2_1, F, FlagRotate, 'Stacking', 'v', 'Interactive', 0); |
---|
| 297 | end |
---|
| 298 | end |
---|
| 299 | |
---|
| 300 | return; |
---|