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];
47 if (points.size() < 8)
48 throw std::invalid_argument(
"At least 8 points required");
51 std::vector<double> A(points.size() * 9);
52 for (std::size_t i = 0; i < points.size(); ++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;
67 std::vector<double> V(9 * 9);
68 math::matrix_svd<double>(&A[0], points.size(), 9,
nullptr,
nullptr, &V[0]);
71 for (
int i = 0; i < 9; ++i)
72 (*result)[i] = V[i * 9 + 8];
85 for (
int i = 0; i < 8; ++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];
105 math::matrix_svd<double, 8, 9>(A,
nullptr,
nullptr, &V);
107 std::copy(*f, *f + 9, **result);
140 double avg = (S(0, 0) + S(0, 1)) / 2.0;
149 std::vector<CameraPose>* result)
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;
170 for (
int i = 0; i < 3; ++i)
173 for (
int i = 0; i < 3; ++i)
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;
190 throw std::runtime_error(
"Invalid rotation matrix");
212 cross_product_matrix(e2, &ex);
220 *result = ex * P2 * P1inv;
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]);
246 return p2_F_p1 / sum;
Matrix class for arbitrary dimensions and types.
Vector< T, N > col(int index) const
Returns a column of the matrix as vector.
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
Matrix< T, N, M > & fill(T const &value)
Fills all vector elements with the given value.
Matrix< T, M, N > & transpose(void)
Transpose the current matrix.
Vector class for arbitrary dimensions and types.
#define MATH_EPSILON_EQ(x, v, eps)
T fastpow(T const &base, unsigned int exp)
Takes base to the integer power of 'exp'.
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.
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).
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
#define SFM_NAMESPACE_BEGIN
The camera pose is the 3x4 matrix P = K [R | t].
math::Matrix< double, 3, 3 > R
void fill_p_matrix(math::Matrix< double, 3, 4 > *result) const
Returns the P matrix as the product K [R | t].
math::Vector< double, 3 > t
Two image coordinates which correspond to each other in terms of observing the same point in the scen...