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;