diff options
Diffstat (limited to 'test/umeyama.cpp')
| -rw-r--r-- | test/umeyama.cpp | 183 |
1 files changed, 183 insertions, 0 deletions
diff --git a/test/umeyama.cpp b/test/umeyama.cpp new file mode 100644 index 000000000..b6c9be3a5 --- /dev/null +++ b/test/umeyama.cpp @@ -0,0 +1,183 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "main.h" + +#include <Eigen/Core> +#include <Eigen/Geometry> + +#include <Eigen/LU> // required for MatrixBase::determinant +#include <Eigen/SVD> // required for SVD + +using namespace Eigen; + +// Constructs a random matrix from the unitary group U(size). +template <typename T> +Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixUnitary(int size) +{ + typedef T Scalar; + typedef typename NumTraits<Scalar>::Real RealScalar; + + typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType; + + MatrixType Q; + + int max_tries = 40; + double is_unitary = false; + + while (!is_unitary && max_tries > 0) + { + // initialize random matrix + Q = MatrixType::Random(size, size); + + // orthogonalize columns using the Gram-Schmidt algorithm + for (int col = 0; col < size; ++col) + { + typename MatrixType::ColXpr colVec = Q.col(col); + for (int prevCol = 0; prevCol < col; ++prevCol) + { + typename MatrixType::ColXpr prevColVec = Q.col(prevCol); + colVec -= colVec.dot(prevColVec)*prevColVec; + } + Q.col(col) = colVec.normalized(); + } + + // this additional orthogonalization is not necessary in theory but should enhance + // the numerical orthogonality of the matrix + for (int row = 0; row < size; ++row) + { + typename MatrixType::RowXpr rowVec = Q.row(row); + for (int prevRow = 0; prevRow < row; ++prevRow) + { + typename MatrixType::RowXpr prevRowVec = Q.row(prevRow); + rowVec -= rowVec.dot(prevRowVec)*prevRowVec; + } + Q.row(row) = rowVec.normalized(); + } + + // final check + is_unitary = Q.isUnitary(); + --max_tries; + } + + if (max_tries == 0) + eigen_assert(false && "randMatrixUnitary: Could not construct unitary matrix!"); + + return Q; +} + +// Constructs a random matrix from the special unitary group SU(size). +template <typename T> +Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixSpecialUnitary(int size) +{ + typedef T Scalar; + typedef typename NumTraits<Scalar>::Real RealScalar; + + typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType; + + // initialize unitary matrix + MatrixType Q = randMatrixUnitary<Scalar>(size); + + // tweak the first column to make the determinant be 1 + Q.col(0) *= internal::conj(Q.determinant()); + + return Q; +} + +template <typename MatrixType> +void run_test(int dim, int num_elements) +{ + typedef typename internal::traits<MatrixType>::Scalar Scalar; + typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX; + typedef Matrix<Scalar, Eigen::Dynamic, 1> VectorX; + + // MUST be positive because in any other case det(cR_t) may become negative for + // odd dimensions! + const Scalar c = internal::abs(internal::random<Scalar>()); + + MatrixX R = randMatrixSpecialUnitary<Scalar>(dim); + VectorX t = Scalar(50)*VectorX::Random(dim,1); + + MatrixX cR_t = MatrixX::Identity(dim+1,dim+1); + cR_t.block(0,0,dim,dim) = c*R; + cR_t.block(0,dim,dim,1) = t; + + MatrixX src = MatrixX::Random(dim+1, num_elements); + src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1)); + + MatrixX dst = cR_t*src; + + MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements)); + + const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm(); + VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon()); +} + +template<typename Scalar, int Dimension> +void run_fixed_size_test(int num_elements) +{ + typedef Matrix<Scalar, Dimension+1, Dynamic> MatrixX; + typedef Matrix<Scalar, Dimension+1, Dimension+1> HomMatrix; + typedef Matrix<Scalar, Dimension, Dimension> FixedMatrix; + typedef Matrix<Scalar, Dimension, 1> FixedVector; + + const int dim = Dimension; + + // MUST be positive because in any other case det(cR_t) may become negative for + // odd dimensions! + const Scalar c = internal::abs(internal::random<Scalar>()); + + FixedMatrix R = randMatrixSpecialUnitary<Scalar>(dim); + FixedVector t = Scalar(50)*FixedVector::Random(dim,1); + + HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1); + cR_t.block(0,0,dim,dim) = c*R; + cR_t.block(0,dim,dim,1) = t; + + MatrixX src = MatrixX::Random(dim+1, num_elements); + src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1)); + + MatrixX dst = cR_t*src; + + Block<MatrixX, Dimension, Dynamic> src_block(src,0,0,dim,num_elements); + Block<MatrixX, Dimension, Dynamic> dst_block(dst,0,0,dim,num_elements); + + HomMatrix cR_t_umeyama = umeyama(src_block, dst_block); + + const Scalar error = ( cR_t_umeyama*src - dst ).array().square().sum(); + + VERIFY(error < Scalar(10)*std::numeric_limits<Scalar>::epsilon()); +} + +void test_umeyama() +{ + for (int i=0; i<g_repeat; ++i) + { + const int num_elements = internal::random<int>(40,500); + + // works also for dimensions bigger than 3... + for (int dim=2; dim<8; ++dim) + { + CALL_SUBTEST_1(run_test<MatrixXd>(dim, num_elements)); + CALL_SUBTEST_2(run_test<MatrixXf>(dim, num_elements)); + } + + CALL_SUBTEST_3((run_fixed_size_test<float, 2>(num_elements))); + CALL_SUBTEST_4((run_fixed_size_test<float, 3>(num_elements))); + CALL_SUBTEST_5((run_fixed_size_test<float, 4>(num_elements))); + + CALL_SUBTEST_6((run_fixed_size_test<double, 2>(num_elements))); + CALL_SUBTEST_7((run_fixed_size_test<double, 3>(num_elements))); + CALL_SUBTEST_8((run_fixed_size_test<double, 4>(num_elements))); + } + + // Those two calls don't compile and result in meaningful error messages! + // umeyama(MatrixXcf(),MatrixXcf()); + // umeyama(MatrixXcd(),MatrixXcd()); +} |