1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
|
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or1 FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.
#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 ).array().square().sum();
VERIFY(error < Scalar(10)*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());
}
|
