From be93c8bdb88de1d743ebb0c928c8d944f3239c1e Mon Sep 17 00:00:00 2001
From: Lisa Julia Nebel <lisa_julia.nebel@tu-dresden.de>
Date: Thu, 11 Feb 2021 18:41:22 +0100
Subject: [PATCH] Move the polar decomposition to a separate file and add the
algorithm by Higham and Noferini (from Robin Fraenzel)
In the test for the Higham and Noferini algorithm, we see:
Unfortunately, for matrices that are close to an orthogonal matrix, this algorithm is
about 2x slower. One can benefit from the Higham and Noferini algorithm only if the
matrix is quite far away from an orthogonal one.
---
dune/gfe/localprojectedfefunction.hh | 26 +--
dune/gfe/polardecomposition.hh | 326 +++++++++++++++++++++++++++
test/CMakeLists.txt | 1 +
test/polardecompositiontest.cc | 243 ++++++++++++++++++++
4 files changed, 572 insertions(+), 24 deletions(-)
create mode 100644 dune/gfe/polardecomposition.hh
create mode 100644 test/polardecompositiontest.cc
diff --git a/dune/gfe/localprojectedfefunction.hh b/dune/gfe/localprojectedfefunction.hh
index 80fa763d..9f9c5d77 100644
--- a/dune/gfe/localprojectedfefunction.hh
+++ b/dune/gfe/localprojectedfefunction.hh
@@ -10,6 +10,7 @@
#include <dune/gfe/rotation.hh>
#include <dune/gfe/rigidbodymotion.hh>
#include <dune/gfe/linearalgebra.hh>
+#include <dune/gfe/polardecomposition.hh>
namespace Dune {
@@ -174,29 +175,6 @@ namespace Dune {
static const int spaceDim = TargetSpace::TangentVector::dimension;
- static FieldMatrix<field_type,3,3> polarFactor(const FieldMatrix<field_type,3,3>& matrix)
- {
- int maxIterations = 100;
- double tol = 0.001;
- // Use Higham's method
- auto polar = matrix;
- for (size_t i=0; i<maxIterations; i++)
- {
- auto oldPolar = polar;
- auto polarInvert = polar;
- polarInvert.invert();
- for (size_t j=0; j<polar.N(); j++)
- for (size_t k=0; k<polar.M(); k++)
- polar[j][k] = 0.5 * (polar[j][k] + polarInvert[k][j]);
- oldPolar -= polar;
- if (oldPolar.frobenius_norm() < tol) {
- break;
- }
- }
-
- return polar;
- }
-
/**
* \param A The argument of the projection
* \param polar The image of the projection, i.e., the polar factor of A
@@ -309,7 +287,7 @@ namespace Dune {
}
// Project back onto SO(3)
- result.set(polarFactor(interpolatedMatrix));
+ result.set(Dune::GFE::PolarDecomposition()(interpolatedMatrix));
return result;
}
diff --git a/dune/gfe/polardecomposition.hh b/dune/gfe/polardecomposition.hh
new file mode 100644
index 00000000..6f3ecb86
--- /dev/null
+++ b/dune/gfe/polardecomposition.hh
@@ -0,0 +1,326 @@
+// -*- tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 2 -*-
+// vi: set et ts=4 sw=2 sts=2:
+#ifndef DUNE_GFE_POLARDECOMPOSITION_HH
+#define DUNE_GFE_POLARDECOMPOSITION_HH
+
+#include <dune/common/fmatrix.hh>
+#include <dune/common/version.hh>
+#include <dune/common/exceptions.hh>
+#include <dune/gfe/rotation.hh>
+#include <dune/gfe/linearalgebra.hh>
+
+///////////////////////////////////////////////////////////////////////////////////////////
+// Two Methods to compute the Polar Factor of a 3x3 matrix
+///////////////////////////////////////////////////////////////////////////////////////////
+
+namespace Dune::GFE {
+
+ class PolarDecomposition
+ {
+ public:
+
+ /** \brief Compute the polar factor of a matrix iteratively
+ Attention: For matrices that are quite far away from an orthogonal matrix,
+ this might return a matrix with determinant = -1 ! */
+ template <class field_type>
+ FieldMatrix<field_type,3,3> operator() (const FieldMatrix<field_type,3,3>& matrix, double tol = 0.001) const
+ {
+ int maxIterations = 100;
+ // Use Higham's method
+ auto polar = matrix;
+ for (size_t i=0; i<maxIterations; i++)
+ {
+ auto oldPolar = polar;
+ auto polarInvert = polar;
+ polarInvert.invert();
+ for (size_t j=0; j<polar.N(); j++)
+ for (size_t k=0; k<polar.M(); k++)
+ polar[j][k] = 0.5 * (polar[j][k] + polarInvert[k][j]);
+ oldPolar -= polar;
+ if (oldPolar.frobenius_norm() < tol) {
+ break;
+ }
+ }
+ return polar;
+ }
+ };
+
+ class HighamNoferiniPolarDecomposition
+ {
+ public:
+ /** \brief Compute the polar factor part of a matrix using the paper
+ "An algorithm to compute the polar decomposition of a 3 × 3 matrix" from Higham and Noferini (2015)
+ * \param matrix Compute the polar factor part of this matrix
+ * \param tau1 tolerance, default value taken from Higham and Noferini
+ * \param tau2 tolerance, default value taken from Higham and Noferini
+ */
+ template <class field_type>
+ FieldMatrix<field_type, 3, 3> operator() (const FieldMatrix<field_type, 3, 3>& matrix, double tau1 = 0.0001, double tau2 = 0.0001) const
+ {
+ auto normedMatrix = matrix;
+ normedMatrix /= normedMatrix.frobenius_norm();
+ FieldMatrix<field_type,4,4> bilinearmatrix = bilinearMatrix(normedMatrix);
+ auto detB = determinantLaplace(bilinearmatrix);
+ auto detM = normedMatrix.determinant();
+ field_type domEV = 0;
+ if (detM < 0) {
+ detM = -detM;
+ bilinearmatrix = -bilinearmatrix;
+ }
+
+ if ( detB + 1./3 > tau1) { // estimate dominant eigenvalue if well separated => use formula
+ domEV = dominantEV( detB, detM );
+ } else { // eignevalue nearly a double root => use newtons method
+ auto coefficients = characteristicCoefficients(bilinearmatrix);
+ domEV = dominantEVNewton(coefficients,detB);
+ }
+ // compute iterationmatrix B_S
+ FieldMatrix<field_type,4,4> BS = {{domEV,0,0,0},{0,domEV,0,0},{0,0,domEV,0},{0,0,0,domEV}};
+ BS -= bilinearmatrix;
+ if (detB < 1 - tau2) {
+ Rotation<field_type,3> v(obtainQuaternion(BS));
+ FieldMatrix<field_type,3,3> mat;
+ v.matrix(mat);
+ return mat;
+ } else {
+ DUNE_THROW(NotImplemented, "The eigenvalues are not wellseparated => inverse iteration won't converge!");
+ }
+ }
+
+ protected:
+
+ /** \brief Compute the bilinear matrix for a given 3x3 matrix*/
+ template <class field_type>
+ static FieldMatrix<field_type,4,4> bilinearMatrix(const FieldMatrix<field_type,3,3> matrix)
+ {
+ FieldMatrix<field_type,4,4> bilinearmatrix;
+ bilinearmatrix[0][0] = matrix[0][0] + matrix[1][1] + matrix[2][2];
+ bilinearmatrix[0][1] = matrix[1][2] - matrix[2][1];
+ bilinearmatrix[0][2] = matrix[2][0] - matrix[0][2];
+ bilinearmatrix[0][3] = matrix[0][1] - matrix[1][0];
+ bilinearmatrix[1][0] = bilinearmatrix[0][1];
+ bilinearmatrix[1][1] = matrix[0][0] - matrix[1][1] - matrix[2][2];
+ bilinearmatrix[1][2] = matrix[0][1] + matrix[1][0];
+ bilinearmatrix[1][3] = matrix[2][0] + matrix[0][2];
+ bilinearmatrix[2][0] = bilinearmatrix[0][2];
+ bilinearmatrix[2][1] = bilinearmatrix[1][2];
+ bilinearmatrix[2][2] = matrix[1][1] - matrix[0][0] - matrix[2][2];
+ bilinearmatrix[2][3] = matrix[1][2] + matrix[2][1];
+ bilinearmatrix[3][0] = bilinearmatrix[0][3];
+ bilinearmatrix[3][1] = bilinearmatrix[1][3];
+ bilinearmatrix[3][2] = bilinearmatrix[2][3];
+ bilinearmatrix[3][3] = matrix[2][2] - matrix[0][0] - matrix[1][1];
+ return bilinearmatrix;
+ }
+
+ /** \brief Compute the determinant of a 4x4 matrix using the Laplace method
+ The implementation is faster than calling matrix.determinant()*/
+ template <class field_type>
+ static field_type determinantLaplace( const FieldMatrix<field_type,4,4>& matrix)
+ {
+ field_type T2233 = matrix[2][2] * matrix[3][3];
+ field_type T3223 = matrix[3][2] * matrix[2][3];
+ field_type T1 = T2233 - T3223;
+
+ field_type T1233 = matrix[1][2] * matrix[3][3];
+ field_type T3213 = matrix[3][2] * matrix[1][3];
+ field_type T2 = T1233 - T3213;
+
+ field_type T1223 = matrix[1][2] * matrix[2][3];
+ field_type T2213 = matrix[2][2] * matrix[1][3];
+ field_type T3 = T1223 - T2213;
+
+ field_type T0233 = matrix[0][2] * matrix[3][3];
+ field_type T3203 = matrix[3][2] * matrix[0][3];
+ field_type T4 = T3203 - T0233;
+
+ field_type T0223 = matrix[0][2] * matrix[2][3];
+ field_type T2203 = matrix[2][2] * matrix[0][3];
+ field_type T5 = T0223- T2203;
+
+ field_type T0213 = matrix[0][2] * matrix[1][3];
+ field_type T1203 = matrix[1][2] * matrix[0][3];
+ field_type T6 = T0213-T1203;
+
+ return matrix[0][0] * (matrix[1][1] * T1 - matrix[2][1] * T3 + matrix[3][1] * T3)
+ - matrix[1][0] * (matrix[0][1] * T1 + matrix[2][1] * T4 + matrix[3][1] * T5)
+ + matrix[2][0] * (matrix[0][1] * T2 + matrix[1][1] * T4 + matrix[3][1] * T6)
+ - matrix[3][0] * (matrix[0][1] * T2 - matrix[1][1] * T5 + matrix[2][1] * T6);
+ }
+
+ /** \brief Return the factors a,b,c of the characteristic polynomial x^4+a*x^3+b*x^2+c*x + detB */
+ template <class field_type>
+ static FieldVector<field_type,3> characteristicCoefficients(const FieldMatrix<field_type,4,4>& matrix)
+ {
+ field_type a = - Dune::GFE::trace(matrix);
+ field_type b = matrix[0][0] * (matrix[1][1] + matrix[2][2] + matrix[3][3])
+ + matrix[3][3] * (matrix[1][1] + matrix[2][2])
+ - (matrix[1][0] * matrix[0][1] + matrix[2][0] * matrix[0][2] +
+ matrix[3][0] * matrix[0][3] + matrix[1][2] * matrix[2][1] +
+ matrix[1][3] * matrix[3][1] + matrix[3][2] * matrix[2][3]);
+
+ field_type c = matrix[0][0] * (matrix[1][2] * matrix[2][1] + matrix[1][3] * matrix[3][1] + matrix[2][3] * matrix[3][2] -(matrix[2][2] * matrix[3][3] + matrix[1][1] * (matrix[2][2] + matrix[3][3])))
+ + matrix[0][1] * (matrix[1][0] * (matrix[2][2] + matrix[3][3]) -(matrix[1][2] * matrix[2][0] + matrix[1][3] * matrix[3][0]))
+ + matrix[0][2] * (matrix[2][0] * (matrix[1][1] + matrix[3][3]) -(matrix[1][0] * matrix[2][1] + matrix[2][3] * matrix[3][0]))
+ + matrix[0][3] * (matrix[3][0] * (matrix[1][1] + matrix[2][2]) -(matrix[1][0] * matrix[3][1] + matrix[2][0] * matrix[3][2]))
+ + matrix[1][1] * (matrix[2][3] * matrix[3][2] - matrix[2][2] * matrix[3][3])
+ + matrix[1][2] * (matrix[2][1] * matrix[3][3] - matrix[2][3] * matrix[3][1])
+ + matrix[1][3] * (matrix[2][2] * matrix[3][1] - matrix[2][1] * matrix[3][2]);
+ return {a, b, c};
+ }
+
+ /** \brief Return the dominant Eigenvalue of the matrix B */
+ template <class field_type>
+ static field_type dominantEV(field_type detB, field_type detM )
+ {
+ auto c = 8 * detM;
+ auto sigma0 = 1 + 3 * detB;
+ auto sigma1 = 27/16*c*c + 9 * detB - 1;
+ auto alpha = sigma1/std::pow(sigma0, 1.5 );
+ auto z = 4/3 * (1 + std::sqrt(sigma0) * std::cos(std::acos(alpha)/3));
+ auto s = std::sqrt(z)/ 2;
+ auto x = 4-z+c/s;
+ if (x > 0)
+ return s + std::sqrt(x)/2;
+ else
+ return s;
+ }
+
+ /** \brief Return the dominant Eigenvalue of the matrix B.
+ This algorithm corresponds to Algorithm 3.4 in
+ "An algorithm to compute the polar decomposition
+ of a 3 × 3 matrix" from Higham and Noferini (2015)
+ * \param coefficients Coefficients of the characteristic polynomial for the matrix b
+ * \param detB Determinant of the matrix B
+ * \param tol Tolerance for the Newton method, the value is taken from the paper by Higham and Noferini
+ */
+ template <class field_type>
+ static field_type dominantEVNewton(const FieldVector<field_type,3>& coefficients, field_type detB, double tol = 10e-5)
+ {
+ field_type charPol = 0;
+ field_type charPolDeriv = 0;
+ field_type lambdaOld = 3;
+ field_type domEV = std::sqrt(3);
+ while ((lambdaOld - domEV)/domEV > tol) {
+ lambdaOld = domEV;
+ charPol = detB + domEV*( coefficients[2] + domEV*( coefficients[1] + domEV*( coefficients[0]+ domEV ) ) );
+ charPolDeriv= coefficients[2] + domEV * ( 2*coefficients[1]+ domEV * ( 3*coefficients[0] + 4*domEV) );
+ domEV = domEV- charPol / charPolDeriv;
+ }
+ return domEV;
+ }
+
+ /** \brief Return quaternion vector resulting from step 7 and 8 in Algorithm 3.2 from Higham and Noferini (2015)
+ Step 7: Calculate the LDLT factorization of the given matrix with diagonal pivoting
+ Step 8: Using L and the pivotmatix, calculate the vector v
+ * \param shiftedB shifted matrix B as given in chapter 3 of Higham an Noferini: shiftedB = lambda * Id - B, where lambda is the dominant eigenvalue of B
+ \returns v the respective polar factor in quaternion form
+ *\
+ */
+ template <class field_type>
+ static FieldVector<field_type,4> obtainQuaternion(const FieldMatrix<field_type,4,4> shiftedB)
+ {
+ FieldMatrix<field_type,4,4> P = 0;
+ FieldMatrix<field_type,4,4> Pleft = 0;
+ FieldMatrix<field_type,4,4> Pright = 0;
+ FieldMatrix<field_type,4,4> L = {{1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
+ FieldVector<field_type,4> D = {0,0,0,0};
+ FieldVector<field_type,4> Bdiag = {shiftedB[0][0], shiftedB[1][1], shiftedB[2][2], shiftedB[3][3]};
+ int maxi = 0; // Index of the maximal element
+ int secmax = 0;
+ int secmin = 0;
+ int mini = 0; // Index of the minimal element
+
+ // Find Pivotmatrix
+ for (int i = 0; i < 4; ++i) // going through the diagonal searching for the maximum
+ if ( Bdiag[maxi] < Bdiag[i] ) // found a bigger one but smaler than the older ones
+ maxi = i;
+
+ Pleft[0][maxi] = 1;
+ Pright[maxi][0] = 1; // Largest element to the first position
+
+ for (int i = 0; i < 4; ++i) // going through the diagonal searching for the minimum
+ if ( Bdiag[mini] > Bdiag[i] ) // found a smaler one but smaler than the older ones
+ mini = i;
+
+ Pleft[3][mini] = 1;
+ Pright[mini][3] = 1; // Smalest element to the last position
+
+
+ for (int i = 0; i < 4; ++i) {
+ if ( i != maxi & i != mini ) {
+ for (int j = 0; j < 4; ++j) {
+ if ( j != maxi & j != mini & j != i ) {
+ if ( Bdiag[i] < Bdiag[j] ) {
+ Pleft[1][j] = 1; // Second largest element at the second position
+ Pright[j][1] = 1;
+ Pleft[2][i] = 1; // Third largest element at the third position
+ Pright[i][2] = 1;
+ secmin = i;
+ secmax = j;
+ } else {
+ Pleft[2][j] = 1;
+ Pright[j][2] = 1;
+ Pleft[1][i] = 1;
+ Pright[i][1] = 1;
+ secmin = j;
+ secmax = i;
+ }
+ }
+ }
+ }
+ }
+
+ //P = Pleft*shiftedB*Pright;
+ P[maxi][maxi] = shiftedB[0][0];
+ P[maxi][secmax] = shiftedB[0][1];
+ P[maxi][secmin] = shiftedB[0][2];
+ P[maxi][mini] = shiftedB[0][3];
+
+ P[secmax][maxi] = shiftedB[1][0];
+ P[secmax][secmax] = shiftedB[1][1];
+ P[secmax][secmin] = shiftedB[1][2];
+ P[secmax][mini] = shiftedB[1][3];
+
+ P[secmin][maxi] = shiftedB[2][0];
+ P[secmin][secmax] = shiftedB[2][1];
+ P[secmin][secmin] = shiftedB[2][2];
+ P[secmin][mini] = shiftedB[2][3];
+
+ P[mini][maxi] = shiftedB[3][0];
+ P[mini][secmax] = shiftedB[3][1];
+ P[mini][secmin] = shiftedB[3][2];
+ P[mini][mini] = shiftedB[3][3];
+
+ // Choleskydecomposition
+
+ for (int k = 0; k < 4; ++k) { //columns
+ D[k] = P[k][k];
+ for (int j = 0; j < k; ++j)//sum
+ D[k] -= L[k][j]*L[k][j] * D[j];
+
+ for (int i = k+1; i < 4; ++i) { //rows
+ L[i][k] = P[i][k];
+ for (int j = 0; j < k; ++j)//sum
+ L[i][k] -= L[i][j]*L[k][j] * D[j];
+ L[i][k] /= D[k];
+ }
+ }
+
+ FieldVector<field_type,4> v = {0,0,0,0};
+ FieldVector<field_type,4> unnormed = {0,0,0,1};
+ auto neg = -L[3][2];
+ unnormed[0] = L[1][0] * ( L[3][1] + L[2][1]*neg ) + L[2][0] * L[3][2] - L[3][0];
+ unnormed[1] = L[2][1] * L[3][2] - L[3][1];
+ unnormed[2] = neg;
+ unnormed /= unnormed.two_norm();
+ v[0] = unnormed[maxi];
+ v[1] = unnormed[secmax];
+ v[2] = unnormed[secmin];
+ v[3] = unnormed[mini];
+ return v;
+ }
+ };
+}
+
+#endif
\ No newline at end of file
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2f725374..b292a33a 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -8,6 +8,7 @@ set(TESTS
localgfetestfunctiontest
localprojectedfefunctiontest
orthogonalmatrixtest
+ polardecompositiontest
rigidbodymotiontest
rotationtest
svdtest
diff --git a/test/polardecompositiontest.cc b/test/polardecompositiontest.cc
new file mode 100644
index 00000000..cc10676d
--- /dev/null
+++ b/test/polardecompositiontest.cc
@@ -0,0 +1,243 @@
+// -*- tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 2 -*-
+// vi: set et ts=4 sw=2 sts=2:
+#include <dune/common/fmatrix.hh>
+#include <dune/common/fvector.hh>
+#include <dune/gfe/polardecomposition.hh>
+#include <dune/gfe/rotation.hh>
+
+#include <chrono>
+#include <fstream>
+#include <random>
+
+
+using namespace Dune;
+using field_type = double;
+
+
+class PolarDecompositionTest : public Dune::GFE::HighamNoferiniPolarDecomposition {
+ public:
+ using HighamNoferiniPolarDecomposition::HighamNoferiniPolarDecomposition;
+ using HighamNoferiniPolarDecomposition::bilinearMatrix;
+ using HighamNoferiniPolarDecomposition::determinantLaplace;
+ using HighamNoferiniPolarDecomposition::dominantEVNewton;
+ using HighamNoferiniPolarDecomposition::characteristicCoefficients;
+ using HighamNoferiniPolarDecomposition::obtainQuaternion;
+};
+
+/** \brief Check if the matrix is not orthogonal */
+static bool isNotOrthogonal(const FieldMatrix<field_type,3,3>& matrix) {
+ bool notOrthogonal = std::abs(1-matrix.determinant()) > 1e-12;
+ notOrthogonal = notOrthogonal or (( matrix[0]*matrix[1] ) > 1e-12);
+ notOrthogonal = notOrthogonal or (( matrix[0]*matrix[2] ) > 1e-12);
+ notOrthogonal = notOrthogonal or (( matrix[1]*matrix[2] ) > 1e-12);
+ return notOrthogonal;
+}
+
+/** \brief Return the time needed for 1000 polar decompositions of the given matrix using the iterative algorithm
+ and the algorithm by Higham. */
+static FieldVector<field_type,2> measureTimeForPolarDecomposition(const FieldMatrix<field_type, 3, 3>& matrix, double tol = 10e-3)
+{
+ FieldMatrix<field_type, 3, 3> Q1;
+ FieldMatrix<field_type, 3, 3> Q2;
+ FieldVector<field_type,2> actualtime;
+ std::chrono::steady_clock::time_point begindecomold = std::chrono::steady_clock::now();
+ for (int i = 0; i < 1000; ++i) {
+ Q1 = Dune::GFE::PolarDecomposition()(matrix, tol);
+ }
+ std::chrono::steady_clock::time_point enddecomold = std::chrono::steady_clock::now();
+ actualtime[0] = (enddecomold - begindecomold).count();
+
+ std::chrono::steady_clock::time_point begindecom = std::chrono::steady_clock::now();
+ for (int i = 0; i < 1000; ++i) {
+ Q2 = Dune::GFE::HighamNoferiniPolarDecomposition()(matrix, tol, tol);
+ }
+ std::chrono::steady_clock::time_point enddecom = std::chrono::steady_clock::now();
+ actualtime[1] = (enddecom - begindecom).count();
+ return actualtime;
+}
+
+/** \brief Returns a 3x3-Matrix with random entries between 0 and upper Bound*/
+static FieldMatrix<field_type, 3, 3> randomMatrix3d(double upperBound = 1.0)
+{
+ const int dim = 3;
+ FieldMatrix<field_type, dim, dim> matrix;
+ std::random_device rd; // Will be used to obtain a seed for the random number engine
+ std::mt19937 gen(rd()); // Standard mersenne_twister_engine seeded with rd()
+ std::uniform_real_distribution<> dis(0.0, upperBound); // equally distributed between 0 and upper bound
+ for (int i = 0; i < dim; ++i)
+ for (int j = 0; j < dim; ++j)
+ matrix[i][j] = dis(gen);
+
+ return matrix;
+}
+
+/** \brief Returns an "almost orthogonal" 3x3-Matrix where a random perturbation
+ between 0 and maxPerturbation is added to each entry.*/
+static FieldMatrix<field_type, 3, 3> randomMatrixAlmostOrthogonal(double maxPerturbation = 0.1)
+{
+ const int dim = 3;
+ FieldVector<field_type,4> f;
+ std::random_device rd; // Will be used to obtain a seed for the random number engine
+ std::mt19937 gen(rd()); // Standard mersenne_twister_engine seeded with rd()
+ std::uniform_real_distribution<> dis(0.0, 1.0); // equally distributed between 0 and upper bound
+ for (int i = 0; i < 4; ++i)
+ f[i] = dis(gen);
+ Rotation<field_type,3> q(f);
+ q.normalize();
+
+ assert(std::abs(1-q.two_norm()) < 1e-12);
+
+ // Turn it into a matrix
+ FieldMatrix<double,3,3> matrix;
+ q.matrix(matrix);
+
+ if (isNotOrthogonal(matrix))
+ DUNE_THROW(Exception, "Expected the matrix " << matrix << " to be orthogonal, but it is not!");
+
+ //Now perturb this a little bit
+ for (int i = 0; i < dim; ++i)
+ for (int j = 0; j < dim; ++j)
+ matrix[i][j] += dis(gen)*maxPerturbation;
+ return matrix;
+}
+
+static double timeTest(double perturbationFromSO3 = 1.0) {
+ // Define matrices we want to use for testing
+ int numberOfTests = 100;
+ std::vector<double> timeOld(numberOfTests);
+ std::vector<double> timeNew(numberOfTests);
+ double totalTimeOld = 0;
+ double totalTimeNew = 0;
+
+ for (int j = 0; j < numberOfTests; ++j) { // testing loop
+ FieldMatrix<field_type,3,3> N;
+ // Only measure the time if the decomposition is unique and if both algorithms will retun an orthogonal matrix!
+ // Attention: For matrices that are quite far away from an orthogonal matrix, Dune::GFE::PolarDecomposition() might return a matrix with determinant = -1 !
+ double normOfDifference = 10;
+ FieldMatrix<field_type,3,3> Q1;
+ FieldMatrix<field_type,3,3> Q2;
+ while(isNotOrthogonal(Q1) or isNotOrthogonal(Q2) or normOfDifference > 0.001) {
+ N = randomMatrixAlmostOrthogonal(perturbationFromSO3);
+ Q1 = PolarDecompositionTest()(N);
+ Q2 = Dune::GFE::PolarDecomposition()(N);
+ normOfDifference = (Q1-Q2).frobenius_norm();
+ }
+ auto actualtime = measureTimeForPolarDecomposition(N);
+ timeOld[j] = actualtime[0];
+ timeNew[j] = actualtime[1];
+ }
+
+ std::sort(timeOld.begin(), timeOld.end());
+ std::sort(timeNew.begin(), timeNew.end());
+
+ for (int j = 0; j < numberOfTests; ++j) {
+ totalTimeOld += timeOld[j];
+ totalTimeNew += timeNew[j];
+ }
+ std::cout << "Perturbation from an orthogonal matrix: " << perturbationFromSO3 << std::endl;
+ std::cout << "Average (old): " << totalTimeOld/numberOfTests << "[ns]" << std::endl;
+ std::cout << "Average (new): " << totalTimeNew/numberOfTests << "[ns]" << std::endl;
+ std::cout << "Relative: " << totalTimeOld/totalTimeNew << std::endl << std::endl;
+
+ return totalTimeOld/totalTimeNew;
+}
+
+static bool testBilinearMatrix() {
+ FieldMatrix<field_type,3,3> M = { { 20, 0, -3 },
+ { 3, 2.0, 310 },
+ { 5, 0.22, 0 } };
+ M /= M.frobenius_norm();
+ FieldMatrix<field_type,4,4> B = { { 0.0096632, 0.997822, 0.0257685, 0.00644213 },
+ { 0.997822, -0.00322107, 0.0708635, 0.00644213 },
+ { 0.0257685, 0.0708635, 0.00322107, 0.999239 },
+ { 0.00644213, 0.00644213, 0.999239, -0.0096632 } };
+ auto Btest = PolarDecompositionTest::bilinearMatrix(M);
+ Btest -= B;
+ return Btest.frobenius_norm() < 0.001;
+}
+
+static bool test4dDeterminant() {
+ FieldMatrix<field_type,4,4> B = { { 0.0096632, 0.997822, 0.0257685, 0.00644213 },
+ { 0.997822, -0.00322107, 0.0708635, 0.00644213 },
+ { 0.0257685, 0.0708635, 0.00322107, 0.999239 },
+ { 0.00644213, 0.00644213, 0.999239, -0.0096632 } };
+ double detBtest = PolarDecompositionTest::determinantLaplace(B);
+ detBtest -= B.determinant();
+ return std::abs(detBtest) < 0.001;
+}
+
+static bool testdominantEVNewton() {
+ field_type detB = 0.992928;
+ field_type detM = 0.000620104;
+ FieldVector<field_type,3> minpol = {0, -1.99999, -0.00496083};
+ double correctDominantEV = 1.05397;
+
+ auto dominantEVNewton = PolarDecompositionTest::dominantEVNewton(minpol, detB);
+ return std::abs(correctDominantEV - dominantEVNewton) < 0.001;
+}
+
+static bool testCharacteristicPolynomial4D() {
+ FieldMatrix<field_type,4,4> B = { { 0.0096632, 0.997822, 0.0257685, 0.00644213 },
+ { 0.997822, -0.00322107, 0.0708635, 0.00644213 },
+ { 0.0257685, 0.0708635, 0.00322107, 0.999239 },
+ { 0.00644213, 0.00644213, 0.999239, -0.0096632 } };
+ FieldVector<field_type,3> minpol = {0, -1.99999, -0.00496083};
+
+ auto coefficients = PolarDecompositionTest::characteristicCoefficients(B);
+ coefficients -= minpol;
+ return coefficients.two_norm() < 0.001;
+}
+
+static bool testQuaternionFunction() {
+ FieldMatrix<field_type,4,4> BS = { { 1.04431, -0.997822, -0.0257685, -0.00644213 },
+ { -0.997822, 1.05719, -0.0708635, -0.00644213 },
+ { -0.0257685, -0.0708635, 1.05075, -0.999239 },
+ { -0.00644213, -0.00644213, -0.999239, 1.06363 } };
+
+ FieldVector<field_type, 4 > v = { 0.508566, 0.516353, 0.499062, 0.475055 };
+ auto vtest = PolarDecompositionTest::obtainQuaternion(BS);
+ vtest -= v;
+ return vtest.two_norm() < 0.001;
+}
+
+int main (int argc, char *argv[]) try
+{
+ //test the correctness of the algorithm
+ auto testMatrix = randomMatrix3d();
+ auto Q = PolarDecompositionTest()(testMatrix,1e-12,1e-12);
+ if (isNotOrthogonal(Q)) {
+ std::cerr << "The polar decomposition did not return an orthogonal matrix when decomposing: " << std::endl << testMatrix << std::endl;
+ return 1;
+ }
+
+ if (testBilinearMatrix()) {
+ std::cerr << "The test calculation of the bilinearMatrix is wrong!" << std::endl;
+ return 1;
+ }
+ if (!test4dDeterminant()) {
+ std::cerr << "The test calculation of the 4d determinant is wrong!" << std::endl;
+ return 1;
+ }
+ if (!testdominantEVNewton()) {
+ std::cerr << "The test calculation of the dominant eigenvalue is wrong!" << std::endl;
+ return 1;
+ }
+ if (!testCharacteristicPolynomial4D()) {
+ std::cerr << "The test calculation of the characteristic polynomial is wrong!" << std::endl;
+ return 1;
+ }
+ if (!testQuaternionFunction()) {
+ std::cerr << "The test calculation of the quaternion function is wrong!" << std::endl;
+ return 1;
+ }
+
+ timeTest(.1);
+ timeTest(2);
+ timeTest(30);
+
+ return 0;
+
+} catch (Exception& e) {
+ std::cout << e.what() << std::endl;
+}
+
--
GitLab