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
diff --git a/include/ceres/internal/sphere_manifold_functions.h b/include/ceres/internal/sphere_manifold_functions.h
index f9af5ec..ab37605 100644
--- a/include/ceres/internal/sphere_manifold_functions.h
+++ b/include/ceres/internal/sphere_manifold_functions.h
@@ -65,18 +65,16 @@
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<double, AmbientDim, 1> 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 @@
// have trouble deducing the type of v automatically.
ComputeHouseholderVector<VT, double, AmbientSpaceDim>(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 @@
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<TangentSpaceDim>(tangent_size).norm();
+ const double y_last = hy[tangent_size];
+ const double hy_norm = hy.template head<TangentSpaceDim>(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<TangentSpaceDim>(tangent_size);
}
}
@@ -146,11 +145,14 @@
// have trouble deducing the type of v automatically.
ComputeHouseholderVector<VT, double, AmbientSpaceDim>(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 f8f1b23..752513c 100644
--- a/include/ceres/line_manifold.h
+++ b/include/ceres/line_manifold.h
@@ -156,7 +156,7 @@
//
// 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 @@
//
// 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<const AmbientVector> o(x_ptr, size_);
Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
@@ -208,11 +208,8 @@
// 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 @@
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<TangentSpaceDimension>(size_ - 1);
return true;
diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc
index 310fd9a..9dd80a1 100644
--- a/internal/ceres/manifold_test.cc
+++ b/internal/ceres/manifold_test.cc
@@ -703,7 +703,7 @@
}
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 @@
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 @@
}
{
- 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 @@
}
{
- 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, 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 @@
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 @@
}
{
- 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 @@
{
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;
