Geometry module: Bug fixes, test coverage, and benchmarks libeigen/eigen!2391 Co-authored-by: Rasmus Munk Larsen <rmlarsen@gmail.com>
diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h index 8b71cd8..bb6e962 100644 --- a/Eigen/src/Geometry/Quaternion.h +++ b/Eigen/src/Geometry/Quaternion.h
@@ -854,6 +854,7 @@ Scalar scale1; if (absD >= one) { + // Near-parallel quaternions: use lerp to avoid division by ~zero sinTheta. scale0 = Scalar(1) - t; scale1 = t; } else { @@ -883,7 +884,7 @@ // Ken Shoemake, 1987 SIGGRAPH course notes Scalar t = mat.trace(); if (t > Scalar(0)) { - t = sqrt(t + Scalar(1.0)); + t = sqrt(numext::maxi(t + Scalar(1.0), Scalar(0))); q.w() = Scalar(0.5) * t; t = Scalar(0.5) / t; q.x() = (mat.coeff(2, 1) - mat.coeff(1, 2)) * t; @@ -896,7 +897,8 @@ Index j = (i + 1) % 3; Index k = (j + 1) % 3; - t = sqrt(mat.coeff(i, i) - mat.coeff(j, j) - mat.coeff(k, k) + Scalar(1.0)); + // Guard against slightly negative argument from non-orthogonal matrices. + t = sqrt(numext::maxi(mat.coeff(i, i) - mat.coeff(j, j) - mat.coeff(k, k) + Scalar(1.0), Scalar(0))); q.coeffs().coeffRef(i) = Scalar(0.5) * t; t = Scalar(0.5) / t; q.w() = (mat.coeff(k, j) - mat.coeff(j, k)) * t;
diff --git a/Eigen/src/Geometry/Umeyama.h b/Eigen/src/Geometry/Umeyama.h index 8ed6344..246986e 100644 --- a/Eigen/src/Geometry/Umeyama.h +++ b/Eigen/src/Geometry/Umeyama.h
@@ -141,6 +141,13 @@ // Eq. (36)-(37) const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n; + if (src_var <= Scalar(0)) { + // Degenerate: source points have zero variance (all nearly identical). + // Scaling is undefined; return the best-fit pure translation. + Rt.col(m).head(m) = dst_mean - src_mean; + return Rt; + } + // Eq. (42) const Scalar c = Scalar(1) / src_var * svd.singularValues().dot(S);
diff --git a/benchmarks/Geometry/CMakeLists.txt b/benchmarks/Geometry/CMakeLists.txt index fd2c683..4a885a9 100644 --- a/benchmarks/Geometry/CMakeLists.txt +++ b/benchmarks/Geometry/CMakeLists.txt
@@ -1,3 +1,4 @@ eigen_add_benchmark(bench_transforms bench_transforms.cpp) eigen_add_benchmark(bench_geometry bench_geometry.cpp) eigen_add_benchmark(bench_quatmul bench_quatmul.cpp) +eigen_add_benchmark(bench_geometry_ops bench_geometry_ops.cpp)
diff --git a/benchmarks/Geometry/bench_geometry_ops.cpp b/benchmarks/Geometry/bench_geometry_ops.cpp new file mode 100644 index 0000000..4d93533 --- /dev/null +++ b/benchmarks/Geometry/bench_geometry_ops.cpp
@@ -0,0 +1,377 @@ +#include <benchmark/benchmark.h> +#include <Eigen/Core> +#include <Eigen/Geometry> +#include <Eigen/SVD> + +using namespace Eigen; + +// ============================================================================ +// Quaternion operations +// ============================================================================ + +template <typename Scalar> +static void BM_QuatSlerp(benchmark::State& state) { + Quaternion<Scalar> q0 = Quaternion<Scalar>::UnitRandom(); + Quaternion<Scalar> q1 = Quaternion<Scalar>::UnitRandom(); + Quaternion<Scalar> r; + Scalar t = Scalar(0.5); + for (auto _ : state) { + r = q0.slerp(t, q1); + benchmark::DoNotOptimize(r.coeffs().data()); + } +} + +template <typename Scalar> +static void BM_QuatRotateVec(benchmark::State& state) { + Quaternion<Scalar> q = Quaternion<Scalar>::UnitRandom(); + Matrix<Scalar, 3, 1> v = Matrix<Scalar, 3, 1>::Random(); + for (auto _ : state) { + v = q * v; + benchmark::DoNotOptimize(v.data()); + } +} + +template <typename Scalar> +static void BM_QuatNormalize(benchmark::State& state) { + Quaternion<Scalar> q(Scalar(1.1), Scalar(2.2), Scalar(3.3), Scalar(4.4)); + for (auto _ : state) { + q.normalize(); + benchmark::DoNotOptimize(q.coeffs().data()); + } +} + +template <typename Scalar> +static void BM_QuatInverse(benchmark::State& state) { + Quaternion<Scalar> q = Quaternion<Scalar>::UnitRandom(); + Quaternion<Scalar> r; + for (auto _ : state) { + r = q.inverse(); + benchmark::DoNotOptimize(r.coeffs().data()); + } +} + +template <typename Scalar> +static void BM_QuatToRotationMatrix(benchmark::State& state) { + Quaternion<Scalar> q = Quaternion<Scalar>::UnitRandom(); + Matrix<Scalar, 3, 3> m; + for (auto _ : state) { + m = q.toRotationMatrix(); + benchmark::DoNotOptimize(m.data()); + } +} + +template <typename Scalar> +static void BM_QuatFromRotationMatrix(benchmark::State& state) { + Quaternion<Scalar> q = Quaternion<Scalar>::UnitRandom(); + Matrix<Scalar, 3, 3> m = q.toRotationMatrix(); + for (auto _ : state) { + q = m; + benchmark::DoNotOptimize(q.coeffs().data()); + } +} + +template <typename Scalar> +static void BM_QuatAngularDistance(benchmark::State& state) { + Quaternion<Scalar> q0 = Quaternion<Scalar>::UnitRandom(); + Quaternion<Scalar> q1 = Quaternion<Scalar>::UnitRandom(); + Scalar d; + for (auto _ : state) { + d = q0.angularDistance(q1); + benchmark::DoNotOptimize(d); + } +} + +template <typename Scalar> +static void BM_QuatSetFromTwoVectors(benchmark::State& state) { + Matrix<Scalar, 3, 1> v0 = Matrix<Scalar, 3, 1>::Random().normalized(); + Matrix<Scalar, 3, 1> v1 = Matrix<Scalar, 3, 1>::Random().normalized(); + Quaternion<Scalar> q; + for (auto _ : state) { + q.setFromTwoVectors(v0, v1); + benchmark::DoNotOptimize(q.coeffs().data()); + } +} + +// ============================================================================ +// AngleAxis operations +// ============================================================================ + +template <typename Scalar> +static void BM_AngleAxisToRotationMatrix(benchmark::State& state) { + AngleAxis<Scalar> aa(Scalar(1.0), Matrix<Scalar, 3, 1>::UnitX()); + Matrix<Scalar, 3, 3> m; + for (auto _ : state) { + m = aa.toRotationMatrix(); + benchmark::DoNotOptimize(m.data()); + } +} + +template <typename Scalar> +static void BM_AngleAxisFromRotationMatrix(benchmark::State& state) { + Quaternion<Scalar> q = Quaternion<Scalar>::UnitRandom(); + Matrix<Scalar, 3, 3> m = q.toRotationMatrix(); + AngleAxis<Scalar> aa; + for (auto _ : state) { + aa = m; + benchmark::DoNotOptimize(aa); + } +} + +template <typename Scalar> +static void BM_AngleAxisRotateVec(benchmark::State& state) { + AngleAxis<Scalar> aa(Scalar(1.0), Matrix<Scalar, 3, 1>::Random().normalized()); + Matrix<Scalar, 3, 1> v = Matrix<Scalar, 3, 1>::Random(); + for (auto _ : state) { + v = aa * v; + benchmark::DoNotOptimize(v.data()); + } +} + +// ============================================================================ +// Rotation2D operations +// ============================================================================ + +template <typename Scalar> +static void BM_Rotation2DRotateVec(benchmark::State& state) { + Rotation2D<Scalar> r(Scalar(0.5)); + Matrix<Scalar, 2, 1> v = Matrix<Scalar, 2, 1>::Random(); + for (auto _ : state) { + v = r * v; + benchmark::DoNotOptimize(v.data()); + } +} + +template <typename Scalar> +static void BM_Rotation2DSlerp(benchmark::State& state) { + Rotation2D<Scalar> r0(Scalar(0.1)), r1(Scalar(2.5)); + Rotation2D<Scalar> r; + for (auto _ : state) { + r = r0.slerp(Scalar(0.5), r1); + benchmark::DoNotOptimize(r); + } +} + +// ============================================================================ +// Transform inverse +// ============================================================================ + +template <typename Scalar, int Mode> +static void BM_TransformInverse(benchmark::State& state) { + typedef Transform<Scalar, 3, Mode> Trans; + Trans t; + t.setIdentity(); + t.rotate(Quaternion<Scalar>::UnitRandom()); + t.translate(Matrix<Scalar, 3, 1>::Random()); + Trans r; + for (auto _ : state) { + r = t.inverse(); + benchmark::DoNotOptimize(r.data()); + } +} + +// ============================================================================ +// Euler angles +// ============================================================================ + +template <typename Scalar> +static void BM_EulerAnglesExtract(benchmark::State& state) { + Quaternion<Scalar> q = Quaternion<Scalar>::UnitRandom(); + Matrix<Scalar, 3, 3> m = q.toRotationMatrix(); + Matrix<Scalar, 3, 1> ea; + for (auto _ : state) { + ea = m.canonicalEulerAngles(0, 1, 2); + benchmark::DoNotOptimize(ea.data()); + } +} + +// ============================================================================ +// Cross product +// ============================================================================ + +template <typename Scalar> +static void BM_CrossProduct(benchmark::State& state) { + Matrix<Scalar, 3, 1> a = Matrix<Scalar, 3, 1>::Random(); + Matrix<Scalar, 3, 1> b = Matrix<Scalar, 3, 1>::Random(); + Matrix<Scalar, 3, 1> c; + for (auto _ : state) { + c = a.cross(b); + benchmark::DoNotOptimize(c.data()); + } +} + +template <typename Scalar> +static void BM_UnitOrthogonal(benchmark::State& state) { + Matrix<Scalar, 3, 1> v = Matrix<Scalar, 3, 1>::Random(); + Matrix<Scalar, 3, 1> r; + for (auto _ : state) { + r = v.unitOrthogonal(); + benchmark::DoNotOptimize(r.data()); + } +} + +// ============================================================================ +// AlignedBox operations +// ============================================================================ + +template <typename Scalar> +static void BM_AlignedBoxContains(benchmark::State& state) { + AlignedBox<Scalar, 3> box(Matrix<Scalar, 3, 1>(-1, -1, -1), Matrix<Scalar, 3, 1>(1, 1, 1)); + Matrix<Scalar, 3, 1> p = Matrix<Scalar, 3, 1>::Random(); + bool result; + for (auto _ : state) { + result = box.contains(p); + benchmark::DoNotOptimize(result); + } +} + +template <typename Scalar> +static void BM_AlignedBoxIntersects(benchmark::State& state) { + AlignedBox<Scalar, 3> box1(Matrix<Scalar, 3, 1>(-1, -1, -1), Matrix<Scalar, 3, 1>(1, 1, 1)); + AlignedBox<Scalar, 3> box2(Matrix<Scalar, 3, 1>(0, 0, 0), Matrix<Scalar, 3, 1>(2, 2, 2)); + bool result; + for (auto _ : state) { + result = box1.intersects(box2); + benchmark::DoNotOptimize(result); + } +} + +template <typename Scalar> +static void BM_AlignedBoxTransform(benchmark::State& state) { + AlignedBox<Scalar, 3> box(Matrix<Scalar, 3, 1>(-1, -1, -1), Matrix<Scalar, 3, 1>(1, 1, 1)); + Transform<Scalar, 3, Isometry> t; + t.setIdentity(); + t.rotate(Quaternion<Scalar>::UnitRandom()); + t.translate(Matrix<Scalar, 3, 1>::Random()); + for (auto _ : state) { + AlignedBox<Scalar, 3> result = box.transformed(t); + benchmark::DoNotOptimize(result); + } +} + +// ============================================================================ +// Hyperplane / ParametrizedLine +// ============================================================================ + +template <typename Scalar> +static void BM_HyperplaneSignedDistance(benchmark::State& state) { + Hyperplane<Scalar, 3> plane = Hyperplane<Scalar, 3>::Through( + Matrix<Scalar, 3, 1>(1, 0, 0), Matrix<Scalar, 3, 1>(0, 1, 0), Matrix<Scalar, 3, 1>(0, 0, 1)); + Matrix<Scalar, 3, 1> p = Matrix<Scalar, 3, 1>::Random(); + Scalar d; + for (auto _ : state) { + d = plane.signedDistance(p); + benchmark::DoNotOptimize(d); + } +} + +template <typename Scalar> +static void BM_LinePointDistance(benchmark::State& state) { + ParametrizedLine<Scalar, 3> line(Matrix<Scalar, 3, 1>::Zero(), Matrix<Scalar, 3, 1>::UnitX()); + Matrix<Scalar, 3, 1> p = Matrix<Scalar, 3, 1>::Random(); + Scalar d; + for (auto _ : state) { + d = line.distance(p); + benchmark::DoNotOptimize(d); + } +} + +template <typename Scalar> +static void BM_LinePlaneIntersection(benchmark::State& state) { + ParametrizedLine<Scalar, 3> line(Matrix<Scalar, 3, 1>::Zero(), Matrix<Scalar, 3, 1>::UnitZ()); + Hyperplane<Scalar, 3> plane(Matrix<Scalar, 3, 1>::UnitZ(), Scalar(-5)); + Scalar t; + for (auto _ : state) { + t = line.intersectionParameter(plane); + benchmark::DoNotOptimize(t); + } +} + +// ============================================================================ +// Umeyama +// ============================================================================ + +template <typename Scalar, int Dim> +static void BM_Umeyama(benchmark::State& state) { + const int n = state.range(0); + Matrix<Scalar, Dim, Dynamic> src = Matrix<Scalar, Dim, Dynamic>::Random(Dim, n); + // Apply a known transform + Matrix<Scalar, Dim, Dim> R = Quaternion<Scalar>::UnitRandom().toRotationMatrix(); + Matrix<Scalar, Dim, 1> t = Matrix<Scalar, Dim, 1>::Random() * Scalar(10); + Matrix<Scalar, Dim, Dynamic> dst = (R * src).colwise() + t; + for (auto _ : state) { + auto T = umeyama(src, dst); + benchmark::DoNotOptimize(T); + } +} + +// ============================================================================ +// Registration +// ============================================================================ + +// Quaternion +BENCHMARK(BM_QuatSlerp<float>); +BENCHMARK(BM_QuatSlerp<double>); +BENCHMARK(BM_QuatRotateVec<float>); +BENCHMARK(BM_QuatRotateVec<double>); +BENCHMARK(BM_QuatNormalize<float>); +BENCHMARK(BM_QuatNormalize<double>); +BENCHMARK(BM_QuatInverse<float>); +BENCHMARK(BM_QuatInverse<double>); +BENCHMARK(BM_QuatToRotationMatrix<float>); +BENCHMARK(BM_QuatToRotationMatrix<double>); +BENCHMARK(BM_QuatFromRotationMatrix<float>); +BENCHMARK(BM_QuatFromRotationMatrix<double>); +BENCHMARK(BM_QuatAngularDistance<float>); +BENCHMARK(BM_QuatAngularDistance<double>); +BENCHMARK(BM_QuatSetFromTwoVectors<float>); +BENCHMARK(BM_QuatSetFromTwoVectors<double>); + +// AngleAxis +BENCHMARK(BM_AngleAxisToRotationMatrix<float>); +BENCHMARK(BM_AngleAxisToRotationMatrix<double>); +BENCHMARK(BM_AngleAxisFromRotationMatrix<float>); +BENCHMARK(BM_AngleAxisFromRotationMatrix<double>); +BENCHMARK(BM_AngleAxisRotateVec<float>); +BENCHMARK(BM_AngleAxisRotateVec<double>); + +// Rotation2D +BENCHMARK(BM_Rotation2DRotateVec<float>); +BENCHMARK(BM_Rotation2DRotateVec<double>); +BENCHMARK(BM_Rotation2DSlerp<float>); +BENCHMARK(BM_Rotation2DSlerp<double>); + +// Transform inverse +BENCHMARK(BM_TransformInverse<float, Isometry>); +BENCHMARK(BM_TransformInverse<float, Affine>); +BENCHMARK(BM_TransformInverse<double, Isometry>); +BENCHMARK(BM_TransformInverse<double, Affine>); + +// Euler angles +BENCHMARK(BM_EulerAnglesExtract<float>); +BENCHMARK(BM_EulerAnglesExtract<double>); + +// Cross product & orthogonal +BENCHMARK(BM_CrossProduct<float>); +BENCHMARK(BM_CrossProduct<double>); +BENCHMARK(BM_UnitOrthogonal<float>); +BENCHMARK(BM_UnitOrthogonal<double>); + +// AlignedBox +BENCHMARK(BM_AlignedBoxContains<float>); +BENCHMARK(BM_AlignedBoxContains<double>); +BENCHMARK(BM_AlignedBoxIntersects<float>); +BENCHMARK(BM_AlignedBoxIntersects<double>); +BENCHMARK(BM_AlignedBoxTransform<float>); +BENCHMARK(BM_AlignedBoxTransform<double>); + +// Hyperplane / ParametrizedLine +BENCHMARK(BM_HyperplaneSignedDistance<float>); +BENCHMARK(BM_HyperplaneSignedDistance<double>); +BENCHMARK(BM_LinePointDistance<float>); +BENCHMARK(BM_LinePointDistance<double>); +BENCHMARK(BM_LinePlaneIntersection<float>); +BENCHMARK(BM_LinePlaneIntersection<double>); + +// Umeyama +BENCHMARK(BM_Umeyama<float, 3>)->Arg(10)->Arg(100)->Arg(1000); +BENCHMARK(BM_Umeyama<double, 3>)->Arg(10)->Arg(100)->Arg(1000);
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 5a7cdae..fad0267 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt
@@ -288,6 +288,7 @@ ei_add_test(geo_hyperplane) ei_add_test(geo_transformations) ei_add_test(geo_homogeneous) +ei_add_test(geo_misc) ei_add_test(stdvector) ei_add_test(stdvector_overload) ei_add_test(stdlist)
diff --git a/test/geo_eulerangles.cpp b/test/geo_eulerangles.cpp index 11c0449..ac00d0f 100644 --- a/test/geo_eulerangles.cpp +++ b/test/geo_eulerangles.cpp
@@ -162,22 +162,22 @@ // Similar to above, but with pi instead of pi/2 Vector3 ea_pi = ea; ea_pi[1] = kPi; - test_with_some_zeros(ea_gl); + test_with_some_zeros(ea_pi); ea_pi[1] += internal::random<Scalar>(-smallVal, smallVal); - test_with_some_zeros(ea_gl); + test_with_some_zeros(ea_pi); ea_pi[1] = -kPi; - test_with_some_zeros(ea_gl); + test_with_some_zeros(ea_pi); ea_pi[1] += internal::random<Scalar>(-smallVal, smallVal); - test_with_some_zeros(ea_gl); + test_with_some_zeros(ea_pi); ea_pi[1] = kPi; ea_pi[2] = ea_pi[0]; - test_with_some_zeros(ea_gl); + test_with_some_zeros(ea_pi); ea_pi[1] += internal::random<Scalar>(-smallVal, smallVal); - test_with_some_zeros(ea_gl); + test_with_some_zeros(ea_pi); ea_pi[1] = -kPi; - test_with_some_zeros(ea_gl); + test_with_some_zeros(ea_pi); ea_pi[1] += internal::random<Scalar>(-smallVal, smallVal); - test_with_some_zeros(ea_gl); + test_with_some_zeros(ea_pi); ea[2] = ea[0] = internal::random<Scalar>(0, kPi); check_all_var(ea);
diff --git a/test/geo_misc.cpp b/test/geo_misc.cpp new file mode 100644 index 0000000..ab88a63 --- /dev/null +++ b/test/geo_misc.cpp
@@ -0,0 +1,625 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2025 Rasmus Munk Larsen <rmlarsen@google.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/. + +// Tests for Geometry module classes that lack dedicated test coverage: +// - AngleAxis edge cases +// - Rotation2D standalone tests +// - Translation standalone tests +// - Scaling standalone tests +// - Quaternion fromRotationMatrix with non-orthogonal input +// - Quaternion slerp edge cases +// - Umeyama degenerate input +// - Transform shear ground truth + +#include "main.h" +#include <Eigen/Geometry> +#include <Eigen/LU> +#include <Eigen/SVD> + +template <typename Scalar> +void angleaxis_edge_cases() { + typedef Matrix<Scalar, 3, 1> Vector3; + typedef Matrix<Scalar, 3, 3> Matrix3; + typedef AngleAxis<Scalar> AngleAxisx; + typedef Quaternion<Scalar> Quaternionx; + + Scalar eps = test_precision<Scalar>(); + + // Identity rotation (angle = 0) + { + AngleAxisx aa(Scalar(0), Vector3::UnitZ()); + Matrix3 m = aa.toRotationMatrix(); + VERIFY_IS_APPROX(m, Matrix3::Identity()); + // Verify orthogonality: R^T * R = I + VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity()); + VERIFY_IS_APPROX(m.determinant(), Scalar(1)); + Quaternionx q(aa); + VERIFY_IS_APPROX(q.toRotationMatrix(), Matrix3::Identity()); + VERIFY_IS_APPROX(q.norm(), Scalar(1)); + } + + // 180-degree rotation around each axis + { + for (int axis = 0; axis < 3; ++axis) { + Vector3 ax = Vector3::Zero(); + ax(axis) = Scalar(1); + AngleAxisx aa(Scalar(EIGEN_PI), ax); + Matrix3 m = aa.toRotationMatrix(); + // Should be its own inverse + VERIFY_IS_APPROX(m * m, Matrix3::Identity()); + // Verify orthogonality + VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity()); + VERIFY_IS_APPROX(m.determinant(), Scalar(1)); + // Diagonal entry for the rotation axis should be +1, others -1 + VERIFY_IS_APPROX(m(axis, axis), Scalar(1)); + for (int j = 0; j < 3; ++j) { + if (j != axis) VERIFY_IS_APPROX(m(j, j), Scalar(-1)); + } + } + } + + // Very small angle: verify Rodrigues formula accuracy + { + Scalar tiny = Scalar(10) * NumTraits<Scalar>::epsilon(); + AngleAxisx aa(tiny, Vector3::UnitX()); + Matrix3 m = aa.toRotationMatrix(); + VERIFY((m - Matrix3::Identity()).norm() < Scalar(4) * tiny); + // Orthogonality should hold to machine precision even for tiny angles + VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity()); + VERIFY_IS_APPROX(m.determinant(), Scalar(1)); + // Round-trip through quaternion + Quaternionx q(aa); + AngleAxisx aa2(q); + VERIFY(abs(aa2.angle()) < Scalar(4) * tiny); + } + + // Round-trip: AngleAxis -> Matrix -> AngleAxis + { + Scalar a = internal::random<Scalar>(Scalar(0.1), Scalar(EIGEN_PI) - Scalar(0.1)); + Vector3 axis = Vector3::Random().normalized(); + AngleAxisx aa(a, axis); + Matrix3 m = aa.toRotationMatrix(); + VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity()); + VERIFY_IS_APPROX(m.determinant(), Scalar(1)); + AngleAxisx aa2; + aa2.fromRotationMatrix(m); + VERIFY_IS_APPROX(aa.toRotationMatrix(), aa2.toRotationMatrix()); + // Verify angle is preserved + VERIFY(abs(aa2.angle() - a) < eps); + } + + // Round-trip for near-180-degree rotation + { + Scalar a = Scalar(EIGEN_PI) - Scalar(10) * NumTraits<Scalar>::epsilon(); + Vector3 axis = Vector3::Random().normalized(); + AngleAxisx aa(a, axis); + Matrix3 m = aa.toRotationMatrix(); + VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity()); + VERIFY_IS_APPROX(m.determinant(), Scalar(1)); + AngleAxisx aa2(m); + // The rotation should be equivalent even if axis flips sign + VERIFY_IS_APPROX(aa.toRotationMatrix(), aa2.toRotationMatrix()); + } + + // Accessors + { + Vector3 axis = Vector3::Random().normalized(); + Scalar angle = internal::random<Scalar>(Scalar(-EIGEN_PI), Scalar(EIGEN_PI)); + AngleAxisx aa(angle, axis); + VERIFY_IS_APPROX(aa.angle(), angle); + VERIFY_IS_APPROX(aa.axis(), axis); + + // Mutable accessors + aa.angle() = Scalar(1); + aa.axis() = Vector3::UnitZ(); + VERIFY_IS_APPROX(aa.angle(), Scalar(1)); + VERIFY_IS_APPROX(aa.axis(), Vector3::UnitZ()); + } + + // inverse + { + Scalar a = internal::random<Scalar>(Scalar(0.1), Scalar(EIGEN_PI)); + Vector3 axis = Vector3::Random().normalized(); + AngleAxisx aa(a, axis); + VERIFY_IS_APPROX((aa * aa.inverse()).toRotationMatrix(), Matrix3::Identity()); + } + + // isApprox + { + Scalar a = internal::random<Scalar>(Scalar(0.1), Scalar(EIGEN_PI)); + Vector3 axis = Vector3::Random().normalized(); + AngleAxisx aa1(a, axis); + AngleAxisx aa2(a + eps / Scalar(10), axis); + VERIFY(aa1.isApprox(aa2, eps)); + } + + // Quaternion round-trip: AngleAxis -> Quaternion -> AngleAxis + { + Scalar a = internal::random<Scalar>(Scalar(0.1), Scalar(EIGEN_PI) - Scalar(0.1)); + Vector3 axis = Vector3::Random().normalized(); + AngleAxisx aa(a, axis); + Quaternionx q(aa); + VERIFY_IS_APPROX(q.norm(), Scalar(1)); + AngleAxisx aa2(q); + VERIFY(abs(aa2.angle() - a) < eps); + // Axis should agree (possibly with sign flip for angle > pi) + VERIFY(aa2.axis().isApprox(axis, eps) || aa2.axis().isApprox(-axis, eps)); + } +} + +template <typename Scalar> +void rotation2d_standalone() { + typedef Matrix<Scalar, 2, 1> Vector2; + typedef Matrix<Scalar, 2, 2> Matrix2; + typedef Rotation2D<Scalar> Rotation2Dx; + + Scalar eps = test_precision<Scalar>(); + + // Construction and basic properties + { + Rotation2Dx r(Scalar(0)); + VERIFY_IS_APPROX(r.toRotationMatrix(), Matrix2::Identity()); + } + + // 90-degree rotation + { + Rotation2Dx r(Scalar(EIGEN_PI) / Scalar(2)); + Vector2 v(1, 0); + Vector2 rv = r * v; + VERIFY_IS_APPROX(rv, Vector2(0, 1)); + } + + // 180-degree rotation + { + Rotation2Dx r(Scalar(EIGEN_PI)); + Vector2 v(1, 0); + Vector2 rv = r * v; + VERIFY_IS_APPROX(rv, Vector2(-1, 0)); + } + + // inverse + { + Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); + Rotation2Dx r(a); + VERIFY_IS_APPROX((r * r.inverse()).toRotationMatrix(), Matrix2::Identity()); + } + + // composition + { + Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); + Scalar b = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); + Rotation2Dx ra(a), rb(b); + VERIFY_IS_APPROX((ra * rb).toRotationMatrix(), ra.toRotationMatrix() * rb.toRotationMatrix()); + } + + // fromRotationMatrix round-trip: angle should agree to tight tolerance + { + Scalar a = internal::random<Scalar>(-Scalar(EIGEN_PI), Scalar(EIGEN_PI)); + Rotation2Dx r(a); + Matrix2 m = r.toRotationMatrix(); + // Verify orthogonality + VERIFY_IS_APPROX(m.transpose() * m, Matrix2::Identity()); + VERIFY_IS_APPROX(m.determinant(), Scalar(1)); + Rotation2Dx r2(m); + // Angles should agree mod 2*pi. Use rotation matrices for exact comparison. + VERIFY_IS_APPROX(r2.toRotationMatrix(), r.toRotationMatrix()); + } + + // slerp at t=0.5 of inverse should give ~0 or ~pi + { + Scalar a = internal::random<Scalar>(Scalar(0.1), Scalar(EIGEN_PI) / Scalar(2)); + Rotation2Dx r(a); + Rotation2Dx half = r.slerp(Scalar(0.5), r.inverse()); + // The midpoint between a rotation and its inverse via shortest path should be ~0 or ~pi + Scalar halfAngle = half.smallestAngle(); + VERIFY(abs(halfAngle) < eps || abs(abs(halfAngle) - Scalar(EIGEN_PI)) < eps); + } + + // Rotation2D slerp interpolation: verify linearity of angle + { + Scalar a0 = internal::random<Scalar>(Scalar(-EIGEN_PI), Scalar(EIGEN_PI)); + Scalar a1 = a0 + internal::random<Scalar>(Scalar(0.1), Scalar(1.0)); + Rotation2Dx r0(a0), r1(a1); + Scalar t = Scalar(0.3); + Rotation2Dx rt = r0.slerp(t, r1); + // Slerp for 2D rotations is just linear interpolation of angle along shortest path. + Scalar expected = a0 + t * Rotation2Dx(a1 - a0).smallestAngle(); + VERIFY(abs(Rotation2Dx(rt.angle() - expected).smallestAngle()) < eps); + } + + // smallestAngle range + for (int k = 0; k < 100; ++k) { + Scalar a = internal::random<Scalar>(Scalar(-100), Scalar(100)); + Rotation2Dx r(a); + VERIFY(r.smallestAngle() >= -Scalar(EIGEN_PI)); + VERIFY(r.smallestAngle() <= Scalar(EIGEN_PI)); + VERIFY(r.smallestPositiveAngle() >= Scalar(0)); + VERIFY(r.smallestPositiveAngle() <= Scalar(2) * Scalar(EIGEN_PI)); + } +} + +template <typename Scalar> +void translation_standalone() { + typedef Matrix<Scalar, 3, 1> Vector3; + typedef Translation<Scalar, 3> Translation3; + typedef Transform<Scalar, 3, Isometry> Isometry3; + + // Construction + { + Vector3 v(1, 2, 3); + Translation3 t(v); + VERIFY_IS_APPROX(t.vector(), v); + + Translation3 t2(Scalar(1), Scalar(2), Scalar(3)); + VERIFY_IS_APPROX(t.vector(), t2.vector()); + } + + // Identity + { + Translation3 t = Translation3::Identity(); + VERIFY_IS_APPROX(t.vector(), Vector3::Zero()); + } + + // Composition + { + Vector3 v1(1, 2, 3), v2(4, 5, 6); + Translation3 t1(v1), t2(v2); + Translation3 t3 = t1 * t2; + VERIFY_IS_APPROX(t3.vector(), v1 + v2); + } + + // Inverse + { + Vector3 v(1, 2, 3); + Translation3 t(v); + Translation3 tinv = t.inverse(); + VERIFY_IS_APPROX(tinv.vector(), -v); + VERIFY_IS_APPROX((t * tinv).vector(), Vector3::Zero()); + } + + // Translation * vector (via transform) + { + Vector3 v(1, 2, 3), p(10, 20, 30); + Translation3 t(v); + VERIFY_IS_APPROX(t * p, v + p); + } + + // Casting + { + Translation3 t(Vector3(1, 2, 3)); + Translation<float, 3> tf = t.template cast<float>(); + VERIFY_IS_APPROX(tf.template cast<Scalar>().vector(), t.vector()); + } + + // Convert to Isometry + { + Vector3 v(1, 2, 3); + Translation3 t(v); + Isometry3 iso(t); + VERIFY_IS_APPROX(iso.translation(), v); + typedef Matrix<Scalar, 3, 3> Matrix3; + VERIFY_IS_APPROX(iso.linear(), Matrix3::Identity()); + } +} + +template <typename Scalar> +void scaling_standalone() { + typedef Matrix<Scalar, 3, 1> Vector3; + typedef Translation<Scalar, 3> Translation3; + typedef Transform<Scalar, 3, Affine> Affine3; + typedef UniformScaling<Scalar> UniformScalingx; + + // UniformScaling construction + { + UniformScalingx s(Scalar(2)); + VERIFY_IS_APPROX(s.factor(), Scalar(2)); + } + + // UniformScaling * vector + { + UniformScalingx s(Scalar(3)); + Vector3 v(1, 2, 3); + // UniformScaling * Translation + Translation3 t(v); + Affine3 result = s * t; + VERIFY_IS_APPROX(result.translation(), v * Scalar(3)); + } + + // UniformScaling composition + { + UniformScalingx s1(Scalar(2)), s2(Scalar(3)); + UniformScalingx s3 = s1 * s2; + VERIFY_IS_APPROX(s3.factor(), Scalar(6)); + } + + // UniformScaling inverse + { + UniformScalingx s(Scalar(4)); + UniformScalingx si = s.inverse(); + VERIFY_IS_APPROX(si.factor(), Scalar(1) / Scalar(4)); + VERIFY_IS_APPROX((s * si).factor(), Scalar(1)); + } + + // Scaling convenience functions + { + Scalar s = Scalar(3); + auto su = Eigen::Scaling(s); + VERIFY_IS_APPROX(su.factor(), s); + + Vector3 sv(1, 2, 3); + auto sd = Eigen::Scaling(sv); + VERIFY_IS_APPROX(Vector3(sd.diagonal()), sv); + } +} + +template <typename Scalar> +void quaternion_from_non_orthogonal_matrix() { + typedef Matrix<Scalar, 3, 3> Matrix3; + typedef Quaternion<Scalar> Quaternionx; + + // Slightly non-orthogonal matrix should not produce NaN + { + Matrix3 m = Matrix3::Identity(); + // Perturb to make slightly non-orthogonal + m(0, 0) += Scalar(1e-6); + m(1, 1) -= Scalar(1e-6); + Quaternionx q(m); + // Should not be NaN + VERIFY(!(numext::isnan)(q.w())); + VERIFY(!(numext::isnan)(q.x())); + VERIFY(!(numext::isnan)(q.y())); + VERIFY(!(numext::isnan)(q.z())); + // Should still be approximately a rotation + VERIFY(abs(q.norm() - Scalar(1)) < Scalar(0.01)); + } + + // Negative trace case with non-orthogonal matrix + { + // 180-degree rotation around Z axis, slightly perturbed + Matrix3 m; + m << Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(1); + m(0, 0) += Scalar(1e-6); + Quaternionx q(m); + VERIFY(!(numext::isnan)(q.w())); + VERIFY(!(numext::isnan)(q.x())); + VERIFY(!(numext::isnan)(q.y())); + VERIFY(!(numext::isnan)(q.z())); + } + + // Matrix with trace very close to -1 (worst case for positive trace branch) + { + Matrix3 m; + m << Scalar(-1), Scalar(0), Scalar(0), Scalar(0), Scalar(-0.5), Scalar(0), Scalar(0), Scalar(0), Scalar(-0.5); + // trace = -2, negative trace path + Quaternionx q(m); + VERIFY(!(numext::isnan)(q.w())); + VERIFY(!(numext::isnan)(q.x())); + } + + // Verify graceful handling of arbitrary non-rotation matrices. + // Shoemake's argmax-diagonal selection makes the sqrt argument non-negative + // for any real matrix, but the numext::maxi guard provides additional safety. + { + Matrix3 m; + m << Scalar(-0.4), Scalar(0.5), Scalar(0), Scalar(-0.5), Scalar(-0.4), Scalar(0), Scalar(0), Scalar(0), + Scalar(-0.4); + Quaternionx q(m); + VERIFY(!(numext::isnan)(q.w())); + VERIFY(!(numext::isnan)(q.x())); + VERIFY(!(numext::isnan)(q.y())); + VERIFY(!(numext::isnan)(q.z())); + } +} + +template <typename Scalar> +void quaternion_slerp_edge_cases() { + typedef Quaternion<Scalar> Quaternionx; + typedef AngleAxis<Scalar> AngleAxisx; + typedef Matrix<Scalar, 3, 1> Vector3; + typedef Matrix<Scalar, 3, 3> Matrix3; + + Scalar eps = test_precision<Scalar>(); + // The slerp formula preserves unit norm to O(epsilon^2) for unit inputs. + Scalar tight = Scalar(32) * NumTraits<Scalar>::epsilon(); + + // slerp with identical quaternions + { + Quaternionx q = Quaternionx::UnitRandom(); + Quaternionx r = q.slerp(Scalar(0.5), q); + VERIFY_IS_APPROX(r.coeffs(), q.coeffs()); + VERIFY(abs(r.norm() - Scalar(1)) < tight); + } + + // slerp at t=0 and t=1 + { + Quaternionx q0 = Quaternionx::UnitRandom(); + Quaternionx q1 = Quaternionx::UnitRandom(); + Quaternionx r0 = q0.slerp(Scalar(0), q1); + Quaternionx r1 = q0.slerp(Scalar(1), q1); + VERIFY(abs(r0.norm() - Scalar(1)) < tight); + VERIFY(abs(r1.norm() - Scalar(1)) < tight); + VERIFY_IS_APPROX(r0.toRotationMatrix(), q0.toRotationMatrix()); + VERIFY_IS_APPROX(r1.toRotationMatrix(), q1.toRotationMatrix()); + } + + // slerp with antipodal quaternions (represent same rotation) + { + Quaternionx q = Quaternionx::UnitRandom(); + Quaternionx qn; + qn.coeffs() = -q.coeffs(); + for (Scalar t = 0; t <= Scalar(1.001); t += Scalar(0.25)) { + Quaternionx r = q.slerp(t, qn); + VERIFY(abs(r.norm() - Scalar(1)) < tight); + // Should stay at the same rotation + VERIFY_IS_APPROX(r.toRotationMatrix(), q.toRotationMatrix()); + } + } + + // slerp with 180-degree apart quaternions + { + Vector3 axis1 = Vector3::UnitX(); + Quaternionx q0(AngleAxisx(Scalar(0), axis1)); + Quaternionx q1(AngleAxisx(Scalar(EIGEN_PI), axis1)); + Quaternionx mid = q0.slerp(Scalar(0.5), q1); + VERIFY(abs(mid.norm() - Scalar(1)) < tight); + // Midpoint should be a 90-degree rotation + AngleAxisx aa(mid); + VERIFY(abs(aa.angle() - Scalar(EIGEN_PI) / Scalar(2)) < test_precision<Scalar>()); + } + + // slerp unit-norm preservation and rotation matrix orthogonality + { + Quaternionx q0 = Quaternionx::UnitRandom(); + Quaternionx q1 = Quaternionx::UnitRandom(); + for (Scalar t = 0; t <= Scalar(1.001); t += Scalar(0.05)) { + Quaternionx r = q0.slerp(t, q1); + VERIFY(abs(r.norm() - Scalar(1)) < tight); + Matrix3 m = r.toRotationMatrix(); + VERIFY_IS_APPROX(m.transpose() * m, Matrix3::Identity()); + VERIFY_IS_APPROX(m.determinant(), Scalar(1)); + } + } + + // slerp monotonicity: angle from q0 should increase monotonically + { + Quaternionx q0 = Quaternionx::UnitRandom(); + Quaternionx q1 = Quaternionx::UnitRandom(); + Scalar prev_angle = Scalar(0); + for (Scalar t = Scalar(0); t <= Scalar(1.001); t += Scalar(0.05)) { + Quaternionx r = q0.slerp(t, q1); + Scalar angle = AngleAxisx(r * q0.inverse()).angle(); + if (angle > Scalar(EIGEN_PI)) angle = Scalar(2) * Scalar(EIGEN_PI) - angle; + VERIFY(angle >= prev_angle - tight); + prev_angle = angle; + } + } +} + +template <typename Scalar> +void umeyama_degenerate() { + typedef Matrix<Scalar, 3, Dynamic> Points3; + typedef Matrix<Scalar, 3, 1> Vector3; + typedef Matrix<Scalar, 3, 3> Matrix3; + typedef Matrix<Scalar, 4, 4> Matrix4; + + // All source points identical + { + int n = 10; + Points3 src(3, n); + src.colwise() = Vector3(1, 2, 3); + Points3 dst = Points3::Random(3, n); + + Matrix4 T = umeyama(src, dst); + // Result should NOT be NaN + VERIFY(!(numext::isnan)(T(0, 0))); + VERIFY(!(numext::isnan)(T(0, 3))); + } + + // All destination points identical + { + int n = 10; + Points3 src = Points3::Random(3, n); + Points3 dst(3, n); + dst.colwise() = Vector3(4, 5, 6); + + Matrix4 T = umeyama(src, dst); + VERIFY(!(numext::isnan)(T(0, 0))); + } + + // Pure translation (no rotation or scaling) + { + int n = 20; + Points3 src = Points3::Random(3, n); + Vector3 offset(10, 20, 30); + Points3 dst = src.colwise() + offset; + + Matrix4 T = umeyama(src, dst); + // Translation should be recovered + Vector3 t_recovered = T.template block<3, 1>(0, 3); + VERIFY_IS_APPROX(t_recovered, offset); + // Rotation should be identity + Matrix3 R = T.template block<3, 3>(0, 0); + VERIFY(R.isApprox(Matrix3::Identity(), test_precision<Scalar>())); + } +} + +template <typename Scalar> +void shear_ground_truth() { + typedef Transform<Scalar, 2, Affine> Transform2; + typedef Matrix<Scalar, 2, 1> Vector2; + + // Verify the actual matrix produced by shear() + // Note: Eigen's shear(sx, sy) applies the matrix [[1, sy], [sx, 1]] to columns, + // meaning sx controls the y-component shear and sy controls the x-component shear. + { + Transform2 t; + t.setIdentity(); + t.shear(Scalar(2), Scalar(0)); + // After shear(2, 0), linear part should be [[1, 0], [2, 1]] + VERIFY_IS_APPROX(t.linear()(0, 0), Scalar(1)); + VERIFY_IS_APPROX(t.linear()(0, 1), Scalar(0)); + VERIFY_IS_APPROX(t.linear()(1, 0), Scalar(2)); + VERIFY_IS_APPROX(t.linear()(1, 1), Scalar(1)); + } + + { + Transform2 t; + t.setIdentity(); + t.shear(Scalar(0), Scalar(3)); + // After shear(0, 3), linear part should be [[1, 3], [0, 1]] + VERIFY_IS_APPROX(t.linear()(0, 0), Scalar(1)); + VERIFY_IS_APPROX(t.linear()(0, 1), Scalar(3)); + VERIFY_IS_APPROX(t.linear()(1, 0), Scalar(0)); + VERIFY_IS_APPROX(t.linear()(1, 1), Scalar(1)); + } + + // Verify shear on a point + { + Transform2 t; + t.setIdentity(); + t.shear(Scalar(2), Scalar(3)); + Vector2 v(1, 0); + Vector2 result = t.linear() * v; + // [[1, 3], [2, 1]] * [1, 0]^T = [1, 2] + VERIFY_IS_APPROX(result, Vector2(1, 2)); + + v = Vector2(0, 1); + result = t.linear() * v; + // [[1, 3], [2, 1]] * [0, 1]^T = [3, 1] + VERIFY_IS_APPROX(result, Vector2(3, 1)); + } +} + +EIGEN_DECLARE_TEST(geo_misc) { + for (int i = 0; i < g_repeat; i++) { + CALL_SUBTEST_1(angleaxis_edge_cases<float>()); + CALL_SUBTEST_2(angleaxis_edge_cases<double>()); + + CALL_SUBTEST_3(rotation2d_standalone<float>()); + CALL_SUBTEST_4(rotation2d_standalone<double>()); + + CALL_SUBTEST_5(translation_standalone<float>()); + CALL_SUBTEST_5(translation_standalone<double>()); + + CALL_SUBTEST_6(scaling_standalone<float>()); + CALL_SUBTEST_6(scaling_standalone<double>()); + + CALL_SUBTEST_7(quaternion_from_non_orthogonal_matrix<float>()); + CALL_SUBTEST_7(quaternion_from_non_orthogonal_matrix<double>()); + + CALL_SUBTEST_8(quaternion_slerp_edge_cases<float>()); + CALL_SUBTEST_8(quaternion_slerp_edge_cases<double>()); + + CALL_SUBTEST_9(umeyama_degenerate<float>()); + CALL_SUBTEST_9(umeyama_degenerate<double>()); + + CALL_SUBTEST_10(shear_ground_truth<float>()); + CALL_SUBTEST_10(shear_ground_truth<double>()); + } +}
diff --git a/test/geo_quaternion.cpp b/test/geo_quaternion.cpp index 159a937..27c6881 100644 --- a/test/geo_quaternion.cpp +++ b/test/geo_quaternion.cpp
@@ -37,7 +37,7 @@ Scalar theta = AA(q * q0.inverse()).angle(); VERIFY(abs(q.norm() - 1) < largeEps); if (theta_tot == 0) - VERIFY(theta_tot == 0); + VERIFY(theta < largeEps); else VERIFY(abs(theta - t * theta_tot) < largeEps); } @@ -55,7 +55,6 @@ typedef AngleAxis<Scalar> AngleAxisx; Scalar largeEps = test_precision<Scalar>(); - if (internal::is_same<Scalar, float>::value) largeEps = Scalar(1e-3); Scalar eps = internal::random<Scalar>() * Scalar(1e-2);