Squashed 'third_party/eigen/' content from commit 61d72f6
Change-Id: Iccc90fa0b55ab44037f018046d2fcffd90d9d025
git-subtree-dir: third_party/eigen
git-subtree-split: 61d72f6383cfa842868c53e30e087b0258177257
diff --git a/test/umeyama.cpp b/test/umeyama.cpp
new file mode 100644
index 0000000..2e80924
--- /dev/null
+++ b/test/umeyama.cpp
@@ -0,0 +1,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());
+}