1 /* 2 * Copyright (C) 2011 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 /* $Id: db_image_homography.cpp,v 1.2 2011/06/17 14:03:31 mbansal Exp $ */ 18 19 #include "db_utilities.h" 20 #include "db_image_homography.h" 21 #include "db_framestitching.h" 22 #include "db_metrics.h" 23 24 25 26 /***************************************************************** 27 * Lean and mean begins here * 28 *****************************************************************/ 29 30 /*Compute the linear constraint on H obtained by requiring that the 31 ratio between coordinate i_num and i_den of xp is equal to the ratio 32 between coordinate i_num and i_den of Hx. i_zero should be set to 33 the coordinate not equal to i_num or i_den. No normalization is used*/ 34 inline void db_SProjImagePointPointConstraint(double c[9],int i_num,int i_den,int i_zero, 35 double xp[3],double x[3]) 36 { 37 db_MultiplyScalarCopy3(c+3*i_den,x, xp[i_num]); 38 db_MultiplyScalarCopy3(c+3*i_num,x, -xp[i_den]); 39 db_Zero3(c+3*i_zero); 40 } 41 42 /*Compute two constraints on H generated by the correspondence (Xp,X), 43 assuming that Xp ~= H*X. No normalization is used*/ 44 inline void db_SProjImagePointPointConstraints(double c1[9],double c2[9],double xp[3],double x[3]) 45 { 46 int ma_ind; 47 48 /*Find index of coordinate of Xp with largest absolute value*/ 49 ma_ind=db_MaxAbsIndex3(xp); 50 51 /*Generate 2 constraints, 52 each constraint is generated by considering the ratio between a 53 coordinate and the largest absolute value coordinate*/ 54 switch(ma_ind) 55 { 56 case 0: 57 db_SProjImagePointPointConstraint(c1,1,0,2,xp,x); 58 db_SProjImagePointPointConstraint(c2,2,0,1,xp,x); 59 break; 60 case 1: 61 db_SProjImagePointPointConstraint(c1,0,1,2,xp,x); 62 db_SProjImagePointPointConstraint(c2,2,1,0,xp,x); 63 break; 64 default: 65 db_SProjImagePointPointConstraint(c1,0,2,1,xp,x); 66 db_SProjImagePointPointConstraint(c2,1,2,0,xp,x); 67 } 68 } 69 70 inline void db_SAffineImagePointPointConstraints(double c1[7],double c2[7],double xp[3],double x[3]) 71 { 72 double ct1[9],ct2[9]; 73 74 db_SProjImagePointPointConstraints(ct1,ct2,xp,x); 75 db_Copy6(c1,ct1); c1[6]=ct1[8]; 76 db_Copy6(c2,ct2); c2[6]=ct2[8]; 77 } 78 79 void db_StitchProjective2D_4Points(double H[9], 80 double x1[3],double x2[3],double x3[3],double x4[3], 81 double xp1[3],double xp2[3],double xp3[3],double xp4[3]) 82 { 83 double c[72]; 84 85 /*Collect the constraints*/ 86 db_SProjImagePointPointConstraints(c ,c+9 ,xp1,x1); 87 db_SProjImagePointPointConstraints(c+18,c+27,xp2,x2); 88 db_SProjImagePointPointConstraints(c+36,c+45,xp3,x3); 89 db_SProjImagePointPointConstraints(c+54,c+63,xp4,x4); 90 /*Solve for the nullvector*/ 91 db_NullVector8x9Destructive(H,c); 92 } 93 94 void db_StitchAffine2D_3Points(double H[9], 95 double x1[3],double x2[3],double x3[3], 96 double xp1[3],double xp2[3],double xp3[3]) 97 { 98 double c[42]; 99 100 /*Collect the constraints*/ 101 db_SAffineImagePointPointConstraints(c ,c+7 ,xp1,x1); 102 db_SAffineImagePointPointConstraints(c+14,c+21,xp2,x2); 103 db_SAffineImagePointPointConstraints(c+28,c+35,xp3,x3); 104 /*Solve for the nullvector*/ 105 db_NullVector6x7Destructive(H,c); 106 db_MultiplyScalar6(H,db_SafeReciprocal(H[6])); 107 H[6]=H[7]=0; H[8]=1.0; 108 } 109 110 /*Compute up to three solutions for the focal length given two point correspondences 111 generated by a rotation with a common unknown focal length. No specific normalization 112 of the input points is required. If signed_disambiguation is true, the points are 113 required to be in front of the camera*/ 114 inline void db_CommonFocalLengthFromRotation_2Point(double fsol[3],int *nr_sols,double x1[3],double x2[3],double xp1[3],double xp2[3],int signed_disambiguation=1) 115 { 116 double m,ax,ay,apx,apy,bx,by,bpx,bpy; 117 double p1[2],p2[2],p3[2],p4[2],p5[2],p6[2]; 118 double p7[3],p8[4],p9[5],p10[3],p11[4]; 119 double roots[3]; 120 int nr_roots,i,j; 121 122 /*Solve for focal length using the equation 123 <a,b>^2*<ap,ap><bp,bp>=<ap,bp>^2*<a,a><b,b> 124 where a and ap are the homogenous vectors in the first image 125 after focal length scaling and b,bp are the vectors in the 126 second image*/ 127 128 /*Normalize homogenous coordinates so that last coordinate is one*/ 129 m=db_SafeReciprocal(x1[2]); 130 ax=x1[0]*m; 131 ay=x1[1]*m; 132 m=db_SafeReciprocal(xp1[2]); 133 apx=xp1[0]*m; 134 apy=xp1[1]*m; 135 m=db_SafeReciprocal(x2[2]); 136 bx=x2[0]*m; 137 by=x2[1]*m; 138 m=db_SafeReciprocal(xp2[2]); 139 bpx=xp2[0]*m; 140 bpy=xp2[1]*m; 141 142 /*Compute cubic in l=1/(f^2) 143 by dividing out the root l=0 from the equation 144 (l(ax*bx+ay*by)+1)^2*(l(apx^2+apy^2)+1)*(l(bpx^2+bpy^2)+1)= 145 (l(apx*bpx+apy*bpy)+1)^2*(l(ax^2+ay^2)+1)*(l(bx^2+by^2)+1)*/ 146 p1[1]=ax*bx+ay*by; 147 p2[1]=db_sqr(apx)+db_sqr(apy); 148 p3[1]=db_sqr(bpx)+db_sqr(bpy); 149 p4[1]=apx*bpx+apy*bpy; 150 p5[1]=db_sqr(ax)+db_sqr(ay); 151 p6[1]=db_sqr(bx)+db_sqr(by); 152 p1[0]=p2[0]=p3[0]=p4[0]=p5[0]=p6[0]=1; 153 154 db_MultiplyPoly1_1(p7,p1,p1); 155 db_MultiplyPoly1_2(p8,p2,p7); 156 db_MultiplyPoly1_3(p9,p3,p8); 157 158 db_MultiplyPoly1_1(p10,p4,p4); 159 db_MultiplyPoly1_2(p11,p5,p10); 160 db_SubtractPolyProduct1_3(p9,p6,p11); 161 /*Cubic starts at p9[1]*/ 162 db_SolveCubic(roots,&nr_roots,p9[4],p9[3],p9[2],p9[1]); 163 164 for(j=0,i=0;i<nr_roots;i++) 165 { 166 if(roots[i]>0) 167 { 168 if((!signed_disambiguation) || (db_PolyEval1(p1,roots[i])*db_PolyEval1(p4,roots[i])>0)) 169 { 170 fsol[j++]=db_SafeSqrtReciprocal(roots[i]); 171 } 172 } 173 } 174 *nr_sols=j; 175 } 176 177 int db_StitchRotationCommonFocalLength_3Points(double H[9],double x1[3],double x2[3],double x3[3],double xp1[3],double xp2[3],double xp3[3],double *f,int signed_disambiguation) 178 { 179 double fsol[3]; 180 int nr_sols,i,best_sol,done; 181 double cost,best_cost; 182 double m,hyp[27],x1_temp[3],x2_temp[3],xp1_temp[3],xp2_temp[3]; 183 double *hyp_point,ft; 184 double y[2]; 185 186 db_CommonFocalLengthFromRotation_2Point(fsol,&nr_sols,x1,x2,xp1,xp2,signed_disambiguation); 187 if(nr_sols) 188 { 189 db_DeHomogenizeImagePoint(y,xp3); 190 done=0; 191 for(i=0;i<nr_sols;i++) 192 { 193 ft=fsol[i]; 194 m=db_SafeReciprocal(ft); 195 x1_temp[0]=x1[0]*m; 196 x1_temp[1]=x1[1]*m; 197 x1_temp[2]=x1[2]; 198 x2_temp[0]=x2[0]*m; 199 x2_temp[1]=x2[1]*m; 200 x2_temp[2]=x2[2]; 201 xp1_temp[0]=xp1[0]*m; 202 xp1_temp[1]=xp1[1]*m; 203 xp1_temp[2]=xp1[2]; 204 xp2_temp[0]=xp2[0]*m; 205 xp2_temp[1]=xp2[1]*m; 206 xp2_temp[2]=xp2[2]; 207 208 hyp_point=hyp+9*i; 209 db_StitchCameraRotation_2Points(hyp_point,x1_temp,x2_temp,xp1_temp,xp2_temp); 210 hyp_point[2]*=ft; 211 hyp_point[5]*=ft; 212 hyp_point[6]*=m; 213 hyp_point[7]*=m; 214 cost=db_SquaredReprojectionErrorHomography(y,hyp_point,x3); 215 216 if(!done || cost<best_cost) 217 { 218 done=1; 219 best_cost=cost; 220 best_sol=i; 221 } 222 } 223 224 if(f) *f=fsol[best_sol]; 225 db_Copy9(H,hyp+9*best_sol); 226 return(1); 227 } 228 else 229 { 230 db_Identity3x3(H); 231 if(f) *f=1.0; 232 return(0); 233 } 234 } 235 236 void db_StitchSimilarity2DRaw(double *scale,double R[4],double t[2], 237 double **Xp,double **X,int nr_points,int orientation_preserving, 238 int allow_scaling,int allow_rotation,int allow_translation) 239 { 240 int i; 241 double c[2],cp[2],r[2],rp[2],M[4],s,sp,sc; 242 double *temp,*temp_p; 243 double Aacc,Bacc,Aacc2,Bacc2,divisor,divisor2,m,Am,Bm; 244 245 if(allow_translation) 246 { 247 db_PointCentroid2D(c,X,nr_points); 248 db_PointCentroid2D(cp,Xp,nr_points); 249 } 250 else 251 { 252 db_Zero2(c); 253 db_Zero2(cp); 254 } 255 256 db_Zero4(M); 257 s=sp=0; 258 for(i=0;i<nr_points;i++) 259 { 260 temp= *X++; 261 temp_p= *Xp++; 262 r[0]=(*temp++)-c[0]; 263 r[1]=(*temp++)-c[1]; 264 rp[0]=(*temp_p++)-cp[0]; 265 rp[1]=(*temp_p++)-cp[1]; 266 267 M[0]+=r[0]*rp[0]; 268 M[1]+=r[0]*rp[1]; 269 M[2]+=r[1]*rp[0]; 270 M[3]+=r[1]*rp[1]; 271 272 s+=db_sqr(r[0])+db_sqr(r[1]); 273 sp+=db_sqr(rp[0])+db_sqr(rp[1]); 274 } 275 276 /*Compute scale*/ 277 if(allow_scaling) sc=sqrt(db_SafeDivision(sp,s)); 278 else sc=1.0; 279 *scale=sc; 280 281 /*Compute rotation*/ 282 if(allow_rotation) 283 { 284 /*orientation preserving*/ 285 Aacc=M[0]+M[3]; 286 Bacc=M[2]-M[1]; 287 /*orientation reversing*/ 288 Aacc2=M[0]-M[3]; 289 Bacc2=M[2]+M[1]; 290 if(Aacc!=0.0 || Bacc!=0.0) 291 { 292 divisor=sqrt(Aacc*Aacc+Bacc*Bacc); 293 m=db_SafeReciprocal(divisor); 294 Am=Aacc*m; 295 Bm=Bacc*m; 296 R[0]= Am; 297 R[1]= Bm; 298 R[2]= -Bm; 299 R[3]= Am; 300 } 301 else 302 { 303 db_Identity2x2(R); 304 divisor=0.0; 305 } 306 if(!orientation_preserving && (Aacc2!=0.0 || Bacc2!=0.0)) 307 { 308 divisor2=sqrt(Aacc2*Aacc2+Bacc2*Bacc2); 309 if(divisor2>divisor) 310 { 311 m=db_SafeReciprocal(divisor2); 312 Am=Aacc2*m; 313 Bm=Bacc2*m; 314 R[0]= Am; 315 R[1]= Bm; 316 R[2]= Bm; 317 R[3]= -Am; 318 } 319 } 320 } 321 else db_Identity2x2(R); 322 323 /*Compute translation*/ 324 if(allow_translation) 325 { 326 t[0]=cp[0]-sc*(R[0]*c[0]+R[1]*c[1]); 327 t[1]=cp[1]-sc*(R[2]*c[0]+R[3]*c[1]); 328 } 329 else db_Zero2(t); 330 } 331 332 333
