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
18using namespace Eigen;
19
20//  Constructs a random matrix from the unitary group U(size).
21template <typename T>
22Eigen::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).
76template <typename T>
77Eigen::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
93template <typename MatrixType>
94void 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
122template<typename Scalar, int Dimension>
123void 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
158void 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