Sascha Haeberling | 8bddf8c | 2013-08-14 11:20:34 -0700 | [diff] [blame] | 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 | |