From f62dccdb37f2104856c621bce7fd79e7c92d5a9b Mon Sep 17 00:00:00 2001 From: "Julio L. Paneque" Date: Thu, 4 Aug 2022 12:41:10 +0200 Subject: [PATCH] Fix the Sphere and Line Manifold formulations This PR changes the Sphere and Line Manifold formulations so that their tangent spaces represent traveled angles (for the sphere and the line direction vector) and traveled distance (for the line origin). These magnitudes were previously halved according to "Hartley & Zisserman (2nd Edition)", but in the majority of the state of the art this is not done, following the convention that magnitudes in the tangent space of the unit sphere represent geodesic distances traveled on that manifold. The same scale factor appears in the Quaternion Manifold implementation and will be studied in a further PR. This PR also adds an additional case in the Sphere Minus operator when hy_norm == 0. The value of y_minus_x was fixed to 0 but actually its last term can also be Pi depending on y_last. Finally, new unit tests for the Plus and Minus operator are added, along with new tests for the 2D Sphere (a.k.a. Circle) Manifold. Change-Id: I9456f1675b20da49bede5d6759aabf3cdfb26eae --- .../internal/sphere_manifold_functions.h | 30 ++-- include/ceres/line_manifold.h | 11 +- internal/ceres/manifold_test.cc | 137 ++++++++++++++++-- 3 files changed, 148 insertions(+), 30 deletions(-) diff --git a/include/ceres/internal/sphere_manifold_functions.h b/include/ceres/internal/sphere_manifold_functions.h index f9af5ec15..ab3760549 100644 --- a/include/ceres/internal/sphere_manifold_functions.h +++ b/include/ceres/internal/sphere_manifold_functions.h @@ -65,18 +65,16 @@ inline void ComputeSphereManifoldPlus(const VT& v, double beta, const XT& x, const DeltaT& delta, - double norm_delta, + const double norm_delta, XPlusDeltaT* x_plus_delta) { constexpr int AmbientDim = VT::RowsAtCompileTime; // Map the delta from the minimum representation to the over parameterized // homogeneous vector. See B.2 p.25 equation (106) - (107) for more details. - const double norm_delta_div_2 = 0.5 * norm_delta; - const double sin_delta_by_delta = - std::sin(norm_delta_div_2) / norm_delta_div_2; + const double sin_delta_by_delta = std::sin(norm_delta) / norm_delta; Eigen::Matrix y(v.size()); - y << 0.5 * sin_delta_by_delta * delta, std::cos(norm_delta_div_2); + y << sin_delta_by_delta * delta, std::cos(norm_delta); // Apply the delta update to remain on the sphere. *x_plus_delta = x.norm() * ApplyHouseholderVector(y, v, beta); @@ -98,11 +96,11 @@ inline void ComputeSphereManifoldPlusJacobian(const VT& x, // have trouble deducing the type of v automatically. ComputeHouseholderVector(x, &v, &beta); - // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the + // The Jacobian is equal to J = H.leftCols(size_ - 1) where H is the // Householder matrix (H = I - beta * v * v'). for (int i = 0; i < tangent_size; ++i) { - (*jacobian).col(i) = -0.5 * beta * v(i) * v; - (*jacobian)(i, i) += 0.5; + (*jacobian).col(i) = -beta * v(i) * v; + (*jacobian)(i, i) += 1.0; } (*jacobian) *= x.norm(); } @@ -120,12 +118,13 @@ inline void ComputeSphereManifoldMinus( const AmbientVector hy = ApplyHouseholderVector(y, v, beta) / x.norm(); // Calculate y - x. See B.2 p.25 equation (108). - double y_last = hy[tangent_size]; - double hy_norm = hy.template head(tangent_size).norm(); + const double y_last = hy[tangent_size]; + const double hy_norm = hy.template head(tangent_size).norm(); if (hy_norm == 0.0) { y_minus_x->setZero(); + y_minus_x->data()[tangent_size - 1] = y_last >= 0 ? 0.0 : M_PI; } else { - *y_minus_x = 2.0 * std::atan2(hy_norm, y_last) / hy_norm * + *y_minus_x = std::atan2(hy_norm, y_last) / hy_norm * hy.template head(tangent_size); } } @@ -146,11 +145,14 @@ inline void ComputeSphereManifoldMinusJacobian(const VT& x, // have trouble deducing the type of v automatically. ComputeHouseholderVector(x, &v, &beta); - // The Jacobian is equal to J = 2.0 * H.leftCols(size_ - 1) where H is the + // The Jacobian is equal to J = H.leftCols(size_ - 1) where H is the // Householder matrix (H = I - beta * v * v'). for (int i = 0; i < tangent_size; ++i) { - (*jacobian).row(i) = -2.0 * beta * v(i) * v; - (*jacobian)(i, i) += 2.0; + // NOTE: The transpose is used for correctness (the product is expected to + // be a row vector), although here there seems to be no difference between + // transposing or not for Eigen (possibly a compile-time auto fix). + (*jacobian).row(i) = -beta * v(i) * v.transpose(); + (*jacobian)(i, i) += 1.0; } (*jacobian) /= x.norm(); } diff --git a/include/ceres/line_manifold.h b/include/ceres/line_manifold.h index f8f1b2352..752513c59 100644 --- a/include/ceres/line_manifold.h +++ b/include/ceres/line_manifold.h @@ -156,7 +156,7 @@ bool LineManifold::Plus(const double* x_ptr, // // The direction update function Plus_d is the same as as the SphereManifold: // - // d* = H_{v(d)} [0.5 sinc(0.5 |delta_d|) delta_d, cos(0.5 |delta_d|)]^T + // d* = H_{v(d)} [sinc(|delta_d|) delta_d, cos(|delta_d|)]^T // // where H is the householder matrix // H_{v} = I - (2 / |v|^2) v v^T @@ -165,7 +165,7 @@ bool LineManifold::Plus(const double* x_ptr, // // The origin point update function Plus_o is defined as // - // o* = o + H_{v(d)} [0.5 delta_o, 0]^T. + // o* = o + H_{v(d)} [delta_o, 0]^T. Eigen::Map o(x_ptr, size_); Eigen::Map d(x_ptr + size_, size_); @@ -208,11 +208,8 @@ bool LineManifold::Plus(const double* x_ptr, // perpendicular to the line direction. This is achieved by using the // householder matrix of the direction and allow only movements // perpendicular to e_n. - // - // The factor of 0.5 is used to be consistent with the line direction - // update. AmbientVector y(size_); - y << 0.5 * delta_o, 0; + y << delta_o, 0; o_plus_delta += internal::ApplyHouseholderVector(y, v, beta); return true; @@ -266,7 +263,7 @@ bool LineManifold::Minus(const double* y_ptr, AmbientVector delta_o = y_o - x_o; const AmbientVector h_delta_o = - 2.0 * internal::ApplyHouseholderVector(delta_o, v, beta); + internal::ApplyHouseholderVector(delta_o, v, beta); y_minus_x_o = h_delta_o.template head(size_ - 1); return true; diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc index 310fd9a70..9dd80a1ad 100644 --- a/internal/ceres/manifold_test.cc +++ b/internal/ceres/manifold_test.cc @@ -703,7 +703,7 @@ TEST(SphereManifold, NearZeroTest1) { } TEST(SphereManifold, NearZeroTest2) { - Vector4d x{0.001, 0.0, 0.0, 0.0}; + Vector4d x{0.01, 0.0, 0.0, 0.0}; Vector3d delta{0.0, 1.0, 0.0}; Vector4d y = Vector4d::Zero(); SphereManifold<4> manifold; @@ -711,12 +711,49 @@ TEST(SphereManifold, NearZeroTest2) { EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance); } -TEST(SphereManifold, Plus) { +TEST(SphereManifold, Plus2DTest) { + Eigen::Vector2d x{0.0, 1.0}; + SphereManifold<2> manifold; + + { + double delta[1]{M_PI_4}; + Eigen::Vector2d y = Eigen::Vector2d::Zero(); + EXPECT_TRUE(manifold.Plus(x.data(), delta, y.data())); + const Eigen::Vector2d gtY(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0); + EXPECT_LT((y - gtY).norm(), kTolerance); + } + + { + double delta[1]{M_PI_2}; + Eigen::Vector2d y = Eigen::Vector2d::Zero(); + EXPECT_TRUE(manifold.Plus(x.data(), delta, y.data())); + const Eigen::Vector2d gtY = Eigen::Vector2d::UnitX(); + EXPECT_LT((y - gtY).norm(), kTolerance); + } + + { + double delta[1]{M_PI}; + Eigen::Vector2d y = Eigen::Vector2d::Zero(); + EXPECT_TRUE(manifold.Plus(x.data(), delta, y.data())); + const Eigen::Vector2d gtY = -Eigen::Vector2d::UnitY(); + EXPECT_LT((y - gtY).norm(), kTolerance); + } + + { + double delta[1]{2.0 * M_PI}; + Eigen::Vector2d y = Eigen::Vector2d::Zero(); + EXPECT_TRUE(manifold.Plus(x.data(), delta, y.data())); + const Eigen::Vector2d gtY = Eigen::Vector2d::UnitY(); + EXPECT_LT((y - gtY).norm(), kTolerance); + } +} + +TEST(SphereManifold, Plus3DTest) { Eigen::Vector3d x{0.0, 0.0, 1.0}; SphereManifold<3> manifold; { - Eigen::Vector2d delta{M_PI, 0.0}; + Eigen::Vector2d delta{M_PI_2, 0.0}; Eigen::Vector3d y = Eigen::Vector3d::Zero(); EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); const Eigen::Vector3d gtY = Eigen::Vector3d::UnitX(); @@ -724,7 +761,23 @@ TEST(SphereManifold, Plus) { } { - Eigen::Vector2d delta{0.0, M_PI}; + Eigen::Vector2d delta{M_PI, 0.0}; + Eigen::Vector3d y = Eigen::Vector3d::Zero(); + EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); + const Eigen::Vector3d gtY = -Eigen::Vector3d::UnitZ(); + EXPECT_LT((y - gtY).norm(), kTolerance); + } + + { + Eigen::Vector2d delta{2.0 * M_PI, 0.0}; + Eigen::Vector3d y = Eigen::Vector3d::Zero(); + EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); + const Eigen::Vector3d gtY = Eigen::Vector3d::UnitZ(); + EXPECT_LT((y - gtY).norm(), kTolerance); + } + + { + Eigen::Vector2d delta{0.0, M_PI_2}; Eigen::Vector3d y = Eigen::Vector3d::Zero(); EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); const Eigen::Vector3d gtY = Eigen::Vector3d::UnitY(); @@ -732,12 +785,78 @@ TEST(SphereManifold, Plus) { } { - Eigen::Vector2d delta = Eigen::Vector2d(1, 1).normalized() * M_PI; + Eigen::Vector2d delta{0.0, M_PI}; + Eigen::Vector3d y = Eigen::Vector3d::Zero(); + EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); + const Eigen::Vector3d gtY = -Eigen::Vector3d::UnitZ(); + EXPECT_LT((y - gtY).norm(), kTolerance); + } + + { + Eigen::Vector2d delta{0.0, 2.0 * M_PI}; + Eigen::Vector3d y = Eigen::Vector3d::Zero(); + EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); + const Eigen::Vector3d gtY = Eigen::Vector3d::UnitZ(); + EXPECT_LT((y - gtY).norm(), kTolerance); + } + + { + Eigen::Vector2d delta = Eigen::Vector2d(1, 1).normalized() * M_PI_2; Eigen::Vector3d y = Eigen::Vector3d::Zero(); EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); const Eigen::Vector3d gtY(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0, 0.0); EXPECT_LT((y - gtY).norm(), kTolerance); } + + { + Eigen::Vector2d delta = Eigen::Vector2d(1, 1).normalized() * M_PI; + Eigen::Vector3d y = Eigen::Vector3d::Zero(); + EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); + const Eigen::Vector3d gtY = -Eigen::Vector3d::UnitZ(); + EXPECT_LT((y - gtY).norm(), kTolerance); + } +} + +TEST(SphereManifold, Minus2DTest) { + Eigen::Vector2d x{1.0, 0.0}; + SphereManifold<2> manifold; + + { + double delta[1]; + const Eigen::Vector2d y(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0); + const double gtDelta{M_PI_4}; + EXPECT_TRUE(manifold.Minus(y.data(), x.data(), delta)); + EXPECT_LT(std::abs(delta[0] - gtDelta), kTolerance); + } + + { + double delta[1]; + const Eigen::Vector2d y(-1, 0); + const double gtDelta{M_PI}; + EXPECT_TRUE(manifold.Minus(y.data(), x.data(), delta)); + EXPECT_LT(std::abs(delta[0] - gtDelta), kTolerance); + } +} + +TEST(SphereManifold, Minus3DTest) { + Eigen::Vector3d x{1.0, 0.0, 0.0}; + SphereManifold<3> manifold; + + { + Eigen::Vector2d delta; + const Eigen::Vector3d y(std::sqrt(2.0) / 2.0, 0.0, std::sqrt(2.0) / 2.0); + const Eigen::Vector2d gtDelta(M_PI_4, 0.0); + EXPECT_TRUE(manifold.Minus(y.data(), x.data(), delta.data())); + EXPECT_LT((delta - gtDelta).norm(), kTolerance); + } + + { + Eigen::Vector2d delta; + const Eigen::Vector3d y(-1, 0, 0); + const Eigen::Vector2d gtDelta(0.0, M_PI); + EXPECT_TRUE(manifold.Minus(y.data(), x.data(), delta.data())); + EXPECT_LT((delta - gtDelta).norm(), kTolerance); + } } TEST(SphereManifold, DeathTests) { @@ -822,7 +941,7 @@ TEST(LineManifold, ZeroOriginPointTest3D) { TEST(LineManifold, ZeroOriginPointTest4D) { const Vector8d x = Vector8d::Unit(7); Vector6d delta; - delta << 0.0, 0.0, 0.0, 1.0, 2.0, 3.0; + delta << 0.0, 0.0, 0.0, 0.5, 1.0, 1.5; Vector8d y = Vector8d::Zero(); LineManifold<4> manifold; @@ -857,7 +976,7 @@ TEST(LineManifold, Plus) { LineManifold<3> manifold; { - Vector4d delta{0.0, 4.0, M_PI, 0.0}; + Vector4d delta{0.0, 2.0, M_PI_2, 0.0}; Vector6d y = Vector6d::Random(); EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); Vector6d gtY; @@ -866,7 +985,7 @@ TEST(LineManifold, Plus) { } { - Vector4d delta{6.0, 0.0, 0.0, M_PI}; + Vector4d delta{3.0, 0.0, 0.0, M_PI_2}; Vector6d y = Vector6d::Zero(); EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); Vector6d gtY; @@ -876,7 +995,7 @@ TEST(LineManifold, Plus) { { Vector4d delta; - delta << Vector2d(2.0, 4.0), Vector2d(1, 1).normalized() * M_PI; + delta << Vector2d(1.0, 2.0), Vector2d(1, 1).normalized() * M_PI_2; Vector6d y = Vector6d::Zero(); EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data())); Vector6d gtY;