ground/gcs/src/libs/eigen/test/umeyama.cpp

   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>
  22 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixUnitary(int size)
  23 {
  24   typedef T Scalar;
  25   typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;
  26
  27   MatrixType Q;
  28
  29   int max_tries = 40;
  30   double is_unitary = false;
  31
  32   while (!is_unitary && max_tries > 0)
  33   {
  34     // initialize random matrix
  35     Q = MatrixType::Random(size, size);
  36
  37     // orthogonalize columns using the Gram-Schmidt algorithm
  38     for (int col = 0; col < size; ++col)
  39     {
  40       typename MatrixType::ColXpr colVec = Q.col(col);
  41       for (int prevCol = 0; prevCol < col; ++prevCol)
  42       {
  43         typename MatrixType::ColXpr prevColVec = Q.col(prevCol);
  44         colVec -= colVec.dot(prevColVec)*prevColVec;
  45       }
  46       Q.col(col) = colVec.normalized();
  47     }
  48
  49     // this additional orthogonalization is not necessary in theory but should enhance
  50     // the numerical orthogonality of the matrix
  51     for (int row = 0; row < size; ++row)
  52     {
  53       typename MatrixType::RowXpr rowVec = Q.row(row);
  54       for (int prevRow = 0; prevRow < row; ++prevRow)
  55       {
  56         typename MatrixType::RowXpr prevRowVec = Q.row(prevRow);
  57         rowVec -= rowVec.dot(prevRowVec)*prevRowVec;
  58       }
  59       Q.row(row) = rowVec.normalized();
  60     }
  61
  62     // final check
  63     is_unitary = Q.isUnitary();
  64     --max_tries;
  65   }
  66
  67   if (max_tries == 0)
  68     eigen_assert(false && "randMatrixUnitary: Could not construct unitary matrix!");
  69
  70   return Q;
  71 }
  72
  73 //  Constructs a random matrix from the special unitary group SU(size).
  74 template <typename T>
  75 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixSpecialUnitary(int size)
  76 {
  77   typedef T Scalar;
  78
  79   typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;
  80
  81   // initialize unitary matrix
  82   MatrixType Q = randMatrixUnitary<Scalar>(size);
  83
  84   // tweak the first column to make the determinant be 1
  85   Q.col(0) *= numext::conj(Q.determinant());
  86
  87   return Q;
  88 }
  89
  90 template <typename MatrixType>
  91 void run_test(int dim, int num_elements)
  92 {
  93   using std::abs;
  94   typedef typename internal::traits<MatrixType>::Scalar Scalar;
  95   typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
  96   typedef Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
  97
  98   // MUST be positive because in any other case det(cR_t) may become negative for
  99   // odd dimensions!
 100   const Scalar c = abs(internal::random<Scalar>());
 101
 102   MatrixX R = randMatrixSpecialUnitary<Scalar>(dim);
 103   VectorX t = Scalar(50)*VectorX::Random(dim,1);
 104
 105   MatrixX cR_t = MatrixX::Identity(dim+1,dim+1);
 106   cR_t.block(0,0,dim,dim) = c*R;
 107   cR_t.block(0,dim,dim,1) = t;
 108
 109   MatrixX src = MatrixX::Random(dim+1, num_elements);
 110   src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));
 111
 112   MatrixX dst = cR_t*src;
 113
 114   MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements));
 115
 116   const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm();
 117   VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon());
 118 }
 119
 120 template<typename Scalar, int Dimension>
 121 void run_fixed_size_test(int num_elements)
 122 {
 123   using std::abs;
 124   typedef Matrix<Scalar, Dimension+1, Dynamic> MatrixX;
 125   typedef Matrix<Scalar, Dimension+1, Dimension+1> HomMatrix;
 126   typedef Matrix<Scalar, Dimension, Dimension> FixedMatrix;
 127   typedef Matrix<Scalar, Dimension, 1> FixedVector;
 128
 129   const int dim = Dimension;
 130
 131   // MUST be positive because in any other case det(cR_t) may become negative for
 132   // odd dimensions!
 133   // Also if c is to small compared to t.norm(), problem is ill-posed (cf. Bug 744)
 134   const Scalar c = internal::random<Scalar>(0.5, 2.0);
 135
 136   FixedMatrix R = randMatrixSpecialUnitary<Scalar>(dim);
 137   FixedVector t = Scalar(32)*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 ).squaredNorm();
 154
 155   VERIFY(error < Scalar(16)*std::numeric_limits<Scalar>::epsilon());
 156 }
 157
 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 }