| /* |
| * Copyright (C) 2011 The Android Open Source Project |
| * |
| * Licensed under the Apache License, Version 2.0 (the "License"); |
| * you may not use this file except in compliance with the License. |
| * You may obtain a copy of the License at |
| * |
| * http://www.apache.org/licenses/LICENSE-2.0 |
| * |
| * Unless required by applicable law or agreed to in writing, software |
| * distributed under the License is distributed on an "AS IS" BASIS, |
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| * See the License for the specific language governing permissions and |
| * limitations under the License. |
| */ |
| |
| /* $Id: db_image_homography.cpp,v 1.2 2011/06/17 14:03:31 mbansal Exp $ */ |
| |
| #include "db_utilities.h" |
| #include "db_image_homography.h" |
| #include "db_framestitching.h" |
| #include "db_metrics.h" |
| |
| |
| |
| /***************************************************************** |
| * Lean and mean begins here * |
| *****************************************************************/ |
| |
| /*Compute the linear constraint on H obtained by requiring that the |
| ratio between coordinate i_num and i_den of xp is equal to the ratio |
| between coordinate i_num and i_den of Hx. i_zero should be set to |
| the coordinate not equal to i_num or i_den. No normalization is used*/ |
| inline void db_SProjImagePointPointConstraint(double c[9],int i_num,int i_den,int i_zero, |
| double xp[3],double x[3]) |
| { |
| db_MultiplyScalarCopy3(c+3*i_den,x, xp[i_num]); |
| db_MultiplyScalarCopy3(c+3*i_num,x, -xp[i_den]); |
| db_Zero3(c+3*i_zero); |
| } |
| |
| /*Compute two constraints on H generated by the correspondence (Xp,X), |
| assuming that Xp ~= H*X. No normalization is used*/ |
| inline void db_SProjImagePointPointConstraints(double c1[9],double c2[9],double xp[3],double x[3]) |
| { |
| int ma_ind; |
| |
| /*Find index of coordinate of Xp with largest absolute value*/ |
| ma_ind=db_MaxAbsIndex3(xp); |
| |
| /*Generate 2 constraints, |
| each constraint is generated by considering the ratio between a |
| coordinate and the largest absolute value coordinate*/ |
| switch(ma_ind) |
| { |
| case 0: |
| db_SProjImagePointPointConstraint(c1,1,0,2,xp,x); |
| db_SProjImagePointPointConstraint(c2,2,0,1,xp,x); |
| break; |
| case 1: |
| db_SProjImagePointPointConstraint(c1,0,1,2,xp,x); |
| db_SProjImagePointPointConstraint(c2,2,1,0,xp,x); |
| break; |
| default: |
| db_SProjImagePointPointConstraint(c1,0,2,1,xp,x); |
| db_SProjImagePointPointConstraint(c2,1,2,0,xp,x); |
| } |
| } |
| |
| inline void db_SAffineImagePointPointConstraints(double c1[7],double c2[7],double xp[3],double x[3]) |
| { |
| double ct1[9],ct2[9]; |
| |
| db_SProjImagePointPointConstraints(ct1,ct2,xp,x); |
| db_Copy6(c1,ct1); c1[6]=ct1[8]; |
| db_Copy6(c2,ct2); c2[6]=ct2[8]; |
| } |
| |
| void 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]) |
| { |
| double c[72]; |
| |
| /*Collect the constraints*/ |
| db_SProjImagePointPointConstraints(c ,c+9 ,xp1,x1); |
| db_SProjImagePointPointConstraints(c+18,c+27,xp2,x2); |
| db_SProjImagePointPointConstraints(c+36,c+45,xp3,x3); |
| db_SProjImagePointPointConstraints(c+54,c+63,xp4,x4); |
| /*Solve for the nullvector*/ |
| db_NullVector8x9Destructive(H,c); |
| } |
| |
| void db_StitchAffine2D_3Points(double H[9], |
| double x1[3],double x2[3],double x3[3], |
| double xp1[3],double xp2[3],double xp3[3]) |
| { |
| double c[42]; |
| |
| /*Collect the constraints*/ |
| db_SAffineImagePointPointConstraints(c ,c+7 ,xp1,x1); |
| db_SAffineImagePointPointConstraints(c+14,c+21,xp2,x2); |
| db_SAffineImagePointPointConstraints(c+28,c+35,xp3,x3); |
| /*Solve for the nullvector*/ |
| db_NullVector6x7Destructive(H,c); |
| db_MultiplyScalar6(H,db_SafeReciprocal(H[6])); |
| H[6]=H[7]=0; H[8]=1.0; |
| } |
| |
| /*Compute up to three solutions for the focal length given two point correspondences |
| generated by a rotation with a common unknown focal length. No specific normalization |
| of the input points is required. If signed_disambiguation is true, the points are |
| required to be in front of the camera*/ |
| 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) |
| { |
| double m,ax,ay,apx,apy,bx,by,bpx,bpy; |
| double p1[2],p2[2],p3[2],p4[2],p5[2],p6[2]; |
| double p7[3],p8[4],p9[5],p10[3],p11[4]; |
| double roots[3]; |
| int nr_roots,i,j; |
| |
| /*Solve for focal length using the equation |
| <a,b>^2*<ap,ap><bp,bp>=<ap,bp>^2*<a,a><b,b> |
| where a and ap are the homogenous vectors in the first image |
| after focal length scaling and b,bp are the vectors in the |
| second image*/ |
| |
| /*Normalize homogenous coordinates so that last coordinate is one*/ |
| m=db_SafeReciprocal(x1[2]); |
| ax=x1[0]*m; |
| ay=x1[1]*m; |
| m=db_SafeReciprocal(xp1[2]); |
| apx=xp1[0]*m; |
| apy=xp1[1]*m; |
| m=db_SafeReciprocal(x2[2]); |
| bx=x2[0]*m; |
| by=x2[1]*m; |
| m=db_SafeReciprocal(xp2[2]); |
| bpx=xp2[0]*m; |
| bpy=xp2[1]*m; |
| |
| /*Compute cubic in l=1/(f^2) |
| by dividing out the root l=0 from the equation |
| (l(ax*bx+ay*by)+1)^2*(l(apx^2+apy^2)+1)*(l(bpx^2+bpy^2)+1)= |
| (l(apx*bpx+apy*bpy)+1)^2*(l(ax^2+ay^2)+1)*(l(bx^2+by^2)+1)*/ |
| p1[1]=ax*bx+ay*by; |
| p2[1]=db_sqr(apx)+db_sqr(apy); |
| p3[1]=db_sqr(bpx)+db_sqr(bpy); |
| p4[1]=apx*bpx+apy*bpy; |
| p5[1]=db_sqr(ax)+db_sqr(ay); |
| p6[1]=db_sqr(bx)+db_sqr(by); |
| p1[0]=p2[0]=p3[0]=p4[0]=p5[0]=p6[0]=1; |
| |
| db_MultiplyPoly1_1(p7,p1,p1); |
| db_MultiplyPoly1_2(p8,p2,p7); |
| db_MultiplyPoly1_3(p9,p3,p8); |
| |
| db_MultiplyPoly1_1(p10,p4,p4); |
| db_MultiplyPoly1_2(p11,p5,p10); |
| db_SubtractPolyProduct1_3(p9,p6,p11); |
| /*Cubic starts at p9[1]*/ |
| db_SolveCubic(roots,&nr_roots,p9[4],p9[3],p9[2],p9[1]); |
| |
| for(j=0,i=0;i<nr_roots;i++) |
| { |
| if(roots[i]>0) |
| { |
| if((!signed_disambiguation) || (db_PolyEval1(p1,roots[i])*db_PolyEval1(p4,roots[i])>0)) |
| { |
| fsol[j++]=db_SafeSqrtReciprocal(roots[i]); |
| } |
| } |
| } |
| *nr_sols=j; |
| } |
| |
| 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) |
| { |
| double fsol[3]; |
| int nr_sols,i,best_sol,done; |
| double cost,best_cost; |
| double m,hyp[27],x1_temp[3],x2_temp[3],xp1_temp[3],xp2_temp[3]; |
| double *hyp_point,ft; |
| double y[2]; |
| |
| db_CommonFocalLengthFromRotation_2Point(fsol,&nr_sols,x1,x2,xp1,xp2,signed_disambiguation); |
| if(nr_sols) |
| { |
| db_DeHomogenizeImagePoint(y,xp3); |
| done=0; |
| for(i=0;i<nr_sols;i++) |
| { |
| ft=fsol[i]; |
| m=db_SafeReciprocal(ft); |
| x1_temp[0]=x1[0]*m; |
| x1_temp[1]=x1[1]*m; |
| x1_temp[2]=x1[2]; |
| x2_temp[0]=x2[0]*m; |
| x2_temp[1]=x2[1]*m; |
| x2_temp[2]=x2[2]; |
| xp1_temp[0]=xp1[0]*m; |
| xp1_temp[1]=xp1[1]*m; |
| xp1_temp[2]=xp1[2]; |
| xp2_temp[0]=xp2[0]*m; |
| xp2_temp[1]=xp2[1]*m; |
| xp2_temp[2]=xp2[2]; |
| |
| hyp_point=hyp+9*i; |
| db_StitchCameraRotation_2Points(hyp_point,x1_temp,x2_temp,xp1_temp,xp2_temp); |
| hyp_point[2]*=ft; |
| hyp_point[5]*=ft; |
| hyp_point[6]*=m; |
| hyp_point[7]*=m; |
| cost=db_SquaredReprojectionErrorHomography(y,hyp_point,x3); |
| |
| if(!done || cost<best_cost) |
| { |
| done=1; |
| best_cost=cost; |
| best_sol=i; |
| } |
| } |
| |
| if(f) *f=fsol[best_sol]; |
| db_Copy9(H,hyp+9*best_sol); |
| return(1); |
| } |
| else |
| { |
| db_Identity3x3(H); |
| if(f) *f=1.0; |
| return(0); |
| } |
| } |
| |
| void 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) |
| { |
| int i; |
| double c[2],cp[2],r[2],rp[2],M[4],s,sp,sc; |
| double *temp,*temp_p; |
| double Aacc,Bacc,Aacc2,Bacc2,divisor,divisor2,m,Am,Bm; |
| |
| if(allow_translation) |
| { |
| db_PointCentroid2D(c,X,nr_points); |
| db_PointCentroid2D(cp,Xp,nr_points); |
| } |
| else |
| { |
| db_Zero2(c); |
| db_Zero2(cp); |
| } |
| |
| db_Zero4(M); |
| s=sp=0; |
| for(i=0;i<nr_points;i++) |
| { |
| temp= *X++; |
| temp_p= *Xp++; |
| r[0]=(*temp++)-c[0]; |
| r[1]=(*temp++)-c[1]; |
| rp[0]=(*temp_p++)-cp[0]; |
| rp[1]=(*temp_p++)-cp[1]; |
| |
| M[0]+=r[0]*rp[0]; |
| M[1]+=r[0]*rp[1]; |
| M[2]+=r[1]*rp[0]; |
| M[3]+=r[1]*rp[1]; |
| |
| s+=db_sqr(r[0])+db_sqr(r[1]); |
| sp+=db_sqr(rp[0])+db_sqr(rp[1]); |
| } |
| |
| /*Compute scale*/ |
| if(allow_scaling) sc=sqrt(db_SafeDivision(sp,s)); |
| else sc=1.0; |
| *scale=sc; |
| |
| /*Compute rotation*/ |
| if(allow_rotation) |
| { |
| /*orientation preserving*/ |
| Aacc=M[0]+M[3]; |
| Bacc=M[2]-M[1]; |
| /*orientation reversing*/ |
| Aacc2=M[0]-M[3]; |
| Bacc2=M[2]+M[1]; |
| if(Aacc!=0.0 || Bacc!=0.0) |
| { |
| divisor=sqrt(Aacc*Aacc+Bacc*Bacc); |
| m=db_SafeReciprocal(divisor); |
| Am=Aacc*m; |
| Bm=Bacc*m; |
| R[0]= Am; |
| R[1]= Bm; |
| R[2]= -Bm; |
| R[3]= Am; |
| } |
| else |
| { |
| db_Identity2x2(R); |
| divisor=0.0; |
| } |
| if(!orientation_preserving && (Aacc2!=0.0 || Bacc2!=0.0)) |
| { |
| divisor2=sqrt(Aacc2*Aacc2+Bacc2*Bacc2); |
| if(divisor2>divisor) |
| { |
| m=db_SafeReciprocal(divisor2); |
| Am=Aacc2*m; |
| Bm=Bacc2*m; |
| R[0]= Am; |
| R[1]= Bm; |
| R[2]= Bm; |
| R[3]= -Am; |
| } |
| } |
| } |
| else db_Identity2x2(R); |
| |
| /*Compute translation*/ |
| if(allow_translation) |
| { |
| t[0]=cp[0]-sc*(R[0]*c[0]+R[1]*c[1]); |
| t[1]=cp[1]-sc*(R[2]*c[0]+R[3]*c[1]); |
| } |
| else db_Zero2(t); |
| } |
| |
| |