From c20c7471a2b13dd0c7933513470fb47e5b5e247e Mon Sep 17 00:00:00 2001
From: Oliver Sander <oliver.sander@tu-dresden.de>
Date: Wed, 2 Mar 2022 18:24:45 +0100
Subject: [PATCH] Test (some of) the exp and log methods
This triggers bugs in the Rotation and RigidBodyMotion classes.
In particular, it triggers what Jonathan Youett fixed in the
never-merged merge request
https://gitlab.mn.tu-dresden.de/osander/dune-gfe/-/merge_requests/2
These bugs are fixed in this commit, too:
* The RigidBodyMotion class gets a `log` method
* The Rotation<3>::log method now returns EmbeddedTangentVector
instead of TangentVector.
---
CHANGELOG.md | 11 +++++++++++
dune/gfe/rigidbodymotion.hh | 27 ++++++++++++++++++++++++++-
dune/gfe/rotation.hh | 35 ++++++++++++++++++++++-------------
dune/gfe/unitvector.hh | 4 ++++
test/targetspacetest.cc | 20 ++++++++++++++++++++
5 files changed, 83 insertions(+), 14 deletions(-)
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 05bb4347..b7a71f58 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -5,3 +5,14 @@
- Fix the return value of `ProductManifold::log`: It was `TangentVector`,
but now it is `EmbeddedTangentVector`.
+
+- The `RigidBodyMotion` class has a `log` method now.
+
+- The method `Rotation<3>::log` now returns an `EmbeddedTangentVector`
+ instead of a `SkewMatrix`. This is consistent with the other manifold
+ implementations.
+
+- Deprecate the method `RigidBodyMotion::difference`; the method
+ `RigidBodyMotion::log`. Watch out: The `difference` method was buggy!
+ See https://gitlab.mn.tu-dresden.de/osander/dune-gfe/-/merge_requests/2
+ for details.
diff --git a/dune/gfe/rigidbodymotion.hh b/dune/gfe/rigidbodymotion.hh
index 85e24ae3..4a4704ff 100644
--- a/dune/gfe/rigidbodymotion.hh
+++ b/dune/gfe/rigidbodymotion.hh
@@ -99,6 +99,26 @@ public:
return result;
}
+ /** \brief Compute difference vector from a to b on the tangent space of a */
+ static EmbeddedTangentVector log(const RigidBodyMotion<ctype,N>& a,
+ const RigidBodyMotion<ctype,N>& b)
+ {
+ EmbeddedTangentVector result;
+
+ // Usual linear difference
+ for (int i=0; i<N; i++)
+ result[i] = b.r[i] - a.r[i];
+
+ // Subtract orientations on the tangent space of 'a'
+ typename Rotation<ctype,N>::EmbeddedTangentVector v = Rotation<ctype,N>::log(a.q, b.q);
+
+ // Compute difference on T_a SO(3)
+ for (int i=0; i<Rotation<ctype,N>::EmbeddedTangentVector::dimension; i++)
+ result[i+N] = v[i];
+
+ return result;
+ }
+
/** \brief Compute geodesic distance from a to b */
static T distance(const RigidBodyMotion<ctype,N>& a, const RigidBodyMotion<ctype,N>& b) {
@@ -109,7 +129,12 @@ public:
return std::sqrt(euclideanDistanceSquared + rotationDistance*rotationDistance);
}
- /** \brief Compute difference vector from a to b on the tangent space of a */
+ /** \brief Compute difference vector from a to b on the tangent space of a
+
+ * \warning The method is buggy! See https://gitlab.mn.tu-dresden.de/osander/dune-gfe/-/merge_requests/2
+ * \deprecated Use the log method instead!
+ */
+ [[deprecated("Use RigidBodyMotion::log instead of RigidBodyMotion::difference!")]]
static TangentVector difference(const RigidBodyMotion<ctype,N>& a,
const RigidBodyMotion<ctype,N>& b) {
diff --git a/dune/gfe/rotation.hh b/dune/gfe/rotation.hh
index 6a1cc01a..139a3034 100644
--- a/dune/gfe/rotation.hh
+++ b/dune/gfe/rotation.hh
@@ -305,7 +305,10 @@ public:
}
- /** \brief The exponential map from a given point $p \in SO(3)$. */
+ /** \brief The exponential map from a given point $p \in SO(3)$.
+
+ * \param v A tangent vector *at the identity*!
+ */
static Rotation<T,3> exp(const Rotation<T,3>& p, const SkewMatrix<T,3>& v) {
Rotation<T,3> corr = exp(v);
return p.mult(corr);
@@ -403,6 +406,12 @@ public:
return skew;
}
+ /** \brief Derivative of the exponential map at the identity
+ *
+ * The exponential map at the identity is a map from a neighborhood of zero to the neighborhood of the identity rotation.
+ *
+ * \param v Where to evaluate the derivative of the (exponential map at the identity)
+ */
static Dune::FieldMatrix<T,4,3> Dexp(const SkewMatrix<T,3>& v) {
using std::cos;
@@ -613,7 +622,10 @@ public:
/** \brief Compute the vector in T_aSO(3) that is mapped by the exponential map
to the geodesic from a to b
*/
- static SkewMatrix<T,3> log(const Rotation<T,3>& a, const Rotation<T,3>& b) {
+ static EmbeddedTangentVector log(const Rotation<T,3>& a, const Rotation<T,3>& b) {
+
+ // embedded tangent vector at identity
+ Quaternion<T> v;
Quaternion<T> diff = a;
diff.invert();
@@ -622,7 +634,6 @@ public:
// Compute the geodesical distance between a and b on SO(3)
// Due to numerical dirt, diff[3] may be larger than 1.
// In that case, use 1 instead of diff[3].
- Dune::FieldVector<T,3> v;
if (diff[3] > 1.0) {
v = 0;
@@ -645,13 +656,14 @@ public:
T invSinc = 1/sincHalf(dist);
// Compute log on T_a SO(3)
- v[0] = diff[0] * invSinc;
- v[1] = diff[1] * invSinc;
- v[2] = diff[2] * invSinc;
-
+ v[0] = 0.5 * diff[0] * invSinc;
+ v[1] = 0.5 * diff[1] * invSinc;
+ v[2] = 0.5 * diff[2] * invSinc;
+ v[3] = 0;
}
- return SkewMatrix<T,3>(v);
+ // multiply with base point to get real embedded tangent vector
+ return ((Quaternion<T>) a).mult(v);
}
/** \brief Compute the derivatives of the director vectors with respect to the quaternion coordinates
@@ -917,11 +929,8 @@ public:
static Rotation<T,3> interpolate(const Rotation<T,3>& a, const Rotation<T,3>& b, T omega) {
// Compute log on T_a SO(3)
- SkewMatrix<T,3> v = log(a,b);
-
- v *= omega;
-
- return a.mult(exp(v));
+ EmbeddedTangentVector v = log(a,b);
+ return exp(a, omega*v);
}
/** \brief Interpolate between two rotations
diff --git a/dune/gfe/unitvector.hh b/dune/gfe/unitvector.hh
index 7122d1cd..7a41e843 100644
--- a/dune/gfe/unitvector.hh
+++ b/dune/gfe/unitvector.hh
@@ -219,6 +219,10 @@ public:
return result;
}
+ /** \brief The inverse of the exponential map
+ *
+ * \results A vector in the tangent space of p
+ */
static EmbeddedTangentVector log(const UnitVector& p, const UnitVector& q)
{
EmbeddedTangentVector result = p.projectOntoTangentSpace(q.data_-p.data_);
diff --git a/test/targetspacetest.cc b/test/targetspacetest.cc
index 6747a708..cea7382c 100644
--- a/test/targetspacetest.cc
+++ b/test/targetspacetest.cc
@@ -28,6 +28,22 @@ double diameter(const std::vector<TargetSpace>& v)
const double eps = 1e-4;
+template <class TargetSpace>
+void testExpLog(const TargetSpace& a, const TargetSpace& b)
+{
+ // Check whether exp and log are mutually inverse
+ typename TargetSpace::EmbeddedTangentVector logarithm = TargetSpace::log(a,b);
+ TargetSpace exponential = TargetSpace::exp(a, logarithm);
+
+ if (TargetSpace::distance(b, exponential) > eps)
+ {
+ std::cout << className(a) << ": Exp and log are not mutually inverse." << std::endl;
+ std::cout << "exp(a,log(a,b)): " << exponential << std::endl;
+ std::cout << "b : " << b << std::endl;
+ assert(false);
+ }
+}
+
template <class TargetSpace>
double distanceSquared(const TargetSpace& a, const TargetSpace& b)
{
@@ -344,6 +360,10 @@ void test()
if (diameter(testPointPair) > TargetSpace::convexityRadius)
continue;
+ // Test the exponential map and the logarithm
+ testExpLog(testPoints[i], testPoints[j]);
+
+ // Test the various derivatives of the squared distance
testDerivativesOfDistanceSquared<TargetSpace>(testPoints[i], testPoints[j]);
}
--
GitLab