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*/
db_SProjImagePointPointConstraint(double c[9],int i_num,int i_den,int i_zero,double xp[3],double x[3])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*/
db_SProjImagePointPointConstraints(double c1[9],double c2[9],double xp[3],double x[3])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 
db_SAffineImagePointPointConstraints(double c1[7],double c2[7],double xp[3],double x[3])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 
db_StitchProjective2D_4Points(double H[9],double x1[3],double x2[3],double x3[3],double x4[3],double xp1[3],double xp2[3],double xp3[3],double xp4[3])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 
db_StitchAffine2D_3Points(double H[9],double x1[3],double x2[3],double x3[3],double xp1[3],double xp2[3],double xp3[3])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*/
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)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 
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)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 
db_StitchSimilarity2DRaw(double * scale,double R[4],double t[2],double ** Xp,double ** X,int nr_points,int orientation_preserving,int allow_scaling,int allow_rotation,int allow_translation)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