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