You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

183 lines
5.8 KiB

  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. #include "main.h"
  10. #include <Eigen/Core>
  11. #include <Eigen/Geometry>
  12. #include <Eigen/LU> // required for MatrixBase::determinant
  13. #include <Eigen/SVD> // required for SVD
  14. using namespace StormEigen;
  15. // Constructs a random matrix from the unitary group U(size).
  16. template <typename T>
  17. StormEigen::Matrix<T, StormEigen::Dynamic, Eigen::Dynamic> randMatrixUnitary(int size)
  18. {
  19. typedef T Scalar;
  20. typedef StormEigen::Matrix<Scalar, StormEigen::Dynamic, Eigen::Dynamic> MatrixType;
  21. MatrixType Q;
  22. int max_tries = 40;
  23. double is_unitary = false;
  24. while (!is_unitary && max_tries > 0)
  25. {
  26. // initialize random matrix
  27. Q = MatrixType::Random(size, size);
  28. // orthogonalize columns using the Gram-Schmidt algorithm
  29. for (int col = 0; col < size; ++col)
  30. {
  31. typename MatrixType::ColXpr colVec = Q.col(col);
  32. for (int prevCol = 0; prevCol < col; ++prevCol)
  33. {
  34. typename MatrixType::ColXpr prevColVec = Q.col(prevCol);
  35. colVec -= colVec.dot(prevColVec)*prevColVec;
  36. }
  37. Q.col(col) = colVec.normalized();
  38. }
  39. // this additional orthogonalization is not necessary in theory but should enhance
  40. // the numerical orthogonality of the matrix
  41. for (int row = 0; row < size; ++row)
  42. {
  43. typename MatrixType::RowXpr rowVec = Q.row(row);
  44. for (int prevRow = 0; prevRow < row; ++prevRow)
  45. {
  46. typename MatrixType::RowXpr prevRowVec = Q.row(prevRow);
  47. rowVec -= rowVec.dot(prevRowVec)*prevRowVec;
  48. }
  49. Q.row(row) = rowVec.normalized();
  50. }
  51. // final check
  52. is_unitary = Q.isUnitary();
  53. --max_tries;
  54. }
  55. if (max_tries == 0)
  56. eigen_assert(false && "randMatrixUnitary: Could not construct unitary matrix!");
  57. return Q;
  58. }
  59. // Constructs a random matrix from the special unitary group SU(size).
  60. template <typename T>
  61. StormEigen::Matrix<T, StormEigen::Dynamic, Eigen::Dynamic> randMatrixSpecialUnitary(int size)
  62. {
  63. typedef T Scalar;
  64. typedef StormEigen::Matrix<Scalar, StormEigen::Dynamic, Eigen::Dynamic> MatrixType;
  65. // initialize unitary matrix
  66. MatrixType Q = randMatrixUnitary<Scalar>(size);
  67. // tweak the first column to make the determinant be 1
  68. Q.col(0) *= numext::conj(Q.determinant());
  69. return Q;
  70. }
  71. template <typename MatrixType>
  72. void run_test(int dim, int num_elements)
  73. {
  74. using std::abs;
  75. typedef typename internal::traits<MatrixType>::Scalar Scalar;
  76. typedef Matrix<Scalar, StormEigen::Dynamic, StormEigen::Dynamic> MatrixX;
  77. typedef Matrix<Scalar, StormEigen::Dynamic, 1> VectorX;
  78. // MUST be positive because in any other case det(cR_t) may become negative for
  79. // odd dimensions!
  80. const Scalar c = abs(internal::random<Scalar>());
  81. MatrixX R = randMatrixSpecialUnitary<Scalar>(dim);
  82. VectorX t = Scalar(50)*VectorX::Random(dim,1);
  83. MatrixX cR_t = MatrixX::Identity(dim+1,dim+1);
  84. cR_t.block(0,0,dim,dim) = c*R;
  85. cR_t.block(0,dim,dim,1) = t;
  86. MatrixX src = MatrixX::Random(dim+1, num_elements);
  87. src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));
  88. MatrixX dst = cR_t*src;
  89. MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements));
  90. const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm();
  91. VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon());
  92. }
  93. template<typename Scalar, int Dimension>
  94. void run_fixed_size_test(int num_elements)
  95. {
  96. using std::abs;
  97. typedef Matrix<Scalar, Dimension+1, Dynamic> MatrixX;
  98. typedef Matrix<Scalar, Dimension+1, Dimension+1> HomMatrix;
  99. typedef Matrix<Scalar, Dimension, Dimension> FixedMatrix;
  100. typedef Matrix<Scalar, Dimension, 1> FixedVector;
  101. const int dim = Dimension;
  102. // MUST be positive because in any other case det(cR_t) may become negative for
  103. // odd dimensions!
  104. // Also if c is to small compared to t.norm(), problem is ill-posed (cf. Bug 744)
  105. const Scalar c = internal::random<Scalar>(0.5, 2.0);
  106. FixedMatrix R = randMatrixSpecialUnitary<Scalar>(dim);
  107. FixedVector t = Scalar(32)*FixedVector::Random(dim,1);
  108. HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1);
  109. cR_t.block(0,0,dim,dim) = c*R;
  110. cR_t.block(0,dim,dim,1) = t;
  111. MatrixX src = MatrixX::Random(dim+1, num_elements);
  112. src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));
  113. MatrixX dst = cR_t*src;
  114. Block<MatrixX, Dimension, Dynamic> src_block(src,0,0,dim,num_elements);
  115. Block<MatrixX, Dimension, Dynamic> dst_block(dst,0,0,dim,num_elements);
  116. HomMatrix cR_t_umeyama = umeyama(src_block, dst_block);
  117. const Scalar error = ( cR_t_umeyama*src - dst ).squaredNorm();
  118. VERIFY(error < Scalar(16)*std::numeric_limits<Scalar>::epsilon());
  119. }
  120. void test_umeyama()
  121. {
  122. for (int i=0; i<g_repeat; ++i)
  123. {
  124. const int num_elements = internal::random<int>(40,500);
  125. // works also for dimensions bigger than 3...
  126. for (int dim=2; dim<8; ++dim)
  127. {
  128. CALL_SUBTEST_1(run_test<MatrixXd>(dim, num_elements));
  129. CALL_SUBTEST_2(run_test<MatrixXf>(dim, num_elements));
  130. }
  131. CALL_SUBTEST_3((run_fixed_size_test<float, 2>(num_elements)));
  132. CALL_SUBTEST_4((run_fixed_size_test<float, 3>(num_elements)));
  133. CALL_SUBTEST_5((run_fixed_size_test<float, 4>(num_elements)));
  134. CALL_SUBTEST_6((run_fixed_size_test<double, 2>(num_elements)));
  135. CALL_SUBTEST_7((run_fixed_size_test<double, 3>(num_elements)));
  136. CALL_SUBTEST_8((run_fixed_size_test<double, 4>(num_elements)));
  137. }
  138. // Those two calls don't compile and result in meaningful error messages!
  139. // umeyama(MatrixXcf(),MatrixXcf());
  140. // umeyama(MatrixXcd(),MatrixXcd());
  141. }