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@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>
randMatrixUnitary(int size)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>
randMatrixSpecialUnitary(int size)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>
run_test(int dim,int num_elements)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>
run_fixed_size_test(int num_elements)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
test_umeyama()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