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 Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType; 26 27 MatrixType Q; 28 29 int max_tries = 40; 30 double is_unitary = false; 31 32 while (!is_unitary && max_tries > 0) 33 { 34 // initialize random matrix 35 Q = MatrixType::Random(size, size); 36 37 // orthogonalize columns using the Gram-Schmidt algorithm 38 for (int col = 0; col < size; ++col) 39 { 40 typename MatrixType::ColXpr colVec = Q.col(col); 41 for (int prevCol = 0; prevCol < col; ++prevCol) 42 { 43 typename MatrixType::ColXpr prevColVec = Q.col(prevCol); 44 colVec -= colVec.dot(prevColVec)*prevColVec; 45 } 46 Q.col(col) = colVec.normalized(); 47 } 48 49 // this additional orthogonalization is not necessary in theory but should enhance 50 // the numerical orthogonality of the matrix 51 for (int row = 0; row < size; ++row) 52 { 53 typename MatrixType::RowXpr rowVec = Q.row(row); 54 for (int prevRow = 0; prevRow < row; ++prevRow) 55 { 56 typename MatrixType::RowXpr prevRowVec = Q.row(prevRow); 57 rowVec -= rowVec.dot(prevRowVec)*prevRowVec; 58 } 59 Q.row(row) = rowVec.normalized(); 60 } 61 62 // final check 63 is_unitary = Q.isUnitary(); 64 --max_tries; 65 } 66 67 if (max_tries == 0) 68 eigen_assert(false && "randMatrixUnitary: Could not construct unitary matrix!"); 69 70 return Q; 71 } 72 73 // Constructs a random matrix from the special unitary group SU(size). 74 template <typename T> 75 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixSpecialUnitary(int size) 76 { 77 typedef T Scalar; 78 79 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType; 80 81 // initialize unitary matrix 82 MatrixType Q = randMatrixUnitary<Scalar>(size); 83 84 // tweak the first column to make the determinant be 1 85 Q.col(0) *= numext::conj(Q.determinant()); 86 87 return Q; 88 } 89 90 template <typename MatrixType> 91 void run_test(int dim, int num_elements) 92 { 93 using std::abs; 94 typedef typename internal::traits<MatrixType>::Scalar Scalar; 95 typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX; 96 typedef Matrix<Scalar, Eigen::Dynamic, 1> VectorX; 97 98 // MUST be positive because in any other case det(cR_t) may become negative for 99 // odd dimensions! 100 const Scalar c = abs(internal::random<Scalar>()); 101 102 MatrixX R = randMatrixSpecialUnitary<Scalar>(dim); 103 VectorX t = Scalar(50)*VectorX::Random(dim,1); 104 105 MatrixX cR_t = MatrixX::Identity(dim+1,dim+1); 106 cR_t.block(0,0,dim,dim) = c*R; 107 cR_t.block(0,dim,dim,1) = t; 108 109 MatrixX src = MatrixX::Random(dim+1, num_elements); 110 src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1)); 111 112 MatrixX dst = cR_t*src; 113 114 MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements)); 115 116 const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm(); 117 VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon()); 118 } 119 120 template<typename Scalar, int Dimension> 121 void run_fixed_size_test(int num_elements) 122 { 123 using std::abs; 124 typedef Matrix<Scalar, Dimension+1, Dynamic> MatrixX; 125 typedef Matrix<Scalar, Dimension+1, Dimension+1> HomMatrix; 126 typedef Matrix<Scalar, Dimension, Dimension> FixedMatrix; 127 typedef Matrix<Scalar, Dimension, 1> FixedVector; 128 129 const int dim = Dimension; 130 131 // MUST be positive because in any other case det(cR_t) may become negative for 132 // odd dimensions! 133 // Also if c is to small compared to t.norm(), problem is ill-posed (cf. Bug 744) 134 const Scalar c = internal::random<Scalar>(0.5, 2.0); 135 136 FixedMatrix R = randMatrixSpecialUnitary<Scalar>(dim); 137 FixedVector t = Scalar(32)*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 ).squaredNorm(); 154 155 VERIFY(error < Scalar(16)*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
