10#ifndef MATH_TRANSFORM_HEADER
11#define MATH_TRANSFORM_HEADER
22template <
typename T,
int N>
34template <
typename T,
int N>
40 if (p0.size() != p1.size())
41 throw std::invalid_argument(
"Dimension size mismatch");
43 std::size_t num_correspondences = p0.size();
44 if (num_correspondences < 3)
45 throw std::invalid_argument(
"At least three correspondences required");
49 for (std::size_t i = 0; i < num_correspondences; ++i)
54 c0 /=
static_cast<T
>(num_correspondences);
55 c1 /=
static_cast<T
>(num_correspondences);
60 for (std::size_t i = 0; i < num_correspondences; ++i)
68 cov /=
static_cast<T
>(num_correspondences);
69 sigma2 /=
static_cast<T
>(num_correspondences);
85 F(N - 1, N - 1) = T(-1.0);
97 *trans = c1 + (-R * s * c0);
Matrix class for arbitrary dimensions and types.
Matrix< T, M, N > transposed(void) const
Returns a transposed copy of self by treating rows as columns.
Vector class for arbitrary dimensions and types.
T square_norm(void) const
Computes the squared norm of the vector (much cheaper).
#define MATH_NAMESPACE_BEGIN
#define MATH_NAMESPACE_END
#define MATH_SVD_DEFAULT_ZERO_THRESHOLD
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).
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)