1ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank/* 2ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * Copyright (C) 2011 The Android Open Source Project 3ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * 4ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * Licensed under the Apache License, Version 2.0 (the "License"); 5ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * you may not use this file except in compliance with the License. 6ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * You may obtain a copy of the License at 7ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * 8ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * http://www.apache.org/licenses/LICENSE-2.0 9ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * 10ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * Unless required by applicable law or agreed to in writing, software 11ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * distributed under the License is distributed on an "AS IS" BASIS, 12ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * See the License for the specific language governing permissions and 14ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank * limitations under the License. 15ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank */ 16ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank 17ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank/* $Id: db_image_homography.cpp,v 1.2 2011/06/17 14:03:31 mbansal Exp $ */ 18ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank 19ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank#include "db_utilities.h" 209b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank#include "db_image_homography.h" 219b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank#include "db_framestitching.h" 229b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank#include "db_metrics.h" 239b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank 249b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank 259b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank 260dfbd9efda459c7eab716a8eca5f908973bc585fMarc Blank/***************************************************************** 279b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank* Lean and mean begins here * 28e6c2456aa6c00ef78c6d1d1621511d7ef8507f83Marc Blank*****************************************************************/ 29e6c2456aa6c00ef78c6d1d1621511d7ef8507f83Marc Blank 30e6c2456aa6c00ef78c6d1d1621511d7ef8507f83Marc Blank/*Compute the linear constraint on H obtained by requiring that the 31ea68a80285e175614a7b36adea6fd25f09e3a887Marc Blankratio between coordinate i_num and i_den of xp is equal to the ratio 32b2adf8a5aae8647b728441ef4d56f181f26f848eJorge Lugobetween coordinate i_num and i_den of Hx. i_zero should be set to 339b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blankthe coordinate not equal to i_num or i_den. No normalization is used*/ 349b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blankinline void db_SProjImagePointPointConstraint(double c[9],int i_num,int i_den,int i_zero, 359b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank double xp[3],double x[3]) 36c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank{ 37c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank db_MultiplyScalarCopy3(c+3*i_den,x, xp[i_num]); 38c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank db_MultiplyScalarCopy3(c+3*i_num,x, -xp[i_den]); 39c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank db_Zero3(c+3*i_zero); 40c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank} 41c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank 42c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank/*Compute two constraints on H generated by the correspondence (Xp,X), 430dfbd9efda459c7eab716a8eca5f908973bc585fMarc Blankassuming that Xp ~= H*X. No normalization is used*/ 44c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blankinline void db_SProjImagePointPointConstraints(double c1[9],double c2[9],double xp[3],double x[3]) 450dfbd9efda459c7eab716a8eca5f908973bc585fMarc Blank{ 46c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank int ma_ind; 47c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank 480565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank /*Find index of coordinate of Xp with largest absolute value*/ 49c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank ma_ind=db_MaxAbsIndex3(xp); 50c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank 51c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank /*Generate 2 constraints, 524d8774462ace9a45154b2df418b9f2fe7a9c685dBen Komalo each constraint is generated by considering the ratio between a 53d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank coordinate and the largest absolute value coordinate*/ 5434d9cf7433233c1a61ca04cb5be131b9207c00abMarc Blank switch(ma_ind) 55e951b589c5134a1154ec3743d79236dee54a6519Marc Blank { 56e951b589c5134a1154ec3743d79236dee54a6519Marc Blank case 0: 57e951b589c5134a1154ec3743d79236dee54a6519Marc Blank db_SProjImagePointPointConstraint(c1,1,0,2,xp,x); 58e6c2456aa6c00ef78c6d1d1621511d7ef8507f83Marc Blank db_SProjImagePointPointConstraint(c2,2,0,1,xp,x); 59c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank break; 6077186bb1a174432ef272584374942d8b9228e39cMarc Blank case 1: 6100d91b2e12d65df06916afdc4bebca67fd27214cMarc Blank db_SProjImagePointPointConstraint(c1,0,1,2,xp,x); 62498c903e02ef1b150d6dbd3a01d35839026db264Ben Komalo db_SProjImagePointPointConstraint(c2,2,1,0,xp,x); 6300d91b2e12d65df06916afdc4bebca67fd27214cMarc Blank break; 644471a6960d352242cc65bddf7888cc5335840c74Marc Blank default: 650565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank db_SProjImagePointPointConstraint(c1,0,2,1,xp,x); 66c10a3beef4f048292e6a4ceb31527c5123801517Marc Blank db_SProjImagePointPointConstraint(c2,1,2,0,xp,x); 67277be74f5d0abcc3bb23cd13fae9d628b131e2bfMarc Blank } 68ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank} 690565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank 700565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blankinline void db_SAffineImagePointPointConstraints(double c1[7],double c2[7],double xp[3],double x[3]) 710565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank{ 724f15001bdfd11c79524b4e44d60041967779e763Marc Blank double ct1[9],ct2[9]; 7300d91b2e12d65df06916afdc4bebca67fd27214cMarc Blank 7400d91b2e12d65df06916afdc4bebca67fd27214cMarc Blank db_SProjImagePointPointConstraints(ct1,ct2,xp,x); 7500d91b2e12d65df06916afdc4bebca67fd27214cMarc Blank db_Copy6(c1,ct1); c1[6]=ct1[8]; 7636e08ce9f808425ed573e182812f3a82ef4d5d45Marc Blank db_Copy6(c2,ct2); c2[6]=ct2[8]; 7700d91b2e12d65df06916afdc4bebca67fd27214cMarc Blank} 7800d91b2e12d65df06916afdc4bebca67fd27214cMarc Blank 79ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blankvoid db_StitchProjective2D_4Points(double H[9], 80ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank double x1[3],double x2[3],double x3[3],double x4[3], 81ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank double xp1[3],double xp2[3],double xp3[3],double xp4[3]) 82ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank{ 83ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank double c[72]; 847c582a7fb883b3be728f270fbe5277676fe37cf9Marc Blank 8500d91b2e12d65df06916afdc4bebca67fd27214cMarc Blank /*Collect the constraints*/ 8669c1a51ac81fe2c649e5f2069dc75565d868db98Ben Komalo db_SProjImagePointPointConstraints(c ,c+9 ,xp1,x1); 8769c1a51ac81fe2c649e5f2069dc75565d868db98Ben Komalo db_SProjImagePointPointConstraints(c+18,c+27,xp2,x2); 8891e4233059a8b734dd67ffcfa0d08a0d4d8ab17dMarc Blank db_SProjImagePointPointConstraints(c+36,c+45,xp3,x3); 8991e4233059a8b734dd67ffcfa0d08a0d4d8ab17dMarc Blank db_SProjImagePointPointConstraints(c+54,c+63,xp4,x4); 9091e4233059a8b734dd67ffcfa0d08a0d4d8ab17dMarc Blank /*Solve for the nullvector*/ 9136e08ce9f808425ed573e182812f3a82ef4d5d45Marc Blank db_NullVector8x9Destructive(H,c); 9291e4233059a8b734dd67ffcfa0d08a0d4d8ab17dMarc Blank} 9336e08ce9f808425ed573e182812f3a82ef4d5d45Marc Blank 9436e08ce9f808425ed573e182812f3a82ef4d5d45Marc Blankvoid db_StitchAffine2D_3Points(double H[9], 9549c739b7b7e0663b9a292b0d0ec966814aebe42aMarc Blank double x1[3],double x2[3],double x3[3], 9649c739b7b7e0663b9a292b0d0ec966814aebe42aMarc Blank double xp1[3],double xp2[3],double xp3[3]) 9749c739b7b7e0663b9a292b0d0ec966814aebe42aMarc Blank{ 9818e1e20e3c5e098fd4c038349dddb6112aa130edMarc Blank double c[42]; 9918e1e20e3c5e098fd4c038349dddb6112aa130edMarc Blank 10018e1e20e3c5e098fd4c038349dddb6112aa130edMarc Blank /*Collect the constraints*/ 101c6b98dad8e9002937d5755c61f2d8709807f4d22Marc Blank db_SAffineImagePointPointConstraints(c ,c+7 ,xp1,x1); 102a11e0c0d45e4053824c72dc9042d9b76005da4a6Marc Blank db_SAffineImagePointPointConstraints(c+14,c+21,xp2,x2); 103a11e0c0d45e4053824c72dc9042d9b76005da4a6Marc Blank db_SAffineImagePointPointConstraints(c+28,c+35,xp3,x3); 104a11e0c0d45e4053824c72dc9042d9b76005da4a6Marc Blank /*Solve for the nullvector*/ 1054f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_NullVector6x7Destructive(H,c); 1064f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_MultiplyScalar6(H,db_SafeReciprocal(H[6])); 1074f15001bdfd11c79524b4e44d60041967779e763Marc Blank H[6]=H[7]=0; H[8]=1.0; 1084f15001bdfd11c79524b4e44d60041967779e763Marc Blank} 1094f15001bdfd11c79524b4e44d60041967779e763Marc Blank 1104f15001bdfd11c79524b4e44d60041967779e763Marc Blank/*Compute up to three solutions for the focal length given two point correspondences 11191e4233059a8b734dd67ffcfa0d08a0d4d8ab17dMarc Blankgenerated by a rotation with a common unknown focal length. No specific normalization 1129b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blankof the input points is required. If signed_disambiguation is true, the points are 1139b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blankrequired to be in front of the camera*/ 1149b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blankinline 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) 1159b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank{ 1169b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank double m,ax,ay,apx,apy,bx,by,bpx,bpy; 1179b243f9a4581a63c6369d0f5a4f695edf2e90e38Marc Blank double p1[2],p2[2],p3[2],p4[2],p5[2],p6[2]; 118422a3b5f8b8b3efbecaec9bc53860db546bfbe34Marc Blank double p7[3],p8[4],p9[5],p10[3],p11[4]; 119498c903e02ef1b150d6dbd3a01d35839026db264Ben Komalo double roots[3]; 120498c903e02ef1b150d6dbd3a01d35839026db264Ben Komalo int nr_roots,i,j; 121ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank 122936b0401ac57e2853915bd3535bbd2ab6869c1bbTodd Kennedy /*Solve for focal length using the equation 123936b0401ac57e2853915bd3535bbd2ab6869c1bbTodd Kennedy <a,b>^2*<ap,ap><bp,bp>=<ap,bp>^2*<a,a><b,b> 124936b0401ac57e2853915bd3535bbd2ab6869c1bbTodd Kennedy where a and ap are the homogenous vectors in the first image 125936b0401ac57e2853915bd3535bbd2ab6869c1bbTodd Kennedy after focal length scaling and b,bp are the vectors in the 126498c903e02ef1b150d6dbd3a01d35839026db264Ben Komalo second image*/ 1274f15001bdfd11c79524b4e44d60041967779e763Marc Blank 128ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank /*Normalize homogenous coordinates so that last coordinate is one*/ 1298efd25be4e1db3c0c79aae2ca1b4664b21bb410bMarc Blank m=db_SafeReciprocal(x1[2]); 130936b0401ac57e2853915bd3535bbd2ab6869c1bbTodd Kennedy ax=x1[0]*m; 1318efd25be4e1db3c0c79aae2ca1b4664b21bb410bMarc Blank ay=x1[1]*m; 132d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank m=db_SafeReciprocal(xp1[2]); 133d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank apx=xp1[0]*m; 134d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank apy=xp1[1]*m; 1356f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank m=db_SafeReciprocal(x2[2]); 1366f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank bx=x2[0]*m; 137d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank by=x2[1]*m; 138d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank m=db_SafeReciprocal(xp2[2]); 139d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank bpx=xp2[0]*m; 140d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank bpy=xp2[1]*m; 141d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank 142d1d98cba6f4604c5b88b3c53a09b9741f8c87a54Marc Blank /*Compute cubic in l=1/(f^2) 1436f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank by dividing out the root l=0 from the equation 1446f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank (l(ax*bx+ay*by)+1)^2*(l(apx^2+apy^2)+1)*(l(bpx^2+bpy^2)+1)= 1456f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank (l(apx*bpx+apy*bpy)+1)^2*(l(ax^2+ay^2)+1)*(l(bx^2+by^2)+1)*/ 1466f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank p1[1]=ax*bx+ay*by; 1476f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank p2[1]=db_sqr(apx)+db_sqr(apy); 1486f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank p3[1]=db_sqr(bpx)+db_sqr(bpy); 1496f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank p4[1]=apx*bpx+apy*bpy; 1506f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank p5[1]=db_sqr(ax)+db_sqr(ay); 1516f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank p6[1]=db_sqr(bx)+db_sqr(by); 1526f898deac953e5838fa28f47a16e0fb92bbc81ebMarc Blank p1[0]=p2[0]=p3[0]=p4[0]=p5[0]=p6[0]=1; 153057faf66b737bbc7df2eaf77ee7a63827785e1b9Marc Blank 154a261805b03b853cce662b679da3e16120d521b7eMarc Blank db_MultiplyPoly1_1(p7,p1,p1); 155134346f5b886e6b53074238546653cdc76bbe868Marc Blank db_MultiplyPoly1_2(p8,p2,p7); 156c8e4352ea6cfa67f15140512e84af8ccede222d2Marc Blank db_MultiplyPoly1_3(p9,p3,p8); 157ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank 158ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank db_MultiplyPoly1_1(p10,p4,p4); 1594f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_MultiplyPoly1_2(p11,p5,p10); 1608bad9caf330ff72a73d9b7b98ecba7ce5f57ffc9Marc Blank db_SubtractPolyProduct1_3(p9,p6,p11); 161b43c40606146babc767475bbabac5820efd4c604Marc Blank /*Cubic starts at p9[1]*/ 162b43c40606146babc767475bbabac5820efd4c604Marc Blank db_SolveCubic(roots,&nr_roots,p9[4],p9[3],p9[2],p9[1]); 1638bad9caf330ff72a73d9b7b98ecba7ce5f57ffc9Marc Blank 1648bad9caf330ff72a73d9b7b98ecba7ce5f57ffc9Marc Blank for(j=0,i=0;i<nr_roots;i++) 1658bad9caf330ff72a73d9b7b98ecba7ce5f57ffc9Marc Blank { 1660565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank if(roots[i]>0) 1670565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank { 168e951b589c5134a1154ec3743d79236dee54a6519Marc Blank if((!signed_disambiguation) || (db_PolyEval1(p1,roots[i])*db_PolyEval1(p4,roots[i])>0)) 1694f15001bdfd11c79524b4e44d60041967779e763Marc Blank { 170e951b589c5134a1154ec3743d79236dee54a6519Marc Blank fsol[j++]=db_SafeSqrtReciprocal(roots[i]); 1714f15001bdfd11c79524b4e44d60041967779e763Marc Blank } 172e951b589c5134a1154ec3743d79236dee54a6519Marc Blank } 1734f15001bdfd11c79524b4e44d60041967779e763Marc Blank } 174e951b589c5134a1154ec3743d79236dee54a6519Marc Blank *nr_sols=j; 1754f15001bdfd11c79524b4e44d60041967779e763Marc Blank} 176e951b589c5134a1154ec3743d79236dee54a6519Marc Blank 1774f15001bdfd11c79524b4e44d60041967779e763Marc Blankint 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) 178e951b589c5134a1154ec3743d79236dee54a6519Marc Blank{ 1794f15001bdfd11c79524b4e44d60041967779e763Marc Blank double fsol[3]; 1804f15001bdfd11c79524b4e44d60041967779e763Marc Blank int nr_sols,i,best_sol,done; 1814f15001bdfd11c79524b4e44d60041967779e763Marc Blank double cost,best_cost; 1824f15001bdfd11c79524b4e44d60041967779e763Marc Blank double m,hyp[27],x1_temp[3],x2_temp[3],xp1_temp[3],xp2_temp[3]; 1834f15001bdfd11c79524b4e44d60041967779e763Marc Blank double *hyp_point,ft; 1844f15001bdfd11c79524b4e44d60041967779e763Marc Blank double y[2]; 1854f15001bdfd11c79524b4e44d60041967779e763Marc Blank 1864f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_CommonFocalLengthFromRotation_2Point(fsol,&nr_sols,x1,x2,xp1,xp2,signed_disambiguation); 1874f15001bdfd11c79524b4e44d60041967779e763Marc Blank if(nr_sols) 188936b0401ac57e2853915bd3535bbd2ab6869c1bbTodd Kennedy { 189936b0401ac57e2853915bd3535bbd2ab6869c1bbTodd Kennedy db_DeHomogenizeImagePoint(y,xp3); 1904f15001bdfd11c79524b4e44d60041967779e763Marc Blank done=0; 1914f15001bdfd11c79524b4e44d60041967779e763Marc Blank for(i=0;i<nr_sols;i++) 1924f15001bdfd11c79524b4e44d60041967779e763Marc Blank { 1934f15001bdfd11c79524b4e44d60041967779e763Marc Blank ft=fsol[i]; 1944f15001bdfd11c79524b4e44d60041967779e763Marc Blank m=db_SafeReciprocal(ft); 1954f15001bdfd11c79524b4e44d60041967779e763Marc Blank x1_temp[0]=x1[0]*m; 1964f15001bdfd11c79524b4e44d60041967779e763Marc Blank x1_temp[1]=x1[1]*m; 1974f15001bdfd11c79524b4e44d60041967779e763Marc Blank x1_temp[2]=x1[2]; 1984f15001bdfd11c79524b4e44d60041967779e763Marc Blank x2_temp[0]=x2[0]*m; 1994f15001bdfd11c79524b4e44d60041967779e763Marc Blank x2_temp[1]=x2[1]*m; 200e6c2456aa6c00ef78c6d1d1621511d7ef8507f83Marc Blank x2_temp[2]=x2[2]; 2014f15001bdfd11c79524b4e44d60041967779e763Marc Blank xp1_temp[0]=xp1[0]*m; 202e6c2456aa6c00ef78c6d1d1621511d7ef8507f83Marc Blank xp1_temp[1]=xp1[1]*m; 2034f15001bdfd11c79524b4e44d60041967779e763Marc Blank xp1_temp[2]=xp1[2]; 2044f15001bdfd11c79524b4e44d60041967779e763Marc Blank xp2_temp[0]=xp2[0]*m; 2054f15001bdfd11c79524b4e44d60041967779e763Marc Blank xp2_temp[1]=xp2[1]*m; 2064f15001bdfd11c79524b4e44d60041967779e763Marc Blank xp2_temp[2]=xp2[2]; 2074f15001bdfd11c79524b4e44d60041967779e763Marc Blank 2084f15001bdfd11c79524b4e44d60041967779e763Marc Blank hyp_point=hyp+9*i; 2094f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_StitchCameraRotation_2Points(hyp_point,x1_temp,x2_temp,xp1_temp,xp2_temp); 2104f15001bdfd11c79524b4e44d60041967779e763Marc Blank hyp_point[2]*=ft; 2114f15001bdfd11c79524b4e44d60041967779e763Marc Blank hyp_point[5]*=ft; 2124f15001bdfd11c79524b4e44d60041967779e763Marc Blank hyp_point[6]*=m; 2134f15001bdfd11c79524b4e44d60041967779e763Marc Blank hyp_point[7]*=m; 2144f15001bdfd11c79524b4e44d60041967779e763Marc Blank cost=db_SquaredReprojectionErrorHomography(y,hyp_point,x3); 2154f15001bdfd11c79524b4e44d60041967779e763Marc Blank 2164f15001bdfd11c79524b4e44d60041967779e763Marc Blank if(!done || cost<best_cost) 2174f15001bdfd11c79524b4e44d60041967779e763Marc Blank { 2184f15001bdfd11c79524b4e44d60041967779e763Marc Blank done=1; 2194f15001bdfd11c79524b4e44d60041967779e763Marc Blank best_cost=cost; 2204f15001bdfd11c79524b4e44d60041967779e763Marc Blank best_sol=i; 2214f15001bdfd11c79524b4e44d60041967779e763Marc Blank } 2224f15001bdfd11c79524b4e44d60041967779e763Marc Blank } 2234f15001bdfd11c79524b4e44d60041967779e763Marc Blank 2244f15001bdfd11c79524b4e44d60041967779e763Marc Blank if(f) *f=fsol[best_sol]; 2254f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_Copy9(H,hyp+9*best_sol); 2264f15001bdfd11c79524b4e44d60041967779e763Marc Blank return(1); 2271d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank } 2281d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank else 2291d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank { 2301d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank db_Identity3x3(H); 2311d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank if(f) *f=1.0; 2321d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank return(0); 2331d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank } 2341d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank} 2351d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank 2361d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blankvoid db_StitchSimilarity2DRaw(double *scale,double R[4],double t[2], 2371d4f429f583dd6ba18d0894b2b1d02afb968c96fMarc Blank double **Xp,double **X,int nr_points,int orientation_preserving, 2384f15001bdfd11c79524b4e44d60041967779e763Marc Blank int allow_scaling,int allow_rotation,int allow_translation) 2394f15001bdfd11c79524b4e44d60041967779e763Marc Blank{ 2404f15001bdfd11c79524b4e44d60041967779e763Marc Blank int i; 2414f15001bdfd11c79524b4e44d60041967779e763Marc Blank double c[2],cp[2],r[2],rp[2],M[4],s,sp,sc; 2420d8fe734daad069d8652688d62a2753b0457d1eaMarc Blank double *temp,*temp_p; 2430d8fe734daad069d8652688d62a2753b0457d1eaMarc Blank double Aacc,Bacc,Aacc2,Bacc2,divisor,divisor2,m,Am,Bm; 2440d8fe734daad069d8652688d62a2753b0457d1eaMarc Blank 2450d8fe734daad069d8652688d62a2753b0457d1eaMarc Blank if(allow_translation) 2460d8fe734daad069d8652688d62a2753b0457d1eaMarc Blank { 247aead58d49204e28a78523c19bd86ad14a0599318Marc Blank db_PointCentroid2D(c,X,nr_points); 2484f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_PointCentroid2D(cp,Xp,nr_points); 2494f15001bdfd11c79524b4e44d60041967779e763Marc Blank } 2504f15001bdfd11c79524b4e44d60041967779e763Marc Blank else 2514f15001bdfd11c79524b4e44d60041967779e763Marc Blank { 2524f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_Zero2(c); 2534f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_Zero2(cp); 2544f15001bdfd11c79524b4e44d60041967779e763Marc Blank } 2554f15001bdfd11c79524b4e44d60041967779e763Marc Blank 2564f15001bdfd11c79524b4e44d60041967779e763Marc Blank db_Zero4(M); 2574f15001bdfd11c79524b4e44d60041967779e763Marc Blank s=sp=0; 2584f15001bdfd11c79524b4e44d60041967779e763Marc Blank for(i=0;i<nr_points;i++) 2594f15001bdfd11c79524b4e44d60041967779e763Marc Blank { 2604f15001bdfd11c79524b4e44d60041967779e763Marc Blank temp= *X++; 2614f15001bdfd11c79524b4e44d60041967779e763Marc Blank temp_p= *Xp++; 2624f15001bdfd11c79524b4e44d60041967779e763Marc Blank r[0]=(*temp++)-c[0]; 2634f15001bdfd11c79524b4e44d60041967779e763Marc Blank r[1]=(*temp++)-c[1]; 2644f15001bdfd11c79524b4e44d60041967779e763Marc Blank rp[0]=(*temp_p++)-cp[0]; 2654f15001bdfd11c79524b4e44d60041967779e763Marc Blank rp[1]=(*temp_p++)-cp[1]; 2664f15001bdfd11c79524b4e44d60041967779e763Marc Blank 2674f15001bdfd11c79524b4e44d60041967779e763Marc Blank M[0]+=r[0]*rp[0]; 2684f15001bdfd11c79524b4e44d60041967779e763Marc Blank M[1]+=r[0]*rp[1]; 2694f15001bdfd11c79524b4e44d60041967779e763Marc Blank M[2]+=r[1]*rp[0]; 2704f15001bdfd11c79524b4e44d60041967779e763Marc Blank M[3]+=r[1]*rp[1]; 271ab30d429e0c6069604aead9b5e6845b6b91b6a02Marc Blank 27277186bb1a174432ef272584374942d8b9228e39cMarc Blank s+=db_sqr(r[0])+db_sqr(r[1]); 27348af7392c82262d17700e3fbdccf3a582809d449Marc Blank sp+=db_sqr(rp[0])+db_sqr(rp[1]); 2744f15001bdfd11c79524b4e44d60041967779e763Marc Blank } 2758efd25be4e1db3c0c79aae2ca1b4664b21bb410bMarc Blank 2768efd25be4e1db3c0c79aae2ca1b4664b21bb410bMarc Blank /*Compute scale*/ 2778efd25be4e1db3c0c79aae2ca1b4664b21bb410bMarc Blank if(allow_scaling) sc=sqrt(db_SafeDivision(sp,s)); 2784f15001bdfd11c79524b4e44d60041967779e763Marc Blank else sc=1.0; 2794f15001bdfd11c79524b4e44d60041967779e763Marc Blank *scale=sc; 2804f15001bdfd11c79524b4e44d60041967779e763Marc Blank 2814f15001bdfd11c79524b4e44d60041967779e763Marc Blank /*Compute rotation*/ 2820565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank if(allow_rotation) 2830565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank { 2840565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank /*orientation preserving*/ 2850565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank Aacc=M[0]+M[3]; 2860565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank Bacc=M[2]-M[1]; 2870565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank /*orientation reversing*/ 2880565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank Aacc2=M[0]-M[3]; 2890565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank Bacc2=M[2]+M[1]; 2900565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank if(Aacc!=0.0 || Bacc!=0.0) 2918efd25be4e1db3c0c79aae2ca1b4664b21bb410bMarc Blank { 2928efd25be4e1db3c0c79aae2ca1b4664b21bb410bMarc Blank divisor=sqrt(Aacc*Aacc+Bacc*Bacc); 2938efd25be4e1db3c0c79aae2ca1b4664b21bb410bMarc Blank m=db_SafeReciprocal(divisor); 2940565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank Am=Aacc*m; 2950565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank Bm=Bacc*m; 2960565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank R[0]= Am; 2970565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank R[1]= Bm; 2980565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank R[2]= -Bm; 2990565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank R[3]= Am; 3000565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank } 3010565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank else 3020565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank { 3030565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank db_Identity2x2(R); 3040565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank divisor=0.0; 3050565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank } 3060565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank if(!orientation_preserving && (Aacc2!=0.0 || Bacc2!=0.0)) 3070565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank { 3080565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank divisor2=sqrt(Aacc2*Aacc2+Bacc2*Bacc2); 3090565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank if(divisor2>divisor) 3100565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank { 3110565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank m=db_SafeReciprocal(divisor2); 3120565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank Am=Aacc2*m; 3130565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank Bm=Bacc2*m; 3140565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank R[0]= Am; 3150565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank R[1]= Bm; 3160565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank R[2]= Bm; 3170565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank R[3]= -Am; 3180565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank } 3190565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank } 3200565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank } 3210565fd4f943aa3e5be5e001fb16d2f3d69159de6Marc Blank else db_Identity2x2(R); 3220dfbd9efda459c7eab716a8eca5f908973bc585fMarc Blank 32330726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank /*Compute translation*/ 32430726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank if(allow_translation) 32530726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank { 32630726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank t[0]=cp[0]-sc*(R[0]*c[0]+R[1]*c[1]); 32730726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank t[1]=cp[1]-sc*(R[2]*c[0]+R[3]*c[1]); 32830726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank } 32930726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank else db_Zero2(t); 33030726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank} 33130726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank 33230726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank 33330726eb1240f82e48182a26de78e69c4763f6d2eMarc Blank