MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
transform.h
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann, Nils Moehrle
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#ifndef MATH_TRANSFORM_HEADER
11#define MATH_TRANSFORM_HEADER
12
13#include "math/matrix.h"
14#include "math/matrix_svd.h"
15
17
18/*
19 * Determines the transformation between two lists of corresponding points.
20 * Minimizing sum_i (R * s * p0[i] + t - p1[i])^2
21 */
22template <typename T, int N>
23bool
24determine_transform(std::vector<math::Vector<T, N>> const& p0,
25 std::vector<math::Vector<T, N>> const& p1,
26 math::Matrix<T, N, N>* rot, T* scale, math::Vector<T, N>* trans);
27
29
30/* ------------------------ Implementation ------------------------ */
31
33
34template <typename T, int N>
35bool
37 std::vector<math::Vector<T, N>> const& p1,
38 math::Matrix<T, N, N>* rot, T* scale, math::Vector<T, N>* trans)
39{
40 if (p0.size() != p1.size())
41 throw std::invalid_argument("Dimension size mismatch");
42
43 std::size_t num_correspondences = p0.size();
44 if (num_correspondences < 3)
45 throw std::invalid_argument("At least three correspondences required");
46
47 /* Calculate centroids. */
48 math::Vector<T, N> c0(0.0), c1(0.0);
49 for (std::size_t i = 0; i < num_correspondences; ++i)
50 {
51 c0 += p0[i];
52 c1 += p1[i];
53 }
54 c0 /= static_cast<T>(num_correspondences);
55 c1 /= static_cast<T>(num_correspondences);
56
57 /* Calculate covariance and variance. */
58 T sigma2(0.0);
59 math::Matrix<T, N, N> cov(0.0);
60 for (std::size_t i = 0; i < num_correspondences; ++i)
61 {
62 math::Vector<T, N> pc0 = p0[i] - c0;
63 math::Vector<T, N> pc1 = p1[i] - c1;
64 cov += math::Matrix<T, N, 1>(pc0.begin()) *
66 sigma2 += pc0.square_norm();
67 }
68 cov /= static_cast<T>(num_correspondences);
69 sigma2 /= static_cast<T>(num_correspondences);
70
71 /* Determine rotation and scale. */
73 math::matrix_svd(cov, &U, &S, &V);
74 if (S(N - 1, N - 1) < T(MATH_SVD_DEFAULT_ZERO_THRESHOLD))
75 return false;
76
78 T s = math::matrix_trace(S) / sigma2;
79
80 /* Handle improper rotations (reflections). */
81 if (math::matrix_determinant(R) < T(0.0))
82 {
85 F(N - 1, N - 1) = T(-1.0);
86 s = math::matrix_trace(S * F) / sigma2;
87 R = V * F * U.transposed();
88 }
89
90 if (rot != nullptr)
91 *rot = R;
92
93 if (scale != nullptr)
94 *scale = s;
95
96 if (trans != nullptr)
97 *trans = c1 + (-R * s * c0);
98
99 return true;
100}
101
103
104#endif /* MATH_TRANSFORM_HEADER */
Matrix class for arbitrary dimensions and types.
Definition matrix.h:54
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
Definition matrix.h:448
Vector class for arbitrary dimensions and types.
Definition vector.h:87
T square_norm(void) const
Computes the squared norm of the vector (much cheaper).
Definition vector.h:441
T * begin(void)
Definition vector.h:583
#define MATH_NAMESPACE_BEGIN
Definition defines.h:15
#define MATH_NAMESPACE_END
Definition defines.h:16
#define MATH_SVD_DEFAULT_ZERO_THRESHOLD
Definition matrix_svd.h:30
T matrix_trace(math::Matrix< T, N, N > const &mat)
Calculates the trace of the given matrix.
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
Matrix< T, N, N > & matrix_set_identity(Matrix< T, N, N > *mat)
Sets the given square matrix to the identity matrix.
T matrix_determinant(Matrix< T, N, N > const &mat)
Calculates the determinant of the given matrix.
bool determine_transform(std::vector< math::Vector< T, N > > const &p0, std::vector< math::Vector< T, N > > const &p1, math::Matrix< T, N, N > *rot, T *scale, math::Vector< T, N > *trans)
Definition transform.h:36