MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
fundamental.cc
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann
3 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
4 * All rights reserved.
5 *
6 * This software may be modified and distributed under the terms
7 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
8 */
9
10#include <limits>
11#include <stdexcept>
12
13#include "math/matrix_tools.h"
14#include "math/matrix_svd.h"
15#include "math/functions.h"
16#include "sfm/fundamental.h"
17
19
20namespace
21{
27 template <typename T>
28 void
29 cross_product_matrix (math::Vector<T, 3> const& v,
31 {
32 result->fill(T(0));
33 (*result)(0, 1) = -v[2];
34 (*result)(0, 2) = v[1];
35 (*result)(1, 0) = v[2];
36 (*result)(1, 2) = -v[0];
37 (*result)(2, 0) = -v[1];
38 (*result)(2, 1) = v[0];
39 }
40
41} // namespace
42
43bool
45 FundamentalMatrix* result)
46{
47 if (points.size() < 8)
48 throw std::invalid_argument("At least 8 points required");
49
50 /* Create Nx9 matrix A. Each correspondence creates on row in A. */
51 std::vector<double> A(points.size() * 9);
52 for (std::size_t i = 0; i < points.size(); ++i)
53 {
54 Correspondence2D2D const& p = points[i];
55 A[i * 9 + 0] = p.p2[0] * p.p1[0];
56 A[i * 9 + 1] = p.p2[0] * p.p1[1];
57 A[i * 9 + 2] = p.p2[0] * 1.0;
58 A[i * 9 + 3] = p.p2[1] * p.p1[0];
59 A[i * 9 + 4] = p.p2[1] * p.p1[1];
60 A[i * 9 + 5] = p.p2[1] * 1.0;
61 A[i * 9 + 6] = 1.0 * p.p1[0];
62 A[i * 9 + 7] = 1.0 * p.p1[1];
63 A[i * 9 + 8] = 1.0 * 1.0;
64 }
65
66 /* Compute fundamental matrix using SVD. */
67 std::vector<double> V(9 * 9);
68 math::matrix_svd<double>(&A[0], points.size(), 9, nullptr, nullptr, &V[0]);
69
70 /* Use last column of V as solution. */
71 for (int i = 0; i < 9; ++i)
72 (*result)[i] = V[i * 9 + 8];
73
74 return true;
75}
76
77bool
78fundamental_8_point (Eight2DPoints const& points_view_1,
79 Eight2DPoints const& points_view_2, FundamentalMatrix* result)
80{
81 /*
82 * Create 8x9 matrix A. Each pair of input points creates on row in A.
83 */
85 for (int i = 0; i < 8; ++i)
86 {
87 math::Vector<double, 3> p1 = points_view_1.col(i);
88 math::Vector<double, 3> p2 = points_view_2.col(i);
89 A(i, 0) = p2[0] * p1[0];
90 A(i, 1) = p2[0] * p1[1];
91 A(i, 2) = p2[0] * 1.0;
92 A(i, 3) = p2[1] * p1[0];
93 A(i, 4) = p2[1] * p1[1];
94 A(i, 5) = p2[1] * 1.0;
95 A(i, 6) = 1.0 * p1[0];
96 A(i, 7) = 1.0 * p1[1];
97 A(i, 8) = 1.0 * 1.0;
98 }
99
100 /*
101 * The fundamental matrix F is created from the singular
102 * vector corresponding to the smallest eigenvalue of A.
103 */
105 math::matrix_svd<double, 8, 9>(A, nullptr, nullptr, &V);
107 std::copy(*f, *f + 9, **result);
108
109 return true;
110}
111
112void
114{
115 /*
116 * Constraint enforcement. The fundamental matrix F has rank 2 and 7
117 * degrees of freedom. However, F' computed from point correspondences may
118 * not have rank 2 and it needs to be enforced. To this end, the SVD is
119 * used: F' = USV*, F = UDV* where D = diag(s1, s2, 0) and s1 and s2
120 * are the largest and second largest eigenvalues of F.
121 */
123 math::matrix_svd(*matrix, &U, &S, &V);
124 S(2, 2) = 0.0;
125 *matrix = U * S * V.transposed();
126}
127
128void
130{
131 /*
132 * Constraint enforcement. The essential matrix E has rank 2 and 5 degrees
133 * of freedom. However, E' computed from normalized image correspondences
134 * may not have rank 2 and it needs to be enforced. To this end, the SVD is
135 * used: F' = USV*, F = UDV* where D = diag(s, s, 0), s = (s1 + s2) / 2
136 * and s1 and s2 are the largest and second largest eigenvalues of F.
137 */
139 math::matrix_svd(*matrix, &U, &S, &V);
140 double avg = (S(0, 0) + S(0, 1)) / 2.0;
141 S(0, 0) = avg;
142 S(1, 1) = avg;
143 S(2, 2) = 0.0;
144 *matrix = U * S * V.transposed();
145}
146
147void
149 std::vector<CameraPose>* result)
150{
151 /*
152 * The pose [R|t] for the second camera is extracted from the essential
153 * matrix E and the first camera is given in canonical form [I|0].
154 * The SVD of E = USV is computed. The scale of S' diagonal entries is
155 * irrelevant and S is assumed to be diag(1,1,0).
156 * Details can be found in [Hartley, Zisserman, Sect. 9.6.1].
157 */
158
160 W(0, 1) = -1.0; W(1, 0) = 1.0; W(2, 2) = 1.0;
162 Wt(0, 1) = 1.0; Wt(1, 0) = -1.0; Wt(2, 2) = 1.0;
163
165 math::matrix_svd(matrix, &U, &S, &V);
166
167 // This seems to ensure that det(R) = 1 (instead of -1).
168 // FIXME: Is this the correct way to do it?
169 if (math::matrix_determinant(U) < 0.0)
170 for (int i = 0; i < 3; ++i)
171 U(i,2) = -U(i,2);
172 if (math::matrix_determinant(V) < 0.0)
173 for (int i = 0; i < 3; ++i)
174 V(i,2) = -V(i,2);
175
176 V.transpose();
177 result->clear();
178 result->resize(4);
179 result->at(0).R = U * W * V;
180 result->at(1).R = result->at(0).R;
181 result->at(2).R = U * Wt * V;
182 result->at(3).R = result->at(2).R;
183 result->at(0).t = U.col(2);
184 result->at(1).t = -result->at(0).t;
185 result->at(2).t = result->at(0).t;
186 result->at(3).t = -result->at(0).t;
187
188 // FIXME: Temporary sanity check.
189 if (!MATH_EPSILON_EQ(math::matrix_determinant(result->at(0).R), 1.0, 1e-3))
190 throw std::runtime_error("Invalid rotation matrix");
191}
192
193void
195 FundamentalMatrix* result)
196{
197 /*
198 * The fundamental matrix is obtained from camera matrices.
199 * See Hartley Zisserman (9.1): F = [e2] P2 P1^.
200 * Where P1, P2 are the camera matrices, and P^ is the inverse of P.
201 * e2 is the epipole in the second camera and [e2] is the cross product
202 * matrix for e2.
203 */
205 cam1.fill_p_matrix(&P1);
206 cam2.fill_p_matrix(&P2);
207
208 math::Vec4d c1(cam1.R.transposed() * -cam1.t, 1.0);
209 math::Vec3d e2 = P2 * c1;
210
212 cross_product_matrix(e2, &ex);
213
214 // FIXME: The values in the fundamental matrix can become huge.
215 // The input projection matrix should be given in unit coodinates,
216 // not pixel coordinates? Test and document that.
217
219 math::matrix_pseudo_inverse(P1, &P1inv);
220 *result = ex * P2 * P1inv;
221}
222
223
224double
226{
227 /*
228 * Computes the Sampson distance SD for a given match and fundamental
229 * matrix. SD is computed as [Sect 11.4.3, Hartley, Zisserman]:
230 *
231 * SD = (x'Fx)^2 / ( (Fx)_1^2 + (Fx)_2^2 + (x'F)_1^2 + (x'F)_2^2 )
232 */
233
234 double p2_F_p1 = 0.0;
235 p2_F_p1 += m.p2[0] * (m.p1[0] * F[0] + m.p1[1] * F[1] + F[2]);
236 p2_F_p1 += m.p2[1] * (m.p1[0] * F[3] + m.p1[1] * F[4] + F[5]);
237 p2_F_p1 += 1.0 * (m.p1[0] * F[6] + m.p1[1] * F[7] + F[8]);
238 p2_F_p1 *= p2_F_p1;
239
240 double sum = 0.0;
241 sum += math::fastpow(m.p1[0] * F[0] + m.p1[1] * F[1] + F[2], 2);
242 sum += math::fastpow(m.p1[0] * F[3] + m.p1[1] * F[4] + F[5], 2);
243 sum += math::fastpow(m.p2[0] * F[0] + m.p2[1] * F[3] + F[6], 2);
244 sum += math::fastpow(m.p2[0] * F[1] + m.p2[1] * F[4] + F[7], 2);
245
246 return p2_F_p1 / sum;
247}
248
Matrix class for arbitrary dimensions and types.
Definition matrix.h:54
Vector< T, N > col(int index) const
Returns a column of the matrix as vector.
Definition matrix.h:330
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
Definition matrix.h:448
Matrix< T, N, M > & fill(T const &value)
Fills all vector elements with the given value.
Definition matrix.h:294
Matrix< T, M, N > & transpose(void)
Transpose the current matrix.
Definition matrix.h:441
Vector class for arbitrary dimensions and types.
Definition vector.h:87
#define MATH_EPSILON_EQ(x, v, eps)
Definition defines.h:96
T fastpow(T const &base, unsigned int exp)
Takes base to the integer power of 'exp'.
Definition functions.h:235
void matrix_pseudo_inverse(Matrix< T, M, N > const &A, Matrix< T, N, M > *result, T const &epsilon=T(1e-12))
Computes the Moore–Penrose pseudoinverse of matrix A using the SVD.
Definition matrix_svd.h:902
void matrix_svd(T const *mat_a, int rows, int cols, T *mat_u, T *vec_s, T *mat_v, T const &epsilon=T(1e-12))
SVD for dynamic-size matrices A of size MxN (M rows, N columns).
Definition matrix_svd.h:699
T matrix_determinant(Matrix< T, N, N > const &mat)
Calculates the determinant of the given matrix.
void enforce_fundamental_constraints(FundamentalMatrix *matrix)
Constraints the given matrix to have TWO NON-ZERO eigenvalues.
bool fundamental_least_squares(Correspondences2D2D const &points, FundamentalMatrix *result)
Algorithm to compute the fundamental or essential matrix from image correspondences.
void enforce_essential_constraints(EssentialMatrix *matrix)
Constraints the given matrix to have TWO EQUAL NON-ZERO eigenvalues.
std::vector< Correspondence2D2D > Correspondences2D2D
void fundamental_from_pose(CameraPose const &cam1, CameraPose const &cam2, FundamentalMatrix *result)
Computes the fundamental matrix corresponding to cam1 and cam2.
void pose_from_essential(EssentialMatrix const &matrix, std::vector< CameraPose > *result)
Retrieves the camera matrices from the essential matrix.
bool fundamental_8_point(Eight2DPoints const &points_view_1, Eight2DPoints const &points_view_2, FundamentalMatrix *result)
Algorithm to compute the fundamental or essential matrix from 8 image correspondences.
double sampson_distance(FundamentalMatrix const &F, Correspondence2D2D const &m)
Computes the Sampson distance for an image correspondence given the fundamental matrix between two vi...
#define SFM_NAMESPACE_END
Definition defines.h:14
#define SFM_NAMESPACE_BEGIN
Definition defines.h:13
The camera pose is the 3x4 matrix P = K [R | t].
Definition camera_pose.h:40
math::Matrix< double, 3, 3 > R
Definition camera_pose.h:59
void fill_p_matrix(math::Matrix< double, 3, 4 > *result) const
Returns the P matrix as the product K [R | t].
Definition camera_pose.h:81
math::Vector< double, 3 > t
Definition camera_pose.h:60
Two image coordinates which correspond to each other in terms of observing the same point in the scen...