#include "mex.h" #include /* Copyright Henrik Stewenius, 2005. All Rights Reserved. Permission to use [*] this software for academic purposes is hereby granted without fee or royalty, subject to the following conditions. 1. The software is provided "as is" by the copyright holder, with absolutely no actual or implied warranty of correctness, fitness, intellectual property ownership, or anything else whatsoever. You use the software entirely at your own risk. In no event shall the copyright holder be liable for any direct, indirect or perceived damage whatsoever connected with the use of this work. 2. The software may not be distributed [*], whether in its original or modified form, without explicit written consent from the copyright holder. 3. The copyright holder retains all copyright to the work. 4. Any scholarly work, research or publication making use of this software must explicitly acknowledge the copyright holder. In the case of a publication, citation of the work, and author thereof, titled: @PHDTHESIS{stewenius-pdh-2005, AUTHOR = {H. Stew\'enius}, TITLE = {Gr{\"o}bner Basis Methods for Minimal Problems in Computer Vision}, SCHOOL = {Lund University}, YEAR = 2005, MONTH = APR, PDF = {http://www.maths.lth.se/matematiklth/personal/stewe/THESIS/stewenius_phd_2005.pdf} } is deemed sufficient acknowledgement. Permission to use or distribute [*] this software for commercial or military purposes, or under any conditions not covered by the conditions 1-4 above, is NOT granted. Such permission may be obtained subject to explicit prior written consent from the copyright holder. [*] `Use' denotes any use whatsoever including downloading, compilation, execution and modification. `Distribution' includes any distribution of the software and any derived works such as object files, executable programs and libraries. Solver for 3 points visible from 2 stations. INPUT One argument l which is a cell structure containing 12 planes (4 koordinates each ). These are given as a 2*3 cell structure where each column contains the two observations of each point. Each column contains information on a new point. OUTPUT A 2*4 matrix with one solution (a,b,c,d) per row. Complex solutions are set to 0. Trivial solutions are left out and there is NO special handling of the degeneracies for pure translation and coincident cameras is done. Output is (a,b,c,d) to be used in transformation [a,b,0,c ; b-,a,0,d; 0,0,1,0; 0,0,0,1] that is a is NOT as described in the article! When a solution is complex it is disregarded and set to [0,0,0,0] Code written by Henrik Stewenius in october 2003. Henrik Stewenius http://www.maths.lth.se/~stewe/ When reading this code please refer to appropriate material on solving the 2 stations , 3 points , multiple cameras, eucledian case problem. I have one such article submitted somewhere. Ask for it if you need it. Simplifications used in this code: Knowing which monomials will be used and where their coefficients will end up. Knowing the involved multiples of (1+2t+b^2) polynomial and using this to start the elimination process already when inserting. Knowing which two rows to eliminate in M1 makes us avoid having to construct these rows. That this elimination can be done comes from that the rank of M1 is known. Knowing the solutions of be +i, -i let us remove 2 rows from M2 as this is equivalent to division. Matrices M1 and M2 are transposed at input and the polynomial is computed as poly = M2*null(M1) or if considering the transpositions poly = M2^T*null(M1^T) = (null(M1^T)^T*M2)^T Possible optimisations: Multiply M2 by (-2) avoiding some multiplications. Change lijk to macro, maybee not a big timesaver. After transposing the insert some pointer magic is possible... */ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { mxArray *M1, *M2, *M1_null, *invec[1], *outvec[2]; mxArray *poly,*AA,*BB, *cd , *roots; double k1, ka, kb,kc,kd,kA,kB,kC,kD; double ka_v[3],kb_v[3],kc_v[3],kd_v[3]; double kA_v[3],kB_v[3],kC_v[3],kD_v[3]; double *A,*B, *SOLUTIONS, a_minus_1; double l111, l112,l113,l114; double l121, l122,l123,l124; double l211, l212,l213,l214; double l221, l222,l223,l224; int aktive_poly , i, j, rownumber; double a ,b,c,d,b_test, t_test; double *cdv, *B_real_vec, *B_imag_vec; double *M1v, *M2v ,*l1,*l2; if(nrhs !=1 ) mexErrMsgTxt("solve(l)\n l should be a set of planes"); else if (nlhs>1) mexErrMsgTxt("At most one argument out! "); plhs[0]=mxCreateDoubleMatrix( 3,4 , mxREAL); SOLUTIONS = mxGetPr( plhs[0] ); if (!plhs[0]) mexErrMsgTxt("Saknar plats till ut"); M1 = mxCreateDoubleMatrix( 8,9 , mxREAL); M2 = mxCreateDoubleMatrix( 4,9 , mxREAL); M1v = mxGetPr(M1); M2v = mxGetPr(M2); rownumber=0; for( aktive_poly=0; aktive_poly<3 ; aktive_poly++){ l1 = mxGetPr(mxGetCell( prhs[0] , aktive_poly*2 )) ; l2 = mxGetPr(mxGetCell( prhs[0] , aktive_poly*2 + 1)) ; l111 = l1[0 +2*0]; l112 = l1[0 +2*1]; l113 = l1[0 +2*2]; l114 = l1[0 +2*3]; l121 = l1[1 +2*0]; l122 = l1[1 +2*1]; l123 = l1[1 +2*2]; l124 = l1[1 +2*3]; l211 = l2[0 +2*0]; l212 = l2[0 +2*1]; l213 = l2[0 +2*2]; l214 = l2[0 +2*3]; l221 = l2[1 +2*0]; l222 = l2[1 +2*1]; l223 = l2[1 +2*2]; l224 = l2[1 +2*3]; k1 = l111*l122*l213*l224-l111*l122*l223*l214-l112*l121*l213*l224+l112*l121*l223*l214-l113*l124*l221*l212-l114*l123*l211*l222+l113*l124*l211*l222+l114*l123*l221*l212; ka = -k1; kb = l111*l123*l221*l214-l111*l123*l211*l224-l113*l122*l222*l214-l114*l121*l223*l211+l111*l124*l223*l211-l111*l124*l213*l221-l112*l123*l212*l224+l112*l123*l222*l214+l112*l124*l223*l212-l112*l124*l213*l222+l113*l122*l212*l224+l113*l121*l211*l224-l113*l121*l221*l214-l114*l122*l223*l212+l114*l122*l213*l222+l114*l121*l213*l221; kc = -l112*l121*l213*l221-l111*l122*l223*l211+l112*l121*l223*l211+l111*l122*l213*l221; kd = l111*l122*l213*l222-l112*l121*l213*l222-l111*l122*l223*l212+l112*l121*l223*l212; kA = l112*l123*l211*l222-l112*l123*l221*l212+l113*l122*l221*l212-l113*l122*l211*l222; kB = -l111*l123*l212*l221-l113*l121*l211*l222+l111*l123*l211*l222+l113*l121*l212*l221; kD = kd+kA; kC = kc+kB; ka_v[aktive_poly] = ka; kb_v[aktive_poly] = kb; kc_v[aktive_poly] = kc; kd_v[aktive_poly] = kd; kA_v[aktive_poly] = kA; kB_v[aktive_poly] = kB; M1v[rownumber *8+ 3] = -0.5*kD; M1v[rownumber *8+ 4] = - kB; M1v[rownumber *8+ 5] = -0.5*kD + kA; M1v[rownumber *8+ 7] = -0.5*kC; M2v[rownumber *4+ 2] = -0.5*kb; M2v[rownumber *4+ 3] = -0.5*ka; rownumber++; M1v[rownumber *8+ 2] = -0.5*kD; M1v[rownumber *8+ 3] = -kB; M1v[rownumber *8+ 4] = -0.5*kD + kA; M1v[rownumber *8+ 6] = -0.5*kC; M1v[rownumber *8+ 7] = kA; M2v[rownumber *4+ 1] = -0.5*kb; M2v[rownumber *4+ 2] = -0.5*ka; rownumber++; M1v[rownumber *8+ 0] = kD; M1v[rownumber *8+ 1] = kC; M1v[rownumber *8+ 2] =- kB; M1v[rownumber *8+ 3] = kA; M1v[rownumber *8+ 6] = kA; M1v[rownumber *8+ 7] = kB; M2v[rownumber *4+ 0 ] =-0.5*kb; M2v[rownumber *4+ 1 ] =-0.5*ka; rownumber++; } /* I wish to compute null(M1) */ mexCallMATLAB( 1, invec, 1, &M1,"null" ); M1_null = invec[0]; mxDestroyArray( M1); /*I wish to compute M2*null(M1), this gives the desired polynomial */ outvec[1] = M1_null; outvec[0] = M2; mexCallMATLAB( 1, invec, 2, outvec,"mtimes" ); poly = invec[0]; mxDestroyArray( M1_null); mxDestroyArray( M2); /* I compute the roots of the polynomial*/ mexCallMATLAB( 1, invec, 1, &poly,"roots" ); roots = invec[0]; mxDestroyArray( poly); B_real_vec = mxGetPr( roots ) ; B_imag_vec = mxGetPi( roots ) ; AA= mxCreateDoubleMatrix( 3,2 , mxREAL); BB= mxCreateDoubleMatrix( 3,1 , mxREAL); for(j=0; j<3;j++){ b_test= B_real_vec[j]; if( (!mxIsComplex(roots )) || (B_imag_vec[j] == 0) ){ t_test = -0.5*(b_test*b_test+1); a_minus_1 = 1/t_test; a = (1/t_test) +1; b = b_test*(a-1); A = mxGetPr( AA ); B = mxGetPr( BB ); for( i=0; i<3; i++){ A[i] = kc_v[i] + kA_v[i]*b+kB_v[i]*a ; A[i+3] = kd_v[i]+kA_v[i]*a-kB_v[i]*b ; B[i] = -(ka_v[i]*(a-1)+kb_v[i]*b); } outvec[0] = AA; outvec[1] = BB; mexCallMATLAB( 1, invec, 2, outvec,"mldivide" ); cdv = mxGetPr( invec[0] ); c= cdv[0]; d= cdv[1]; SOLUTIONS[ j+ 0*3] = a; SOLUTIONS[ j+ 1*3] = b; SOLUTIONS[ j+ 2*3] = c; SOLUTIONS[ j+ 3*3] = d; } } mxDestroyArray( roots); mxDestroyArray( AA); mxDestroyArray( BB); }