1 // Copyright (c) 2009 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20
21 #include <cstdio>
22
23 // TODO(keir): This code is plain unfinished! Doesn't even compile!
24
25 #include "libmv/base/vector.h"
26 #include "libmv/multiview/fundamental_kernel.h"
27 #include "libmv/numeric/numeric.h"
28 #include "libmv/numeric/poly.h"
29 #include "libmv/logging/logging.h"
30
31 namespace libmv {
32 namespace fundamental {
33 namespace kernel {
34
Solve(const Mat & x1,const Mat & x2,vector<Mat3> * F)35 void SevenPointSolver::Solve(const Mat &x1, const Mat &x2, vector<Mat3> *F) {
36 assert(2 == x1.rows());
37 assert(7 <= x1.cols());
38 assert(x1.rows() == x2.rows());
39 assert(x1.cols() == x2.cols());
40
41 // Set up the homogeneous system Af = 0 from the equations x'T*F*x = 0.
42 MatX9 A(x1.cols(), 9);
43 EncodeEpipolarEquation(x1, x2, &A);
44
45 // Find the two F matrices in the nullspace of A.
46 Vec9 f1, f2;
47 Nullspace2(&A, &f1, &f2);
48 Mat3 F1 = Map<RMat3>(f1.data());
49 Mat3 F2 = Map<RMat3>(f2.data());
50
51 // Then, use the condition det(F) = 0 to determine F. In other words, solve
52 // det(F1 + a*F2) = 0 for a.
53 double a = F1(0, 0), j = F2(0, 0),
54 b = F1(0, 1), k = F2(0, 1),
55 c = F1(0, 2), l = F2(0, 2),
56 d = F1(1, 0), m = F2(1, 0),
57 e = F1(1, 1), n = F2(1, 1),
58 f = F1(1, 2), o = F2(1, 2),
59 g = F1(2, 0), p = F2(2, 0),
60 h = F1(2, 1), q = F2(2, 1),
61 i = F1(2, 2), r = F2(2, 2);
62
63 // Run fundamental_7point_coeffs.py to get the below coefficients.
64 // The coefficients are in ascending powers of alpha, i.e. P[N]*x^N.
65 double P[4] = {
66 a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g,
67 a*e*r + a*i*n + b*f*p + b*g*o + c*d*q + c*h*m + d*h*l + e*i*j + f*g*k -
68 a*f*q - a*h*o - b*d*r - b*i*m - c*e*p - c*g*n - d*i*k - e*g*l - f*h*j,
69 a*n*r + b*o*p + c*m*q + d*l*q + e*j*r + f*k*p + g*k*o + h*l*m + i*j*n -
70 a*o*q - b*m*r - c*n*p - d*k*r - e*l*p - f*j*q - g*l*n - h*j*o - i*k*m,
71 j*n*r + k*o*p + l*m*q - j*o*q - k*m*r - l*n*p,
72 };
73
74 // Solve for the roots of P[3]*x^3 + P[2]*x^2 + P[1]*x + P[0] = 0.
75 double roots[3];
76 int num_roots = SolveCubicPolynomial(P, roots);
77
78 // Build the fundamental matrix for each solution.
79 for (int kk = 0; kk < num_roots; ++kk) {
80 F->push_back(F1 + roots[kk] * F2);
81 }
82 }
83
84 //template<bool force_essential_constraint>
Solve(const Mat & x1,const Mat & x2,vector<Mat3> * Fs)85 void EightPointSolver::Solve(const Mat &x1, const Mat &x2, vector<Mat3> *Fs) {
86 assert(2 == x1.rows());
87 assert(8 <= x1.cols());
88 assert(x1.rows() == x2.rows());
89 assert(x1.cols() == x2.cols());
90
91 MatX9 A(x1.cols(), 9);
92 EncodeEpipolarEquation(x1, x2, &A);
93
94 Vec9 f;
95 Nullspace(&A, &f);
96 Mat3 F = Map<RMat3>(f.data());
97
98 // Force the fundamental property if the A matrix has full rank.
99 if (x1.cols() > 8) {
100 Eigen::JacobiSVD<Mat3> USV(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
101 Vec3 d = USV.singularValues();
102 d[2] = 0.0;
103 F = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose();
104 }
105 Fs->push_back(F);
106 }
107
108 } // namespace kernel
109 } // namespace fundamental
110 } // namespace libmv
111