1
2 // Copyright (c) 2010 libmv authors.
3 //
4 // Permission is hereby granted, free of charge, to any person obtaining a copy
5 // of this software and associated documentation files (the "Software"), to
6 // deal in the Software without restriction, including without limitation the
7 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
8 // sell copies of the Software, and to permit persons to whom the Software is
9 // furnished to do so, subject to the following conditions:
10 //
11 // The above copyright notice and this permission notice shall be included in
12 // all copies or substantial portions of the Software.
13 //
14 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
19 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
20 // IN THE SOFTWARE.
21
22 // This file is part of OpenMVG, an Open Multiple View Geometry C++ library.
23
24 // Copyright (c) 2012, 2013 Pierre MOULON.
25
26 // This Source Code Form is subject to the terms of the Mozilla Public
27 // License, v. 2.0. If a copy of the MPL was not distributed with this
28 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
29
30 #include "openMVG/multiview/solver_essential_five_point.hpp"
31 #include "openMVG/multiview/solver_fundamental_kernel.hpp"
32
33 namespace openMVG {
34
FivePointsNullspaceBasis(const Mat3X & x1,const Mat3X & x2)35 Mat FivePointsNullspaceBasis(const Mat3X &x1, const Mat3X &x2) {
36 Mat epipolar_constraint = Eigen::Matrix<double,9, 9>::Constant(0.0);
37 fundamental::kernel::EncodeEpipolarEquation(x1, x2, &epipolar_constraint);
38 Eigen::SelfAdjointEigenSolver<Mat> solver
39 (epipolar_constraint.transpose() * epipolar_constraint);
40 return solver.eigenvectors().leftCols<4>();
41 }
42
o1(const Vec & a,const Vec & b)43 Vec o1(const Vec &a, const Vec &b) {
44 Vec res = Vec::Zero(20);
45
46 res(coef_xx) = a(coef_x) * b(coef_x);
47 res(coef_xy) = a(coef_x) * b(coef_y)
48 + a(coef_y) * b(coef_x);
49 res(coef_xz) = a(coef_x) * b(coef_z)
50 + a(coef_z) * b(coef_x);
51 res(coef_yy) = a(coef_y) * b(coef_y);
52 res(coef_yz) = a(coef_y) * b(coef_z)
53 + a(coef_z) * b(coef_y);
54 res(coef_zz) = a(coef_z) * b(coef_z);
55 res(coef_x) = a(coef_x) * b(coef_1)
56 + a(coef_1) * b(coef_x);
57 res(coef_y) = a(coef_y) * b(coef_1)
58 + a(coef_1) * b(coef_y);
59 res(coef_z) = a(coef_z) * b(coef_1)
60 + a(coef_1) * b(coef_z);
61 res(coef_1) = a(coef_1) * b(coef_1);
62
63 return res;
64 }
65
o2(const Vec & a,const Vec & b)66 Vec o2(const Vec &a, const Vec &b) {
67 Vec res(20);
68
69 res(coef_xxx) = a(coef_xx) * b(coef_x);
70 res(coef_xxy) = a(coef_xx) * b(coef_y)
71 + a(coef_xy) * b(coef_x);
72 res(coef_xxz) = a(coef_xx) * b(coef_z)
73 + a(coef_xz) * b(coef_x);
74 res(coef_xyy) = a(coef_xy) * b(coef_y)
75 + a(coef_yy) * b(coef_x);
76 res(coef_xyz) = a(coef_xy) * b(coef_z)
77 + a(coef_yz) * b(coef_x)
78 + a(coef_xz) * b(coef_y);
79 res(coef_xzz) = a(coef_xz) * b(coef_z)
80 + a(coef_zz) * b(coef_x);
81 res(coef_yyy) = a(coef_yy) * b(coef_y);
82 res(coef_yyz) = a(coef_yy) * b(coef_z)
83 + a(coef_yz) * b(coef_y);
84 res(coef_yzz) = a(coef_yz) * b(coef_z)
85 + a(coef_zz) * b(coef_y);
86 res(coef_zzz) = a(coef_zz) * b(coef_z);
87 res(coef_xx) = a(coef_xx) * b(coef_1)
88 + a(coef_x) * b(coef_x);
89 res(coef_xy) = a(coef_xy) * b(coef_1)
90 + a(coef_x) * b(coef_y)
91 + a(coef_y) * b(coef_x);
92 res(coef_xz) = a(coef_xz) * b(coef_1)
93 + a(coef_x) * b(coef_z)
94 + a(coef_z) * b(coef_x);
95 res(coef_yy) = a(coef_yy) * b(coef_1)
96 + a(coef_y) * b(coef_y);
97 res(coef_yz) = a(coef_yz) * b(coef_1)
98 + a(coef_y) * b(coef_z)
99 + a(coef_z) * b(coef_y);
100 res(coef_zz) = a(coef_zz) * b(coef_1)
101 + a(coef_z) * b(coef_z);
102 res(coef_x) = a(coef_x) * b(coef_1)
103 + a(coef_1) * b(coef_x);
104 res(coef_y) = a(coef_y) * b(coef_1)
105 + a(coef_1) * b(coef_y);
106 res(coef_z) = a(coef_z) * b(coef_1)
107 + a(coef_1) * b(coef_z);
108 res(coef_1) = a(coef_1) * b(coef_1);
109
110 return res;
111 }
112
FivePointsPolynomialConstraints(const Mat & E_basis)113 Mat FivePointsPolynomialConstraints(const Mat &E_basis) {
114 // Build the polynomial form of E (equation (8) in Stewenius et al. [1])
115 Vec E[3][3];
116 for (int i = 0; i < 3; ++i) {
117 for (int j = 0; j < 3; ++j) {
118 E[i][j] = Vec::Zero(20);
119 E[i][j](coef_x) = E_basis(3 * i + j, 0);
120 E[i][j](coef_y) = E_basis(3 * i + j, 1);
121 E[i][j](coef_z) = E_basis(3 * i + j, 2);
122 E[i][j](coef_1) = E_basis(3 * i + j, 3);
123 }
124 }
125
126 // The constraint matrix.
127 Mat M(10, 20);
128 int mrow = 0;
129
130 // Determinant constraint det(E) = 0; equation (19) of Nister [2].
131 M.row(mrow++) = o2(o1(E[0][1], E[1][2]) - o1(E[0][2], E[1][1]), E[2][0]) +
132 o2(o1(E[0][2], E[1][0]) - o1(E[0][0], E[1][2]), E[2][1]) +
133 o2(o1(E[0][0], E[1][1]) - o1(E[0][1], E[1][0]), E[2][2]);
134
135 // Cubic singular values constraint.
136 // Equation (20).
137 Vec EET[3][3];
138 for (int i = 0; i < 3; ++i) { // Since EET is symmetric, we only compute
139 for (int j = 0; j < 3; ++j) { // its upper triangular part.
140 if (i <= j) {
141 EET[i][j] = o1(E[i][0], E[j][0])
142 + o1(E[i][1], E[j][1])
143 + o1(E[i][2], E[j][2]);
144 } else {
145 EET[i][j] = EET[j][i];
146 }
147 }
148 }
149
150 // Equation (21).
151 Vec (&L)[3][3] = EET;
152 const Vec trace = 0.5 * (EET[0][0] + EET[1][1] + EET[2][2]);
153 for (const int i : {0,1,2}) {
154 L[i][i] -= trace;
155 }
156
157 // Equation (23).
158 for (const int i : {0,1,2}) {
159 for (const int j : {0,1,2}) {
160 Vec LEij = o2(L[i][0], E[0][j])
161 + o2(L[i][1], E[1][j])
162 + o2(L[i][2], E[2][j]);
163 M.row(mrow++) = LEij;
164 }
165 }
166
167 return M;
168 }
169
FivePointsRelativePose(const Mat3X & x1,const Mat3X & x2,std::vector<Mat3> * Es)170 void FivePointsRelativePose(const Mat3X &x1,
171 const Mat3X &x2,
172 std::vector<Mat3> *Es) {
173 // Step 1: Nullspace Extraction.
174 const Eigen::Matrix<double, 9, 4> E_basis = FivePointsNullspaceBasis(x1, x2);
175
176 // Step 2: Constraint Expansion.
177 const Eigen::Matrix<double, 10, 20> E_constraints = FivePointsPolynomialConstraints(E_basis);
178
179 // Step 3: Gauss-Jordan Elimination (done thanks to a LU decomposition).
180 using Mat10 = Eigen::Matrix<double, 10, 10>;
181 Eigen::FullPivLU<Mat10> c_lu(E_constraints.block<10, 10>(0, 0));
182 const Mat10 M = c_lu.solve(E_constraints.block<10, 10>(0, 10));
183
184 // For next steps we follow the matlab code given in Stewenius et al [1].
185
186 // Build action matrix.
187
188 const Mat10 & B = M.topRightCorner<10,10>();
189 Mat10 At = Mat10::Zero(10,10);
190 At.block<3, 10>(0, 0) = B.block<3, 10>(0, 0);
191 At.row(3) = B.row(4);
192 At.row(4) = B.row(5);
193 At.row(5) = B.row(7);
194 At(6,0) = At(7,1) = At(8,3) = At(9,6) = -1;
195
196 Eigen::EigenSolver<Mat10> eigensolver(At);
197 const auto& eigenvectors = eigensolver.eigenvectors();
198 const auto& eigenvalues = eigensolver.eigenvalues();
199
200 // Build essential matrices for the real solutions.
201 Es->reserve(10);
202 for (int s = 0; s < 10; ++s) {
203 // Only consider real solutions.
204 if (eigenvalues(s).imag() != 0) {
205 continue;
206 }
207 Mat3 E;
208 Eigen::Map<Vec9 >(E.data()) =
209 E_basis * eigenvectors.col(s).tail<4>().real();
210 Es->emplace_back(E.transpose());
211 }
212 }
213
214 } // namespace openMVG
215