QVision: Qt's Image, Video and Computer Vision Library

00001 /*
00002  *      Copyright (C) 2011, 2012. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00024 
00025 #include <qvprojective.h>
00026 
00027 /*
00028 # Get coefficients for polynomial:
00029 #       a x^3 + b x^2 + c x + d
00030 with(linalg):
00031 Mf := Matrix(4,4,symbol=mf):
00032 Mg := Matrix(4,4,symbol=mg):
00033 d := det(Mf - x Mg):
00034 
00035 CodeGeneration[C](Vector([ coeff(d, x, 0), coeff(d, x, 1), coeff(d, x, 2), coeff(d, x, 3), coeff(d, x, 4) ]), optimize = true);
00036 */
00037 // Get coefficients 'a_i' for the following polynomial:
00038 //      determinant(Mf - x Mg) = a_4 x^4 + a_3 x^3 + a_2 x^2 + a_1 x + a_0
00039 QList<double> getPolynomialCoefficientsForMf_xMg(const QVMatrix &Mf, const QVMatrix &Mg)
00040         {
00041         double mf[4][4], mg[4][4];
00042         for(int i = 0; i < 4; i++)
00043                 for(int j = 0; j < 4; j++)
00044                         {
00045                         mf[i][j] = Mf(i,j);
00046                         mg[i][j] = Mg(i,j);
00047                         }
00048 
00049         const double t1 = mf[0][0] * mf[1][1];
00050         const double t2 = mf[2][2] * mf[3][3];
00051         const double t4 = mf[2][3] * mf[3][2];
00052         const double t6 = mf[0][0] * mf[2][1];
00053         const double t7 = mf[1][2] * mf[3][3];
00054         const double t9 = mf[1][3] * mf[3][2];
00055         const double t11 = mf[0][0] * mf[3][1];
00056         const double t12 = mf[1][2] * mf[2][3];
00057         const double t14 = mf[1][3] * mf[2][2];
00058         const double t16 = mf[1][0] * mf[2][1];
00059         const double t17 = mf[0][2] * mf[3][3];
00060         const double t19 = mf[0][3] * mf[3][2];
00061         const double t21 = mf[1][0] * mf[0][1];
00062         const double t24 = mf[1][0] * mf[3][1];
00063         const double t25 = mf[0][3] * mf[2][2];
00064         const double t27 = mf[0][2] * mf[2][3];
00065         const double t29 = t1 * t2 - t1 * t4 - t6 * t7 + t6 * t9 + t11 * t12 - t11 * t14 + t16 * t17 - t16 * t19 + t21 * t4 - t21 * t2 + t24 * t25 - t24 * t27;
00066         const double t30 = mf[2][0] * mf[0][1];
00067         const double t33 = mf[2][0] * mf[1][1];
00068         const double t36 = mf[2][0] * mf[3][1];
00069         const double t37 = mf[0][2] * mf[1][3];
00070         const double t39 = mf[0][3] * mf[1][2];
00071         const double t41 = mf[3][0] * mf[0][1];
00072         const double t44 = mf[3][0] * mf[1][1];
00073         const double t47 = mf[3][0] * mf[2][1];
00074         const double t50 = t30 * t7 - t30 * t9 - t33 * t17 + t33 * t19 + t36 * t37 - t36 * t39 - t41 * t12 + t41 * t14 + t44 * t27 - t44 * t25 - t47 * t37 + t47 * t39;
00075         const double t52 = mf[3][0] * mg[1][1];
00076         const double t54 = mg[3][0] * mf[0][1];
00077         const double t57 = mg[3][0] * mf[1][1];
00078         const double t60 = mg[3][0] * mf[2][1];
00079         const double t63 = mf[3][0] * mg[0][1];
00080         const double t65 = mf[0][2] * mg[1][3];
00081         const double t67 = mg[0][2] * mf[1][3];
00082         const double t69 = mf[0][3] * mg[1][2];
00083         const double t71 = mg[0][3] * mf[1][2];
00084         const double t73 = -t52 * t27 + t54 * t12 - t54 * t14 - t57 * t27 + t57 * t25 + t60 * t37 - t60 * t39 + t63 * t12 + t47 * t65 + t47 * t67 - t47 * t69 - t47 * t71;
00085         const double t74 = mf[3][0] * mg[2][1];
00086         const double t76 = mf[0][0] * mg[3][1];
00087         const double t79 = mf[1][2] * mg[3][3];
00088         const double t81 = mg[2][2] * mf[3][3];
00089         const double t83 = mf[0][0] * mg[1][1];
00090         const double t85 = mg[2][3] * mf[3][2];
00091         const double t87 = mg[1][3] * mf[3][2];
00092         const double t89 = mg[0][2] * mf[2][3];
00093         const double t91 = mf[0][3] * mg[2][2];
00094         const double t93 = mf[1][0] * mg[2][1];
00095         const double t95 = mf[0][2] * mg[2][3];
00096         const double t97 = t74 * t37 - t76 * t12 + t76 * t14 + t6 * t79 - t1 * t81 - t83 * t2 + t1 * t85 - t6 * t87 - t44 * t89 + t44 * t91 + t93 * t19 + t24 * t95;
00097         const double t101 = mg[0][3] * mf[2][2];
00098         const double t103 = mf[1][0] * mg[3][1];
00099         const double t106 = mf[2][3] * mg[3][2];
00100         const double t109 = mg[2][0] * mf[0][1];
00101         const double t111 = mg[2][0] * mf[1][1];
00102         const double t114 = mg[1][0] * mf[2][1];
00103         const double t116 = mf[0][2] * mg[3][3];
00104         const double t118 = t24 * t89 - t24 * t91 - t24 * t101 + t103 * t27 - t30 * t79 - t21 * t106 - t21 * t85 + t109 * t9 + t111 * t17 - t103 * t25 - t114 * t17 + t33 * t116;
00105         const double t119 = mg[0][2] * mf[3][3];
00106         const double t121 = mf[0][3] * mg[3][2];
00107         const double t123 = mg[0][3] * mf[3][2];
00108         const double t125 = mf[2][0] * mg[1][1];
00109         const double t133 = mf[2][0] * mg[3][1];
00110         const double t136 = t33 * t119 - t33 * t121 - t33 * t123 + t125 * t17 + t36 * t69 - t125 * t19 - t36 * t65 - t36 * t67 - t109 * t7 + t36 * t71 - t133 * t37 + t133 * t39;
00111         const double t139 = mf[0][0] * mg[2][1];
00112         const double t141 = mf[1][3] * mg[2][2];
00113         const double t143 = mf[1][3] * mg[3][2];
00114         const double t146 = mg[1][3] * mf[2][2];
00115         const double t149 = mg[1][2] * mf[2][3];
00116         const double t152 = mf[1][0] * mg[0][1];
00117         const double t154 = mf[1][2] * mg[2][3];
00118         const double t156 = mf[2][2] * mg[3][3];
00119         const double t158 = mg[1][2] * mf[3][3];
00120         const double t160 = -t139 * t9 + t11 * t141 - t6 * t143 + t1 * t106 + t11 * t146 + t83 * t4 - t11 * t149 - t44 * t95 + t152 * t2 - t11 * t154 - t1 * t156 + t6 * t158;
00121         const double t165 = mf[2][0] * mg[0][1];
00122         const double t174 = t139 * t7 - t74 * t39 - t63 * t14 + t30 * t87 - t165 * t7 + t165 * t9 - t152 * t4 - t16 * t116 - t16 * t119 + t16 * t121 + t16 * t123 - t93 * t17;
00123         const double t176 = mg[1][0] * mf[0][1];
00124         const double t179 = mg[1][0] * mf[3][1];
00125         const double t183 = mg[2][0] * mf[3][1];
00126         const double t191 = -t176 * t4 + t176 * t2 - t179 * t25 + t179 * t27 - t111 * t19 - t183 * t37 + t183 * t39 + t41 * t154 + t41 * t149 - t41 * t141 - t41 * t146 + t52 * t25;
00127         const double t196 = mg[0][0] * mf[1][1];
00128         const double t199 = mg[0][0] * mf[2][1];
00129         const double t202 = mg[0][0] * mf[3][1];
00130         const double t207 = -t30 * t158 + t30 * t143 + t114 * t19 + t21 * t81 - t196 * t2 + t196 * t4 + t199 * t7 - t199 * t9 - t202 * t12 + t202 * t14 + t21 * t156 + t44 * t101;
00131         const double t211 = mg[2][0] * mg[1][1];
00132         const double t217 = mg[2][0] * mg[3][1];
00133         const double t224 = mg[3][0] * mg[1][1];
00134         const double t232 = t211 * t19 + t183 * t65 + t183 * t67 - t183 * t69 - t183 * t71 + t217 * t37 - t217 * t39 + t57 * t95 + t57 * t89 - t57 * t91 - t57 * t101 + t224 * t27 + t52 * t95 + t52 * t89 - t63 * t154 - t54 * t154 - t54 * t149 + t54 * t146;
00135         const double t234 = mg[3][0] * mg[0][1];
00136         const double t240 = mg[3][0] * mg[2][1];
00137         const double t246 = mg[0][2] * mg[2][3];
00138         const double t250 = mg[0][0] * mg[2][1];
00139         const double t255 = -t224 * t25 - t234 * t12 - t60 * t65 - t60 * t67 + t60 * t69 + t60 * t71 - t240 * t37 + t240 * t39 + t234 * t14 - t93 * t121 - t93 * t123 - t24 * t246 - t103 * t95 - t103 * t89 + t250 * t9 + t111 * t121 + t63 * t146 - t202 * t141;
00140         const double t262 = mg[0][3] * mg[2][2];
00141         const double t268 = mg[1][0] * mg[0][1];
00142         const double t275 = mg[1][0] * mg[2][1];
00143         const double t278 = t54 * t141 + t109 * t79 + t133 * t65 + t103 * t91 + t103 * t101 + t24 * t262 - t176 * t156 - t176 * t81 + t176 * t106 + t176 * t85 - t268 * t2 + t268 * t4 + t114 * t116 + t114 * t119 - t114 * t121 - t114 * t123 + t275 * t17 - t275 * t19;
00144         const double t283 = mg[2][0] * mg[0][1];
00145         const double t294 = mg[2][2] * mg[3][3];
00146         const double t296 = mg[1][2] * mg[2][3];
00147         const double t298 = mg[1][3] * mg[2][2];
00148         const double t301 = -t133 * t71 + t109 * t158 - t109 * t143 - t109 * t87 + t283 * t7 - t283 * t9 - t111 * t116 - t111 * t119 + t111 * t123 - t211 * t17 + t202 * t154 + t196 * t156 - t199 * t158 - t250 * t7 - t21 * t294 - t41 * t296 + t41 * t298 - t63 * t149;
00149         const double t306 = mg[0][2] * mg[1][3];
00150         const double t308 = mg[0][3] * mg[1][2];
00151         const double t317 = mg[2][3] * mg[3][2];
00152         const double t323 = mg[0][2] * mg[3][3];
00153         const double t325 = mg[0][3] * mg[3][2];
00154         const double t327 = -t52 * t91 - t52 * t101 - t47 * t306 + t47 * t308 - t74 * t65 - t74 * t67 + t139 * t143 - t125 * t119 + t74 * t69 + t74 * t71 + t63 * t141 + t21 * t317 - t152 * t156 - t152 * t81 + t152 * t106 + t152 * t85 + t16 * t323 - t16 * t325;
00155         const double t334 = mg[1][0] * mg[3][1];
00156         const double t337 = mg[1][2] * mg[3][3];
00157         const double t339 = mg[1][3] * mg[3][2];
00158         const double t346 = mg[0][0] * mg[1][1];
00159         const double t350 = t93 * t116 + t93 * t119 - t179 * t95 - t179 * t89 + t179 * t91 + t179 * t101 - t334 * t27 + t334 * t25 + t30 * t337 - t30 * t339 + t165 * t79 + t165 * t158 - t165 * t143 - t165 * t87 - t202 * t146 - t346 * t4 + t202 * t149 - t33 * t323;
00160         const double t370 = t33 * t325 - t125 * t116 + t125 * t121 + t125 * t123 + t36 * t306 - t36 * t308 + t133 * t67 - t133 * t69 + t199 * t143 - t139 * t79 - t6 * t337 - t139 * t158 + t11 * t296 + t1 * t294 - t83 * t85 - t1 * t317 + t83 * t81 + t139 * t87;
00161         const double t371 = mg[0][0] * mg[3][1];
00162         const double t390 = t371 * t12 - t371 * t14 - t199 * t79 + t196 * t81 + t346 * t2 - t196 * t85 + t199 * t87 - t196 * t106 + t76 * t154 + t83 * t156 + t76 * t149 - t76 * t141 - t76 * t146 - t11 * t298 + t6 * t339 - t83 * t106 + t44 * t246 - t44 * t262;
00163         const double t406 = -t76 * t296 + t76 * t298 + t139 * t337 - t52 * t246 + t54 * t296 - t54 * t298 + t224 * t91 + t224 * t101 + t60 * t306 - t60 * t308 + t240 * t65 + t240 * t67;
00164         const double t419 = -t240 * t69 - t240 * t71 - t93 * t323 + t93 * t325 + t83 * t317 - t283 * t79 + t165 * t339 - t103 * t262 + t176 * t294 - t176 * t317 + t268 * t156 + t268 * t81;
00165         const double t433 = -t268 * t106 - t268 * t85 - t114 * t323 + t114 * t325 - t275 * t116 - t275 * t119 + t275 * t121 + t275 * t123 + t133 * t308 - t109 * t337 + t109 * t339 - t283 * t158;
00166         const double t446 = t283 * t143 + t283 * t87 + t111 * t323 - t111 * t325 + t211 * t116 + t211 * t119 + t52 * t262 + t74 * t306 - t346 * t81 + t346 * t106 + t202 * t298 - t199 * t339;
00167         const double t461 = -t183 * t306 + t183 * t308 - t217 * t65 - t217 * t67 + t217 * t69 + t217 * t71 - t234 * t141 - t234 * t146 - t57 * t246 + t57 * t262 - t224 * t95 - t224 * t89;
00168         const double t474 = t234 * t149 + t250 * t79 - t74 * t308 + t63 * t296 - t63 * t298 - t250 * t87 + t152 * t294 - t152 * t317 + t179 * t246 + t334 * t95 + t334 * t89 - t334 * t91;
00169         const double t488 = -t334 * t101 - t179 * t262 - t165 * t337 + t199 * t337 + t250 * t158 - t202 * t296 - t196 * t294 + t346 * t85 + t196 * t317 + t125 * t323 - t125 * t325 - t133 * t306;
00170         const double t501 = t234 * t154 - t139 * t339 - t83 * t294 - t250 * t143 - t371 * t154 - t346 * t156 - t371 * t149 + t371 * t141 + t371 * t146 + t103 * t246 - t211 * t121 - t211 * t123;
00171         const double t517 = t371 * t296 - t371 * t298 - t250 * t337 - t346 * t317 + t250 * t339 - t334 * t246 + t334 * t262 + t346 * t294 - t224 * t262 - t240 * t306 + t240 * t308 - t234 * t296;
00172         const double t530 = -t268 * t294 + t268 * t317 + t275 * t323 - t275 * t325 + t283 * t337 - t283 * t339 - t211 * t323 + t211 * t325 + t217 * t306 - t217 * t308 + t234 * t298 + t224 * t246;
00173 
00174         QList<double> result;
00175         result << t29 + t50;
00176         result << t73 + t97 + t118 + t136 + t160 + t174 + t191 + t207;
00177         result << t232 + t255 + t278 + t301 + t327 + t350 + t370 + t390;
00178         result << t406 + t419 + t433 + t446 + t461 + t474 + t488 + t501;
00179         result << t517 + t530;
00180 
00181         return result;
00182         }
00183 
00184 
00185 /*
00186 # Get matrices Mf and Mg.
00187 #
00188 # These matrices are such:
00189 #       Mf * [ a, b, c, 1 ] = U.X.Vt
00190 #       Mg * [ a, b, c, 1 ] = U.cofactorX.Vt
00191 #
00192 # Where 'F' is the fundamental matrix in:
00193 #       F = U.diag(r,s,0).Wt
00194 # or
00195 #       F = (U.X.Et.Wt).(W.Z.Wt)
00196 # and
00197 #       Vt = Et x Wt
00198 #       E = [0,1,0; -1,0,0; 0,0,1]
00199 # -----------------------
00200 # or
00201 #       F = U x diag(r,s,0) x Vt
00202 #
00203 */
00204 
00205 /*
00206 # Get coefficient matrix for expression::
00207 #       Mf(a,b,c,1) = U*X*V^T
00208 # Where
00209 #       X = [r, 0, a; 0, s, b; 0, 0, c]
00210 # Given 'U', 'V', 'r' and 's' values known
00211 with(VectorCalculus):
00212 with(LinearAlgebra):
00213 U := Matrix(3, 3, symbol=u):
00214 V := Matrix(3, 3, symbol=v):
00215 X := Matrix([ [r, 0, a], [0, s, b], [0, 0, c] ]):
00216 f := U.X.Transpose(V):
00217 fVector := Vector([ f[1,1], f[1,2], f[2,1], f[2,2] ]):
00218 fVectorDiffA := diff(fVector, a):
00219 fVectorDiffB := diff(fVector, b):
00220 fVectorDiffC := diff(fVector, c):
00221 fVectorDiffRes := simplify(fVector - a fVectorDiffA - b fVectorDiffB - c fVectorDiffC):
00222 Mf := Matrix([fVectorDiffA, fVectorDiffB, fVectorDiffC, fVectorDiffRes]):
00223 
00224 # Must be vector zero.
00225 simplify(Mf . Vector([a, b, c, 1]) - fVector);
00226 
00227 CodeGeneration[C](Mf, optimize = true);
00228 */
00229 QVMatrix getMfCoeffsMatrix(const QVMatrix &U, const QVMatrix &V, const double r, const double s)
00230         {
00231         double u[4][4], v[4][4];
00232         for(int i = 0; i < 4; i++)
00233                 for(int j = 0; j < 4; j++)
00234                         {
00235                         u[i][j] = U(i,j);
00236                         v[i][j] = V(i,j);
00237                         }
00238 
00239         const double t4 = u[0][0] * r;
00240         const double t6 = u[0][1] * s;
00241         const double t18 = u[1][0] * r;
00242         const double t20 = u[1][1] * s;
00243 
00244         double mf[4][4];
00245         mf[0][0] = u[0][0] * v[0][2];
00246         mf[0][1] = u[0][1] * v[0][2];
00247         mf[0][2] = u[0][2] * v[0][2];
00248         mf[0][3] = t4 * v[0][0] + t6 * v[0][1];
00249         mf[1][0] = u[0][0] * v[1][2];
00250         mf[1][1] = u[0][1] * v[1][2];
00251         mf[1][2] = u[0][2] * v[1][2];
00252         mf[1][3] = t4 * v[1][0] + t6 * v[1][1];
00253         mf[2][0] = u[1][0] * v[0][2];
00254         mf[2][1] = u[1][1] * v[0][2];
00255         mf[2][2] = u[1][2] * v[0][2];
00256         mf[2][3] = t18 * v[0][0] + t20 * v[0][1];
00257         mf[3][0] = u[1][0] * v[1][2];
00258         mf[3][1] = u[1][1] * v[1][2];
00259         mf[3][2] = u[1][2] * v[1][2];
00260         mf[3][3] = t18 * v[1][0] + t20 * v[1][1];
00261 
00262         QVMatrix Mf(4,4);
00263         for(int i = 0; i < 4; i++)
00264                 for(int j = 0; j < 4; j++)
00265                         Mf(i,j) = mf[i][j];
00266 
00267         return Mf;
00268         }
00269 
00270 /*
00271 # Get coefficient matrix for expression::
00272 #       Mf(a,b,c,1) = U*cofactorX*V^T
00273 # Where
00274 #       X = [s.c, 0, 0; 0, r.c, 0; -s.a, -r.b, r.s]
00275 # Given 'U', 'V', 'r' and 's' values known
00276 with(VectorCalculus):
00277 with(LinearAlgebra):
00278 U := Matrix(3, 3, symbol=u):
00279 V := Matrix(3, 3, symbol=v):
00280 cofactorX := Matrix([ [s.c, 0, 0], [0, r.c, 0], [-s.a, -r.b, r.s] ]):
00281 g := U.cofactorX.Transpose(V):
00282 gVector := Vector([ g[1,1], g[1,2], g[2,1], g[2,2] ]):
00283 gVectorDiffA := diff(gVector, a):
00284 gVectorDiffB := diff(gVector, b):
00285 gVectorDiffC := diff(gVector, c):
00286 gVectorDiffRes := simplify(gVector - a gVectorDiffA - b gVectorDiffB - c gVectorDiffC):
00287 Mg := Matrix([gVectorDiffA, gVectorDiffB, gVectorDiffC, gVectorDiffRes]):
00288 
00289 # Must be vector zero.
00290 simplify(Mg . Vector([a, b, c, 1]) - gVector);
00291 
00292 CodeGeneration[C](Mg, optimize = true);
00293 */
00294 
00295 QVMatrix getMgCoeffsMatrix(const QVMatrix &U, const QVMatrix &V, const double r, const double s)
00296         {
00297         double u[4][4], v[4][4];
00298         for(int i = 0; i < 4; i++)
00299                 for(int j = 0; j < 4; j++)
00300                         {
00301                         u[i][j] = U(i,j);
00302                         v[i][j] = V(i,j);
00303                         }
00304 
00305         const double t1 = u[0][2] * s;
00306         const double t3 = u[0][2] * r;
00307         const double t5 = u[0][0] * s;
00308         const double t7 = u[0][1] * r;
00309         const double t10 = s * v[0][2];
00310         const double t17 = s * v[1][2];
00311         const double t19 = u[1][2] * s;
00312         const double t21 = u[1][2] * r;
00313         const double t23 = u[1][0] * s;
00314         const double t25 = u[1][1] * r;
00315 
00316         double mg[4][4];
00317         mg[0][0] = -t1 * v[0][0];
00318         mg[0][1] = -t3 * v[0][1];
00319         mg[0][2] = t5 * v[0][0] + t7 * v[0][1];
00320         mg[0][3] = t3 * t10;
00321         mg[1][0] = -t1 * v[1][0];
00322         mg[1][1] = -t3 * v[1][1];
00323         mg[1][2] = t5 * v[1][0] + t7 * v[1][1];
00324         mg[1][3] = t3 * t17;
00325         mg[2][0] = -t19 * v[0][0];
00326         mg[2][1] = -t21 * v[0][1];
00327         mg[2][2] = t23 * v[0][0] + t25 * v[0][1];
00328         mg[2][3] = t21 * t10;
00329         mg[3][0] = -t19 * v[1][0];
00330         mg[3][1] = -t21 * v[1][1];
00331         mg[3][2] = t23 * v[1][0] + t25 * v[1][1];
00332         mg[3][3] = t21 * t17;
00333 
00334 
00335         QVMatrix Mg(4,4);
00336         for(int i = 0; i < 4; i++)
00337                 for(int j = 0; j < 4; j++)
00338                         Mg(i,j) = mg[i][j];
00339 
00340         return Mg;
00341         }
00342 
00343 QVMatrix getX(const double r, const double s, const double a, const double b, const double c)
00344         {
00345         QVMatrix X(3,3, 0.0);
00346         X(0,0) = r;
00347         X(1,1) = s;
00348         X(0,2) = a;
00349         X(1,2) = b;
00350         X(2,2) = c;
00351         return X;
00352         }
00353 
00354 QVMatrix getCofactorX(const double r, const double s, const double a, const double b, const double c)
00355         {
00356         QVMatrix cofactorX(3,3, 0.0);
00357         cofactorX(0,0) = s*c;
00358         cofactorX(1,1) = r*c;
00359         cofactorX(2,0) = -s*a;
00360         cofactorX(2,1) = -r*b;
00361         cofactorX(2,2) = r*s;
00362         return cofactorX;
00363         }
00364 
00365 double evalPolynomial(const QVVector &coeffs, const double x)
00366         {
00367         double accum = 0.0, xExp = 1.0;
00368         for(int i = 0; i < coeffs.count(); i++)
00369                 {
00370                 accum += coeffs[i]*xExp;
00371                 xExp *= x;
00372                 }
00373 
00374         return accum;
00375         }
00376 
00377 bool    directCamerasFocalsCalibration(const QVMatrix &preF, double &f1, double &f2)
00378         {
00379         const QVMatrix F = preF * ( (determinant(preF) < 0.0)?-1.0:1.0 );
00380 
00381         QVMatrix E(3,3, 0.0);
00382         E(0,1) = 1.0; E(1,0) = -1.0; E(2,2) = 1.0;
00383 
00384         QVMatrix U, W;
00385         QVVector d;
00386         singularValueDecomposition(F, U, d, W);
00387 
00388         QVMatrix V = W*E;
00389 
00390         const double det_U = determinant(U), det_V = determinant(V);
00391 
00392         // determinant(F)<0.0 <=> determinant(U)*determinant(V) < 0.0 ?
00393         // Using (-U, V) raises the same results as using (U, -V) in this procedure?
00394         if (det_U * det_V < 0.0)
00395                 {
00396                 std::cout << "[directCamerasFocalsCalibration] Error 954381." << std::endl;
00397                 std::cout << "[directCamerasFocalsCalibration] det(F) = " << determinant(F) << std::endl;
00398                 return false;
00399                 }
00400         else if (det_U < 0.0)
00401                 {
00402                 U = U * -1.0;
00403                 V = V * -1.0;
00404                 }
00405 
00406         const double r = d[0], s = d[1];
00407 
00408         const QVMatrix  Mf = getMfCoeffsMatrix(U, V, r, s),
00409                         Mg = getMgCoeffsMatrix(U, V, r, s);
00410 
00411         const QVVector coeffs = getPolynomialCoefficientsForMf_xMg(Mf, Mg).toVector();
00412 
00413         const double x = sqrt(ABS(coeffs[1] / coeffs[3]));
00414 
00415         const double polynomialValueAtX = evalPolynomial(coeffs, x);
00416         if (ABS(polynomialValueAtX) > 1e-8)
00417                 {
00418                 std::cout << "[directCamerasFocalsCalibration] Error: could not find x such Mf = x·Mg. polynomial(x) = " << ABS(polynomialValueAtX) << std::endl;
00419                 return false;
00420                 }
00421 
00422         const QVMatrix A = Mf - x * Mg;
00423 
00424         // Sanity check. The determinant of A should be close to zero.
00425         if (ABS(determinant(A) > 1e-8))
00426                 {
00427                 std::cout << "[directCamerasFocalsCalibration] Error: |det(Mf - x Mg)| = " << ABS(determinant(A)) << std::endl;
00428                 return false;
00429                 }
00430 
00431         QVVector vals;
00432         solveHomogeneous(A, vals);
00433         vals = vals / vals[3];
00434 
00435         const QVMatrix  f = U*getX(r, s, vals[0], vals[1], vals[2])*V.transpose(),
00436                                         gx = U*getCofactorX(r, s, vals[0], vals[1], vals[2])*V.transpose() * x;
00437 
00438         // Solve k1, k2 from equations:
00439         //      k1 = a1; k1 = a2;
00440         //      k2 = a3; k2 = a4;
00441         //      k1*a6 - k2*a5 = 0;
00442         const double    a1 = sqrt(ABS(f(0,2) / gx(0,2))),
00443                         a2 = sqrt(ABS(f(1,2) / gx(1,2))),
00444                         a3 = sqrt(ABS(gx(2,0) / f(2,0))),
00445                         a4 = sqrt(ABS(gx(2,1) / f(2,1))),
00446                         a5 = sqrt(ABS(f(2,2))),
00447                         a6 = sqrt(ABS(gx(2,2)));
00448 
00449         const QVMatrix  coeffsMatrix(5,2, (double[5*2]){ 1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 1.0, a6, -a5 });
00450         const QVVector  objectivesVector(5, (double[5]){ a1, a2, a3, a4, 0.0 });
00451         QVVector xVector;
00452         solveBySingularValueDecomposition(coeffsMatrix, xVector, objectivesVector);
00453 
00454         // This error should be almost zero.
00455         const double equationsError = (coeffsMatrix * xVector - objectivesVector).norm2();
00456         if (equationsError > 1e-8)
00457                 {
00458                 std::cout << "[directCamerasFocalsCalibration] Error: norm of residuals for linear equations is not close to zero: " << equationsError << std::endl;
00459                 return false;
00460                 }
00461 
00462         f1 = 1.0/xVector[0];
00463         f2 = 1.0/xVector[1];
00464 
00465         if ( (isnan(f1) or isnan(f2)))
00466                 {
00467                 std::cout << "[directCamerasFocalsCalibration] Got NaN values for focals." << std::endl;
00468                 return false;
00469                 }
00470 
00471         return true;
00472         }
00473 
00474 // Creation of a quadratic function class type
00475 double calibrationErrorForFundamentalMatrix(const QVMatrix &F, const double f1, const double f2)
00476         {
00477         const QVMatrix estimatedE = QVMatrix::diagonal(QV3DPointF(f2, f2, 1.0)) * F * QVMatrix::diagonal(QV3DPointF(f1, f1, 1.0));
00478 
00479         QVVector sv;
00480         singularValues(estimatedE, sv);
00481 
00482         return 1.0 - sv[1]/sv[0];
00483         }
00484 
00485 double QVCalibrationErrorFunction::evaluate(const QVVector &x)
00486         {
00487         const double error = calibrationErrorForFundamentalMatrix(F, x[0], x[1]);
00488         return error;
00489         }
00490 
00491 QVCalibrationErrorFunction::QVCalibrationErrorFunction(const QVMatrix &F): QVFunction<QVVector, double>(), F(F)
00492         { }
00493 
00494 void getPointMatchingsVariance( const QList<QPointFMatching> matchings,
00495                                 const double cx1, const double cy1, const double cx2, const double cy2,
00496                                 double &variance1, double &variance2)
00497         {
00498         QVVector m1x, m1y, m2x, m2y;
00499         foreach(QPointFMatching matching, matchings)
00500                 {
00501                 m1x << matching.first.x() - cx1;
00502                 m1y << matching.first.y() - cy1;
00503                 m2x << matching.second.x() - cx2;
00504                 m2y << matching.second.y() -cy2;
00505                 }
00506 
00507         variance1 = 0.5*(m1x.variance() + m1y.variance());
00508         variance2 = 0.5*(m2x.variance() + m2y.variance());
00509         }
00510 
00511 //calibrateCamerasFocals
00512 bool getCameraFocals(   const QList<QPointFMatching> &matchings,
00513                                                 double &focal1, double &focal2,
00514                                                 const QPointF principalPoint1,
00515                                                 const QPointF principalPoint2,
00516                                                 const GSLMultiminFDFMinimizerType gslMinimizerAlgorithm,
00517                                                 const int optimizationIterations)
00518         {
00519         const double    cx1 = principalPoint1.x(),
00520                                         cy1 = principalPoint1.y(),
00521                                         cx2 = principalPoint2.x(),
00522                                         cy2 = principalPoint2.y();
00523 
00524         // Correct variance of the point projections.
00525         double variance1, variance2;
00526         getPointMatchingsVariance(matchings, cx1, cy1, cx2, cy2, variance1, variance2);
00527 
00528         // Use standard deviation for initial focal correction.
00529         const QVMatrix  Q1 = QVMatrix::cameraCalibrationMatrix(sqrt(variance1), 1.0, cx1, cy1, 0.0),
00530                                         Q2 = QVMatrix::cameraCalibrationMatrix(sqrt(variance2), 1.0, cx2, cy2, 0.0);
00531 
00532         const QList<QPointFMatching> correctedMatchings = correctIntrinsics(Q1, Q2, matchings);
00533 
00534         #ifdef QVOPENCV
00535         const QVMatrix F = cvFindFundamentalMat(correctedMatchings);
00536         #else
00537         const QVMatrix F = computeFundamentalMatrix(correctedMatchings);
00538         #endif
00539 
00540         if (F == QVMatrix())
00541                 return false;
00542 
00543         // Estimate initial focals.
00544         double f1, f2;
00545         if (not directCamerasFocalsCalibration(F, f1, f2))
00546                 {
00547                 std::cout << "[calibrateCamerasFocals] Direct method for focals failed. Initializing focals to 1.0." << std::endl;
00548                 f1 = 1.0;
00549                 f2 = 1.0;
00550                 }
00551 
00552         // Optimize focals.
00553         QVCalibrationErrorFunction error(F);
00554         QVVector fs(2);
00555         fs[0] = f1;
00556         fs[1] = f2;
00557         qvGSLMinimizeFDF(error, fs, gslMinimizerAlgorithm, optimizationIterations);
00558         f1 = fs[0];
00559         f2 = fs[1];
00560 
00561         focal1 = f1 * sqrt(variance1),
00562         focal2 = f2 * sqrt(variance2);
00563 
00564         return true;
00565         }
00566 
00567 // -----------------------------------------------------
00568 
00569
src/qvmath/getCameraFocals.cpp
