1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2009 Hauke Heibel <hauke.heibel (at) gmail.com> 5 // 6 // This Source Code Form is subject to the terms of the Mozilla 7 // Public License v. 2.0. If a copy of the MPL was not distributed 8 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 9 10 #include "main.h" 11 12 #include <Eigen/Core> 13 #include <Eigen/Geometry> 14 15 #include <Eigen/LU> // required for MatrixBase::determinant 16 #include <Eigen/SVD> // required for SVD 17 18 using namespace Eigen; 19 20 // Constructs a random matrix from the unitary group U(size). 21 template <typename T> 22 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixUnitary(int size) 23 { 24 typedef T Scalar; 25 typedef typename NumTraits<Scalar>::Real RealScalar; 26 27 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType; 28 29 MatrixType Q; 30 31 int max_tries = 40; 32 double is_unitary = false; 33 34 while (!is_unitary && max_tries > 0) 35 { 36 // initialize random matrix 37 Q = MatrixType::Random(size, size); 38 39 // orthogonalize columns using the Gram-Schmidt algorithm 40 for (int col = 0; col < size; ++col) 41 { 42 typename MatrixType::ColXpr colVec = Q.col(col); 43 for (int prevCol = 0; prevCol < col; ++prevCol) 44 { 45 typename MatrixType::ColXpr prevColVec = Q.col(prevCol); 46 colVec -= colVec.dot(prevColVec)*prevColVec; 47 } 48 Q.col(col) = colVec.normalized(); 49 } 50 51 // this additional orthogonalization is not necessary in theory but should enhance 52 // the numerical orthogonality of the matrix 53 for (int row = 0; row < size; ++row) 54 { 55 typename MatrixType::RowXpr rowVec = Q.row(row); 56 for (int prevRow = 0; prevRow < row; ++prevRow) 57 { 58 typename MatrixType::RowXpr prevRowVec = Q.row(prevRow); 59 rowVec -= rowVec.dot(prevRowVec)*prevRowVec; 60 } 61 Q.row(row) = rowVec.normalized(); 62 } 63 64 // final check 65 is_unitary = Q.isUnitary(); 66 --max_tries; 67 } 68 69 if (max_tries == 0) 70 eigen_assert(false && "randMatrixUnitary: Could not construct unitary matrix!"); 71 72 return Q; 73 } 74 75 // Constructs a random matrix from the special unitary group SU(size). 76 template <typename T> 77 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixSpecialUnitary(int size) 78 { 79 typedef T Scalar; 80 typedef typename NumTraits<Scalar>::Real RealScalar; 81 82 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType; 83 84 // initialize unitary matrix 85 MatrixType Q = randMatrixUnitary<Scalar>(size); 86 87 // tweak the first column to make the determinant be 1 88 Q.col(0) *= internal::conj(Q.determinant()); 89 90 return Q; 91 } 92 93 template <typename MatrixType> 94 void run_test(int dim, int num_elements) 95 { 96 typedef typename internal::traits<MatrixType>::Scalar Scalar; 97 typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX; 98 typedef Matrix<Scalar, Eigen::Dynamic, 1> VectorX; 99 100 // MUST be positive because in any other case det(cR_t) may become negative for 101 // odd dimensions! 102 const Scalar c = internal::abs(internal::random<Scalar>()); 103 104 MatrixX R = randMatrixSpecialUnitary<Scalar>(dim); 105 VectorX t = Scalar(50)*VectorX::Random(dim,1); 106 107 MatrixX cR_t = MatrixX::Identity(dim+1,dim+1); 108 cR_t.block(0,0,dim,dim) = c*R; 109 cR_t.block(0,dim,dim,1) = t; 110 111 MatrixX src = MatrixX::Random(dim+1, num_elements); 112 src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1)); 113 114 MatrixX dst = cR_t*src; 115 116 MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements)); 117 118 const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm(); 119 VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon()); 120 } 121 122 template<typename Scalar, int Dimension> 123 void run_fixed_size_test(int num_elements) 124 { 125 typedef Matrix<Scalar, Dimension+1, Dynamic> MatrixX; 126 typedef Matrix<Scalar, Dimension+1, Dimension+1> HomMatrix; 127 typedef Matrix<Scalar, Dimension, Dimension> FixedMatrix; 128 typedef Matrix<Scalar, Dimension, 1> FixedVector; 129 130 const int dim = Dimension; 131 132 // MUST be positive because in any other case det(cR_t) may become negative for 133 // odd dimensions! 134 const Scalar c = internal::abs(internal::random<Scalar>()); 135 136 FixedMatrix R = randMatrixSpecialUnitary<Scalar>(dim); 137 FixedVector t = Scalar(50)*FixedVector::Random(dim,1); 138 139 HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1); 140 cR_t.block(0,0,dim,dim) = c*R; 141 cR_t.block(0,dim,dim,1) = t; 142 143 MatrixX src = MatrixX::Random(dim+1, num_elements); 144 src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1)); 145 146 MatrixX dst = cR_t*src; 147 148 Block<MatrixX, Dimension, Dynamic> src_block(src,0,0,dim,num_elements); 149 Block<MatrixX, Dimension, Dynamic> dst_block(dst,0,0,dim,num_elements); 150 151 HomMatrix cR_t_umeyama = umeyama(src_block, dst_block); 152 153 const Scalar error = ( cR_t_umeyama*src - dst ).array().square().sum(); 154 155 VERIFY(error < Scalar(10)*std::numeric_limits<Scalar>::epsilon()); 156 } 157 158 void test_umeyama() 159 { 160 for (int i=0; i<g_repeat; ++i) 161 { 162 const int num_elements = internal::random<int>(40,500); 163 164 // works also for dimensions bigger than 3... 165 for (int dim=2; dim<8; ++dim) 166 { 167 CALL_SUBTEST_1(run_test<MatrixXd>(dim, num_elements)); 168 CALL_SUBTEST_2(run_test<MatrixXf>(dim, num_elements)); 169 } 170 171 CALL_SUBTEST_3((run_fixed_size_test<float, 2>(num_elements))); 172 CALL_SUBTEST_4((run_fixed_size_test<float, 3>(num_elements))); 173 CALL_SUBTEST_5((run_fixed_size_test<float, 4>(num_elements))); 174 175 CALL_SUBTEST_6((run_fixed_size_test<double, 2>(num_elements))); 176 CALL_SUBTEST_7((run_fixed_size_test<double, 3>(num_elements))); 177 CALL_SUBTEST_8((run_fixed_size_test<double, 4>(num_elements))); 178 } 179 180 // Those two calls don't compile and result in meaningful error messages! 181 // umeyama(MatrixXcf(),MatrixXcf()); 182 // umeyama(MatrixXcd(),MatrixXcd()); 183 } 184
