umeyama.cpp
Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra. Eigen itself is part of the KDE project.
00003 //
00004 // Copyright (C) 2009 Hauke Heibel <hauke.heibel@gmail.com>
00005 //
00006 // Eigen is free software; you can redistribute it and/or
00007 // modify it under the terms of the GNU Lesser General Public
00008 // License as published by the Free Software Foundation; either
00009 // version 3 of the License, or (at your option) any later version.
00010 //
00011 // Alternatively, you can redistribute it and/or
00012 // modify it under the terms of the GNU General Public License as
00013 // published by the Free Software Foundation; either version 2 of
00014 // the License, or (at your option) any later version.
00015 //
00016 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00017 // WARRANTY; without even the implied warranty of MERCHANTABILITY or1 FITNESS
00018 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00019 // GNU General Public License for more details.
00020 //
00021 // You should have received a copy of the GNU Lesser General Public
00022 // License and a copy of the GNU General Public License along with
00023 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00024 
00025 #include "main.h"
00026 
00027 #include <Eigen/Core>
00028 #include <Eigen/Geometry>
00029 
00030 #include <Eigen/LU> // required for MatrixBase::determinant
00031 #include <Eigen/SVD> // required for SVD
00032 
00033 using namespace Eigen;
00034 
00035 //  Constructs a random matrix from the unitary group U(size).
00036 template <typename T>
00037 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixUnitary(int size)
00038 {
00039   typedef T Scalar;
00040   typedef typename NumTraits<Scalar>::Real RealScalar;
00041 
00042   typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;
00043 
00044   MatrixType Q;
00045 
00046   int max_tries = 40;
00047   double is_unitary = false;
00048 
00049   while (!is_unitary && max_tries > 0)
00050   {
00051     // initialize random matrix
00052     Q = MatrixType::Random(size, size);
00053 
00054     // orthogonalize columns using the Gram-Schmidt algorithm
00055     for (int col = 0; col < size; ++col)
00056     {
00057       typename MatrixType::ColXpr colVec = Q.col(col);
00058       for (int prevCol = 0; prevCol < col; ++prevCol)
00059       {
00060         typename MatrixType::ColXpr prevColVec = Q.col(prevCol);
00061         colVec -= colVec.dot(prevColVec)*prevColVec;
00062       }
00063       Q.col(col) = colVec.normalized();
00064     }
00065 
00066     // this additional orthogonalization is not necessary in theory but should enhance
00067     // the numerical orthogonality of the matrix
00068     for (int row = 0; row < size; ++row)
00069     {
00070       typename MatrixType::RowXpr rowVec = Q.row(row);
00071       for (int prevRow = 0; prevRow < row; ++prevRow)
00072       {
00073         typename MatrixType::RowXpr prevRowVec = Q.row(prevRow);
00074         rowVec -= rowVec.dot(prevRowVec)*prevRowVec;
00075       }
00076       Q.row(row) = rowVec.normalized();
00077     }
00078 
00079     // final check
00080     is_unitary = Q.isUnitary();
00081     --max_tries;
00082   }
00083 
00084   if (max_tries == 0)
00085     eigen_assert(false && "randMatrixUnitary: Could not construct unitary matrix!");
00086 
00087   return Q;
00088 }
00089 
00090 //  Constructs a random matrix from the special unitary group SU(size).
00091 template <typename T>
00092 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> randMatrixSpecialUnitary(int size)
00093 {
00094   typedef T Scalar;
00095   typedef typename NumTraits<Scalar>::Real RealScalar;
00096 
00097   typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixType;
00098 
00099   // initialize unitary matrix
00100   MatrixType Q = randMatrixUnitary<Scalar>(size);
00101 
00102   // tweak the first column to make the determinant be 1
00103   Q.col(0) *= internal::conj(Q.determinant());
00104 
00105   return Q;
00106 }
00107 
00108 template <typename MatrixType>
00109 void run_test(int dim, int num_elements)
00110 {
00111   typedef typename internal::traits<MatrixType>::Scalar Scalar;
00112   typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
00113   typedef Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
00114 
00115   // MUST be positive because in any other case det(cR_t) may become negative for
00116   // odd dimensions!
00117   const Scalar c = internal::abs(internal::random<Scalar>());
00118 
00119   MatrixX R = randMatrixSpecialUnitary<Scalar>(dim);
00120   VectorX t = Scalar(50)*VectorX::Random(dim,1);
00121 
00122   MatrixX cR_t = MatrixX::Identity(dim+1,dim+1);
00123   cR_t.block(0,0,dim,dim) = c*R;
00124   cR_t.block(0,dim,dim,1) = t;
00125 
00126   MatrixX src = MatrixX::Random(dim+1, num_elements);
00127   src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));
00128 
00129   MatrixX dst = cR_t*src;
00130 
00131   MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements));
00132 
00133   const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm();
00134   VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon());
00135 }
00136 
00137 template<typename Scalar, int Dimension>
00138 void run_fixed_size_test(int num_elements)
00139 {
00140   typedef Matrix<Scalar, Dimension+1, Dynamic> MatrixX;
00141   typedef Matrix<Scalar, Dimension+1, Dimension+1> HomMatrix;
00142   typedef Matrix<Scalar, Dimension, Dimension> FixedMatrix;
00143   typedef Matrix<Scalar, Dimension, 1> FixedVector;
00144 
00145   const int dim = Dimension;
00146 
00147   // MUST be positive because in any other case det(cR_t) may become negative for
00148   // odd dimensions!
00149   const Scalar c = internal::abs(internal::random<Scalar>());
00150 
00151   FixedMatrix R = randMatrixSpecialUnitary<Scalar>(dim);
00152   FixedVector t = Scalar(50)*FixedVector::Random(dim,1);
00153 
00154   HomMatrix cR_t = HomMatrix::Identity(dim+1,dim+1);
00155   cR_t.block(0,0,dim,dim) = c*R;
00156   cR_t.block(0,dim,dim,1) = t;
00157 
00158   MatrixX src = MatrixX::Random(dim+1, num_elements);
00159   src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));
00160 
00161   MatrixX dst = cR_t*src;
00162 
00163   Block<MatrixX, Dimension, Dynamic> src_block(src,0,0,dim,num_elements);
00164   Block<MatrixX, Dimension, Dynamic> dst_block(dst,0,0,dim,num_elements);
00165 
00166   HomMatrix cR_t_umeyama = umeyama(src_block, dst_block);
00167 
00168   const Scalar error = ( cR_t_umeyama*src - dst ).array().square().sum();
00169 
00170   VERIFY(error < Scalar(10)*std::numeric_limits<Scalar>::epsilon());
00171 }
00172 
00173 void test_umeyama()
00174 {
00175   for (int i=0; i<g_repeat; ++i)
00176   {
00177     const int num_elements = internal::random<int>(40,500);
00178 
00179     // works also for dimensions bigger than 3...
00180     for (int dim=2; dim<8; ++dim)
00181     {
00182       CALL_SUBTEST_1(run_test<MatrixXd>(dim, num_elements));
00183       CALL_SUBTEST_2(run_test<MatrixXf>(dim, num_elements));
00184     }
00185 
00186     CALL_SUBTEST_3((run_fixed_size_test<float, 2>(num_elements)));
00187     CALL_SUBTEST_4((run_fixed_size_test<float, 3>(num_elements)));
00188     CALL_SUBTEST_5((run_fixed_size_test<float, 4>(num_elements)));
00189 
00190     CALL_SUBTEST_6((run_fixed_size_test<double, 2>(num_elements)));
00191     CALL_SUBTEST_7((run_fixed_size_test<double, 3>(num_elements)));
00192     CALL_SUBTEST_8((run_fixed_size_test<double, 4>(num_elements)));
00193   }
00194 
00195   // Those two calls don't compile and result in meaningful error messages!
00196   // umeyama(MatrixXcf(),MatrixXcf());
00197   // umeyama(MatrixXcd(),MatrixXcd());
00198 }


libicr
Author(s): Robert Krug
autogenerated on Mon Jan 6 2014 11:33:56