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
|
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
//
// 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 <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 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 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) *= numext::conj(Q.determinant());
return Q;
}
template <typename MatrixType>
void run_test(int dim, int num_elements)
{
using std::abs;
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 = 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 ).norm() / dst.norm();
VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon());
}
template<typename Scalar, int Dimension>
void run_fixed_size_test(int num_elements)
{
using std::abs;
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!
// Also if c is to small compared to t.norm(), problem is ill-posed (cf. Bug 744)
const Scalar c = internal::random<Scalar>(0.5, 2.0);
FixedMatrix R = randMatrixSpecialUnitary<Scalar>(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<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 ).squaredNorm();
VERIFY(error < Scalar(16)*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());
}
|
