blob: 2e8092434820b7b23d1fddbbacf037524230cfd6 [file] [log] [blame]
 // This file is part of Eigen, a lightweight C++ template library // for linear algebra. // // Copyright (C) 2009 Hauke Heibel // // 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 #include #include // required for MatrixBase::determinant #include // required for SVD using namespace Eigen; // Constructs a random matrix from the unitary group U(size). template Eigen::Matrix randMatrixUnitary(int size) { typedef T Scalar; typedef Eigen::Matrix 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 Eigen::Matrix randMatrixSpecialUnitary(int size) { typedef T Scalar; typedef Eigen::Matrix MatrixType; // initialize unitary matrix MatrixType Q = randMatrixUnitary(size); // tweak the first column to make the determinant be 1 Q.col(0) *= numext::conj(Q.determinant()); return Q; } template void run_test(int dim, int num_elements) { using std::abs; typedef typename internal::traits::Scalar Scalar; typedef Matrix MatrixX; typedef Matrix VectorX; // MUST be positive because in any other case det(cR_t) may become negative for // odd dimensions! const Scalar c = abs(internal::random()); MatrixX R = randMatrixSpecialUnitary(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::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::epsilon()); } template void run_fixed_size_test(int num_elements) { using std::abs; typedef Matrix MatrixX; typedef Matrix HomMatrix; typedef Matrix FixedMatrix; typedef Matrix FixedVector; const int dim = Dimension; // MUST be positive because in any other case det(cR_t) may become negative for // odd dimensions! // Also if c is to small compared to t.norm(), problem is ill-posed (cf. Bug 744) const Scalar c = internal::random(0.5, 2.0); FixedMatrix R = randMatrixSpecialUnitary(dim); FixedVector t = Scalar(32)*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::Constant(num_elements, Scalar(1)); MatrixX dst = cR_t*src; Block src_block(src,0,0,dim,num_elements); Block 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 ).squaredNorm(); VERIFY(error < Scalar(16)*std::numeric_limits::epsilon()); } void test_umeyama() { for (int i=0; i(40,500); // works also for dimensions bigger than 3... for (int dim=2; dim<8; ++dim) { CALL_SUBTEST_1(run_test(dim, num_elements)); CALL_SUBTEST_2(run_test(dim, num_elements)); } CALL_SUBTEST_3((run_fixed_size_test(num_elements))); CALL_SUBTEST_4((run_fixed_size_test(num_elements))); CALL_SUBTEST_5((run_fixed_size_test(num_elements))); CALL_SUBTEST_6((run_fixed_size_test(num_elements))); CALL_SUBTEST_7((run_fixed_size_test(num_elements))); CALL_SUBTEST_8((run_fixed_size_test(num_elements))); } // Those two calls don't compile and result in meaningful error messages! // umeyama(MatrixXcf(),MatrixXcf()); // umeyama(MatrixXcd(),MatrixXcd()); }