Add LineManifold.
This MR ports the LineParameterization of manifolds. The unit test are
rewritten to use the manifold test facilities.
The LineManifold is extended so that it can also handle dynamic size
ambient space dimensions.
Change-Id: I1fe3cd34b56f74b72ca028c34f5368e9df9fe4d7
diff --git a/include/ceres/internal/line_manifold.h b/include/ceres/internal/line_manifold.h
new file mode 100644
index 0000000..aa275e2
--- /dev/null
+++ b/include/ceres/internal/line_manifold.h
@@ -0,0 +1,219 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: jodebo_beck@gmx.de (Johannes Beck)
+//
+
+#ifndef CERES_PUBLIC_INTERNAL_LINE_MANIFOLD_H_
+#define CERES_PUBLIC_INTERNAL_LINE_MANIFOLD_H_
+
+#include "ceres/internal/householder_vector.h"
+#include "ceres/internal/sphere_manifold_functions.h"
+
+namespace ceres {
+
+template <int AmbientSpaceDimension>
+LineManifold<AmbientSpaceDimension>::LineManifold()
+ : size_{AmbientSpaceDimension} {
+ static_assert(
+ AmbientSpaceDimension != Eigen::Dynamic,
+ "The size is set to dynamic. Please call the constructor with a size.");
+}
+
+template <int AmbientSpaceDimension>
+LineManifold<AmbientSpaceDimension>::LineManifold(int size) : size_{size} {
+ if (AmbientSpaceDimension != Eigen::Dynamic) {
+ CHECK_EQ(AmbientSpaceDimension, size)
+ << "Specified size by template parameter differs from the supplied "
+ "one.";
+ } else {
+ CHECK_GT(size_, 1)
+ << "The size of the manifold needs to be greater than 1.";
+ }
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::Plus(const double* x_ptr,
+ const double* delta_ptr,
+ double* x_plus_delta_ptr) const {
+ // We seek a box plus operator of the form
+ //
+ // [o*, d*] = Plus([o, d], [delta_o, delta_d])
+ //
+ // where o is the origin point, d is the direction vector, delta_o is
+ // the delta of the origin point and delta_d the delta of the direction and
+ // o* and d* is the updated origin point and direction.
+ //
+ // We separate the Plus operator into the origin point and directional part
+ // d* = Plus_d(d, delta_d)
+ // o* = Plus_o(o, d, delta_o)
+ //
+ // 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
+ //
+ // where H is the householder matrix
+ // H_{v} = I - (2 / |v|^2) v v^T
+ // and
+ // v(d) = d - sign(d_n) |d| e_n.
+ //
+ // The origin point update function Plus_o is defined as
+ //
+ // o* = o + H_{v(d)} [0.5 delta_o, 0]^T.
+
+ Eigen::Map<const AmbientVector> o(x_ptr, size_);
+ Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
+
+ Eigen::Map<const TangentVector> delta_o(delta_ptr, size_ - 1);
+ Eigen::Map<const TangentVector> delta_d(delta_ptr + size_ - 1, size_ - 1);
+ Eigen::Map<AmbientVector> o_plus_delta(x_plus_delta_ptr, size_);
+ Eigen::Map<AmbientVector> d_plus_delta(x_plus_delta_ptr + size_, size_);
+
+ const double norm_delta_d = delta_d.norm();
+
+ o_plus_delta = o;
+
+ // Shortcut for zero delta direction.
+ if (norm_delta_d == 0.0) {
+ d_plus_delta = d;
+
+ if (delta_o.isZero(0.0)) {
+ return true;
+ }
+ }
+
+ // Calculate the householder transformation which is needed for f_d and f_o.
+ AmbientVector v(size_);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // have trouble deducing the type of v automatically.
+ internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
+ double,
+ AmbientSpaceDimension>(d, &v, &beta);
+
+ if (norm_delta_d != 0.0) {
+ internal::ComputeSphereManifoldPlus(
+ v, beta, d, delta_d, norm_delta_d, &d_plus_delta);
+ }
+
+ // The null space is in the direction of the line, so the tangent space is
+ // 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;
+ o_plus_delta += internal::ApplyHouseholderVector(y, v, beta);
+
+ return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::PlusJacobian(
+ const double* x_ptr, double* jacobian_ptr) const {
+ Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
+ Eigen::Map<MatrixPlusJacobian> jacobian(
+ jacobian_ptr, 2 * size_, 2 * (size_ - 1));
+
+ // Clear the Jacobian as only half of the matrix is not zero.
+ jacobian.setZero();
+
+ auto jacobian_d =
+ jacobian
+ .template topLeftCorner<AmbientSpaceDimension, TangentSpaceDimension>(
+ size_, size_ - 1);
+ auto jacobian_o = jacobian.template bottomRightCorner<AmbientSpaceDimension,
+ TangentSpaceDimension>(
+ size_, size_ - 1);
+ internal::ComputeSphereManifoldPlusJacobian(d, &jacobian_d);
+ jacobian_o = jacobian_d;
+ return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::Minus(const double* y_ptr,
+ const double* x_ptr,
+ double* y_minus_x) const {
+ Eigen::Map<const AmbientVector> y_o(y_ptr, size_);
+ Eigen::Map<const AmbientVector> y_d(y_ptr + size_, size_);
+ Eigen::Map<const AmbientVector> x_o(x_ptr, size_);
+ Eigen::Map<const AmbientVector> x_d(x_ptr + size_, size_);
+
+ Eigen::Map<TangentVector> y_minus_x_o(y_minus_x, size_ - 1);
+ Eigen::Map<TangentVector> y_minus_x_d(y_minus_x + size_ - 1, size_ - 1);
+
+ AmbientVector v(size_);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // have trouble deducing the type of v automatically.
+ internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
+ double,
+ AmbientSpaceDimension>(x_d, &v, &beta);
+
+ internal::ComputeSphereManifoldMinus(v, beta, x_d, y_d, &y_minus_x_d);
+
+ AmbientVector delta_o = y_o - x_o;
+ const AmbientVector h_delta_o =
+ 2.0 * internal::ApplyHouseholderVector(delta_o, v, beta);
+ y_minus_x_o = h_delta_o.template head<TangentSpaceDimension>(size_ - 1);
+
+ return true;
+}
+
+template <int AmbientSpaceDimension>
+bool LineManifold<AmbientSpaceDimension>::MinusJacobian(
+ const double* x_ptr, double* jacobian_ptr) const {
+ Eigen::Map<const AmbientVector> d(x_ptr + size_, size_);
+ Eigen::Map<MatrixMinusJacobian> jacobian(
+ jacobian_ptr, 2 * (size_ - 1), 2 * size_);
+
+ // Clear the Jacobian as only half of the matrix is not zero.
+ jacobian.setZero();
+
+ auto jacobian_d =
+ jacobian
+ .template topLeftCorner<TangentSpaceDimension, AmbientSpaceDimension>(
+ size_ - 1, size_);
+ auto jacobian_o = jacobian.template bottomRightCorner<TangentSpaceDimension,
+ AmbientSpaceDimension>(
+ size_ - 1, size_);
+ internal::ComputeSphereManifoldMinusJacobian(d, &jacobian_d);
+ jacobian_o = jacobian_d;
+
+ return true;
+}
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_INTERNAL_LINE_MANIFOLD_H_
\ No newline at end of file
diff --git a/include/ceres/internal/sphere_manifold.h b/include/ceres/internal/sphere_manifold.h
index c29fbcc..1400228 100644
--- a/include/ceres/internal/sphere_manifold.h
+++ b/include/ceres/internal/sphere_manifold.h
@@ -29,7 +29,11 @@
// Author: vitus@google.com (Mike Vitus)
// jodebo_beck@gmx.de (Johannes Beck)
+#ifndef CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_H_
+#define CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_H_
+
#include "ceres/internal/householder_vector.h"
+#include "ceres/internal/sphere_manifold_functions.h"
namespace ceres {
@@ -69,16 +73,6 @@
return true;
}
- // 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;
-
- AmbientVector y(size_);
- y.head(size_ - 1) = 0.5 * sin_delta_by_delta * delta;
- y(size_ - 1) = std::cos(norm_delta_div_2);
-
AmbientVector v(size_);
double beta;
@@ -89,8 +83,8 @@
double,
AmbientSpaceDimension>(x, &v, &beta);
- // Apply the delta update to remain on the sphere.
- x_plus_delta = x.norm() * internal::ApplyHouseholderVector(y, v, beta);
+ internal::ComputeSphereManifoldPlus(
+ v, beta, x, delta, norm_delta, &x_plus_delta);
return true;
}
@@ -100,24 +94,7 @@
const double* x_ptr, double* jacobian_ptr) const {
Eigen::Map<const AmbientVector> x(x_ptr, size_);
Eigen::Map<MatrixPlusJacobian> jacobian(jacobian_ptr, size_, size_ - 1);
-
- AmbientVector v(size_);
- double beta;
-
- // NOTE: The explicit template arguments are needed here because
- // ComputeHouseholderVector is templated and some versions of MSVC
- // have trouble deducing the type of v automatically.
- internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
- double,
- AmbientSpaceDimension>(x, &v, &beta);
-
- // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the
- // Householder matrix (H = I - beta * v * v').
- for (int i = 0; i < size_ - 1; ++i) {
- jacobian.col(i) = -0.5 * beta * v(i) * v;
- jacobian.col(i)(i) += 0.5;
- }
- jacobian *= x.norm();
+ internal::ComputeSphereManifoldPlusJacobian(x, &jacobian);
return true;
}
@@ -140,20 +117,7 @@
internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
double,
AmbientSpaceDimension>(x, &v, &beta);
-
- const AmbientVector hy =
- internal::ApplyHouseholderVector(y, v, beta) / x.norm();
-
- // Calculate y - x. See B.2 p.25 equation (108).
- double y_last = hy[size_ - 1];
- double hy_norm = hy.head(size_ - 1).norm();
- if (hy_norm == 0.0) {
- y_minus_x.setZero();
- } else {
- y_minus_x =
- 2.0 * std::atan2(hy_norm, y_last) / hy_norm * hy.head(size_ - 1);
- }
-
+ internal::ComputeSphereManifoldMinus(v, beta, x, y, &y_minus_x);
return true;
}
@@ -163,24 +127,9 @@
Eigen::Map<const AmbientVector> x(x_ptr, size_);
Eigen::Map<MatrixMinusJacobian> jacobian(jacobian_ptr, size_ - 1, size_);
- AmbientVector v(size_);
- double beta;
-
- // NOTE: The explicit template arguments are needed here because
- // ComputeHouseholderVector is templated and some versions of MSVC
- // have trouble deducing the type of v automatically.
- internal::ComputeHouseholderVector<Eigen::Map<const AmbientVector>,
- double,
- AmbientSpaceDimension>(x, &v, &beta);
-
- // The Jacobian is equal to J = 2.0 * H.leftCols(size_ - 1) where H is the
- // Householder matrix (H = I - beta * v * v').
- for (int i = 0; i < size_ - 1; ++i) {
- jacobian.row(i) = -2.0 * beta * v(i) * v;
- jacobian.row(i)(i) += 2.0;
- }
- jacobian /= x.norm();
-
+ internal::ComputeSphereManifoldMinusJacobian(x, &jacobian);
return true;
}
-} // namespace ceres
\ No newline at end of file
+} // namespace ceres
+
+#endif
diff --git a/include/ceres/internal/sphere_manifold_functions.h b/include/ceres/internal/sphere_manifold_functions.h
new file mode 100644
index 0000000..5be3321
--- /dev/null
+++ b/include/ceres/internal/sphere_manifold_functions.h
@@ -0,0 +1,162 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2022 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: vitus@google.com (Mike Vitus)
+// jodebo_beck@gmx.de (Johannes Beck)
+
+#ifndef CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_HELPERS_H_
+#define CERES_PUBLIC_INTERNAL_SPHERE_MANIFOLD_HELPERS_H_
+
+#include "ceres/internal/householder_vector.h"
+
+// This module contains functions to compute the SphereManifold plus and minus
+// operator and their Jacobians.
+//
+// As the parameters to these functions are shared between them, they are
+// described here: The following variable names are used:
+// Plus(x, delta) = x + delta = x_plus_delta,
+// Minus(y, x) = y - x = y_minus_x.
+//
+// The remaining ones are v and beta which describe the Householder
+// transformation of x, and norm_delta which is the norm of delta.
+//
+// The types of x, y, x_plus_delta and y_minus_x need to be equivalent to
+// Eigen::Matrix<double, AmbientSpaceDimension, 1> and the type of delta needs
+// to be equivalent to Eigen::Matrix<double, TangentSpaceDimension, 1>.
+//
+// The type of Jacobian plus needs to be equivalent to Eigen::Matrix<double,
+// AmbientSpaceDimension, TangentSpaceDimension, Eigen::RowMajor> and for
+// Jacobian minus Eigen::Matrix<double, TangentSpaceDimension,
+// AmbientSpaceDimension, Eigen::RowMajor>.
+//
+// For all vector / matrix inputs and outputs, template parameters are
+// used in order to allow also Eigen::Ref and Eigen block expressions to
+// be passed to the function.
+
+namespace ceres {
+namespace internal {
+
+template <typename VT, typename XT, typename DeltaT, typename XPlusDeltaT>
+inline void ComputeSphereManifoldPlus(const VT& v,
+ double beta,
+ const XT& x,
+ const DeltaT& delta,
+ 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;
+
+ Eigen::Matrix<double, AmbientDim, 1> y(v.size());
+ y << 0.5 * sin_delta_by_delta * delta, std::cos(norm_delta_div_2);
+
+ // Apply the delta update to remain on the sphere.
+ *x_plus_delta = x.norm() * ApplyHouseholderVector(y, v, beta);
+}
+
+template <typename VT, typename JacobianT>
+inline void ComputeSphereManifoldPlusJacobian(const VT& x,
+ JacobianT* jacobian) {
+ constexpr int AmbientSpaceDim = VT::RowsAtCompileTime;
+ using AmbientVector = Eigen::Matrix<double, AmbientSpaceDim, 1>;
+ const int ambient_size = x.size();
+ const int tangent_size = x.size() - 1;
+
+ AmbientVector v(ambient_size);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // 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
+ // 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) *= x.norm();
+}
+
+template <typename VT, typename XT, typename YT, typename YMinusXT>
+inline void ComputeSphereManifoldMinus(
+ const VT& v, double beta, const XT& x, const YT& y, YMinusXT* y_minus_x) {
+ constexpr int AmbientSpaceDim = VT::RowsAtCompileTime;
+ constexpr int TangentSpaceDim =
+ AmbientSpaceDim == Eigen::Dynamic ? Eigen::Dynamic : AmbientSpaceDim - 1;
+ using AmbientVector = Eigen::Matrix<double, AmbientSpaceDim, 1>;
+
+ const int tanget_size = v.size() - 1;
+
+ const AmbientVector hy = ApplyHouseholderVector(y, v, beta) / x.norm();
+
+ // Calculate y - x. See B.2 p.25 equation (108).
+ double y_last = hy[tanget_size];
+ double hy_norm = hy.template head<TangentSpaceDim>(tanget_size).norm();
+ if (hy_norm == 0.0) {
+ y_minus_x->setZero();
+ } else {
+ *y_minus_x = 2.0 * std::atan2(hy_norm, y_last) / hy_norm *
+ hy.template head<TangentSpaceDim>(tanget_size);
+ }
+}
+
+template <typename VT, typename JacobianT>
+inline void ComputeSphereManifoldMinusJacobian(const VT& x,
+ JacobianT* jacobian) {
+ constexpr int AmbientSpaceDim = VT::RowsAtCompileTime;
+ using AmbientVector = Eigen::Matrix<double, AmbientSpaceDim, 1>;
+ const int ambient_size = x.size();
+ const int tangent_size = x.size() - 1;
+
+ AmbientVector v(ambient_size);
+ double beta;
+
+ // NOTE: The explicit template arguments are needed here because
+ // ComputeHouseholderVector is templated and some versions of MSVC
+ // 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
+ // 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;
+ }
+ (*jacobian) /= x.norm();
+}
+
+} // namespace internal
+} // namespace ceres
+
+#endif
diff --git a/include/ceres/manifold.h b/include/ceres/manifold.h
index 178d01b..dcf9750 100644
--- a/include/ceres/manifold.h
+++ b/include/ceres/manifold.h
@@ -423,10 +423,13 @@
//
// The class works with dynamic and static ambient space dimensions. If the
// ambient space dimensions is know at compile time use
+//
// SphereManifold<3> manifold;
+//
// If the ambient space dimensions is not known at compile time the template
// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs to
// be provided as a constructor argument:
+//
// SphereManifold<ceres::DYNAMIC> manifold(ambient_dim);
//
// See section B.2 (p.25) in "Integrating Generic Sensor Fusion Algorithms with
@@ -479,8 +482,80 @@
const int size_{};
};
+// This provides a manifold for lines, where the line is
+// over-parameterized by an origin point and a direction vector. So the
+// parameter vector size needs to be two times the ambient space dimension,
+// where the first half is interpreted as the origin point and the second half
+// as the direction.
+//
+// The plus operator for the line direction is the same as for the
+// SphereManifold. The update of the origin point is
+// perpendicular to the line direction before the update.
+//
+// This manifold is a special case of the affine Grassmannian
+// manifold (see https://en.wikipedia.org/wiki/Affine_Grassmannian_(manifold))
+// for the case Graff_1(R^n).
+//
+// The class works with dynamic and static ambient space dimensions. If the
+// ambient space dimensions is know at compile time use
+//
+// LineManifold<3> manifold;
+//
+// If the ambient space dimensions is not known at compile time the template
+// parameter needs to be set to ceres::DYNAMIC and the actual dimension needs to
+// be provided as a constructor argument:
+//
+// LineManifold<ceres::DYNAMIC> manifold(ambient_dim);
+//
+template <int AmbientSpaceDimension>
+class LineManifold : public Manifold {
+ public:
+ static_assert(AmbientSpaceDimension == DYNAMIC || AmbientSpaceDimension >= 2,
+ "The ambient space must be at least 2.");
+ static_assert(DYNAMIC == Eigen::Dynamic,
+ "ceres::DYNAMIC needs to be the same as Eigen::Dynamic.");
+
+ LineManifold();
+ explicit LineManifold(int size);
+
+ int AmbientSize() const override { return 2 * size_; }
+ int TangentSize() const override { return 2 * (size_ - 1); }
+ bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const override;
+ bool PlusJacobian(const double* x, double* jacobian) const override;
+ bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const override;
+ bool MinusJacobian(const double* x, double* jacobian) const override;
+
+ private:
+ static constexpr int IsDynamic = (AmbientSpaceDimension == Eigen::Dynamic);
+ static constexpr int TangentSpaceDimension =
+ IsDynamic ? Eigen::Dynamic : AmbientSpaceDimension - 1;
+
+ static constexpr int DAmbientSpaceDimension =
+ IsDynamic ? Eigen::Dynamic : 2 * AmbientSpaceDimension;
+ static constexpr int DTangentSpaceDimension =
+ IsDynamic ? Eigen::Dynamic : 2 * TangentSpaceDimension;
+
+ using AmbientVector = Eigen::Matrix<double, AmbientSpaceDimension, 1>;
+ using TangentVector = Eigen::Matrix<double, TangentSpaceDimension, 1>;
+ using MatrixPlusJacobian = Eigen::Matrix<double,
+ DAmbientSpaceDimension,
+ DTangentSpaceDimension,
+ Eigen::RowMajor>;
+ using MatrixMinusJacobian = Eigen::Matrix<double,
+ DTangentSpaceDimension,
+ DAmbientSpaceDimension,
+ Eigen::RowMajor>;
+
+ const int size_{AmbientSpaceDimension};
+};
+
} // namespace ceres
+#include "internal/line_manifold.h"
#include "internal/sphere_manifold.h"
// clang-format off
diff --git a/internal/ceres/manifold_test.cc b/internal/ceres/manifold_test.cc
index 89e0ec7..24c7b43 100644
--- a/internal/ceres/manifold_test.cc
+++ b/internal/ceres/manifold_test.cc
@@ -537,7 +537,7 @@
}
TEST(SphereManifold, NormalFunctionTestDynamic) {
- SphereManifold<Eigen::Dynamic> manifold(5);
+ SphereManifold<ceres::DYNAMIC> manifold(5);
EXPECT_EQ(manifold.AmbientSize(), 5);
EXPECT_EQ(manifold.TangentSize(), 4);
@@ -558,5 +558,170 @@
}
}
+TEST(LineManifold, ZeroTest3D) {
+ using Vec4 = Eigen::Matrix<double, 4, 1>;
+ using Vec6 = Eigen::Matrix<double, 6, 1>;
+
+ const Vec6 x = Vec6::Unit(5);
+ const Vec4 delta = Vec4::Zero();
+ Vec6 y = Vec6::Zero();
+
+ LineManifold<3> manifold;
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+}
+
+TEST(LineManifold, ZeroTest4D) {
+ using Vec6 = Eigen::Matrix<double, 6, 1>;
+ using Vec8 = Eigen::Matrix<double, 8, 1>;
+
+ const Vec8 x = Vec8::Unit(7);
+ const Vec6 delta = Vec6::Zero();
+ Vec8 y = Vec8::Zero();
+
+ LineManifold<4> manifold;
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+}
+
+TEST(LineManifold, ZeroOriginPointTest3D) {
+ using Vec4 = Eigen::Matrix<double, 4, 1>;
+ using Vec6 = Eigen::Matrix<double, 6, 1>;
+
+ const Vec6 x = Vec6::Unit(5);
+ Vec4 delta;
+ delta << 0.0, 0.0, 1.0, 2.0;
+ Vec6 y = Vec6::Zero();
+
+ LineManifold<3> manifold;
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+}
+
+TEST(LineManifold, ZeroOriginPointTest4D) {
+ using Vec6 = Eigen::Matrix<double, 6, 1>;
+ using Vec8 = Eigen::Matrix<double, 8, 1>;
+
+ const Vec8 x = Vec8::Unit(7);
+ Vec6 delta;
+ delta << 0.0, 0.0, 0.0, 1.0, 2.0, 3.0;
+ Vec8 y = Vec8::Zero();
+
+ LineManifold<4> manifold;
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+}
+
+TEST(LineManifold, ZeroDirTest3D) {
+ using Vec4 = Eigen::Matrix<double, 4, 1>;
+ using Vec6 = Eigen::Matrix<double, 6, 1>;
+
+ Vec6 x = Vec6::Unit(5);
+ Vec4 delta;
+ delta << 3.0, 2.0, 0.0, 0.0;
+ Vec6 y = Vec6::Zero();
+
+ LineManifold<3> manifold;
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+}
+
+TEST(LineManifold, ZeroDirTest4D) {
+ using Vec6 = Eigen::Matrix<double, 6, 1>;
+ using Vec8 = Eigen::Matrix<double, 8, 1>;
+
+ Vec8 x = Vec8::Unit(7);
+ Vec6 delta;
+ delta << 3.0, 2.0, 1.0, 0.0, 0.0, 0.0;
+ Vec8 y = Vec8::Zero();
+
+ LineManifold<4> manifold;
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+}
+
+TEST(LineManifold, Plus) {
+ using Vec2 = Eigen::Matrix<double, 2, 1>;
+ using Vec3 = Eigen::Matrix<double, 3, 1>;
+ using Vec4 = Eigen::Matrix<double, 4, 1>;
+ using Vec6 = Eigen::Matrix<double, 6, 1>;
+
+ Vec6 x = Vec6::Unit(5);
+ LineManifold<3> manifold;
+
+ {
+ Vec4 delta{0.0, 4.0, M_PI, 0.0};
+ Vec6 y = Vec6::Random();
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
+ Vec6 gtY;
+ gtY << 2.0 * Vec3::UnitY(), Vec3::UnitX();
+ EXPECT_LT((y - gtY).norm(), kTolerance);
+ }
+
+ {
+ Vec4 delta{6.0, 0.0, 0.0, M_PI};
+ Vec6 y = Vec6::Zero();
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
+ Vec6 gtY;
+ gtY << 3.0 * Vec3::UnitX(), Vec3::UnitY();
+ EXPECT_LT((y - gtY).norm(), kTolerance);
+ }
+
+ {
+ Vec4 delta;
+ delta << Vec2(2.0, 4.0), Vec2(1, 1).normalized() * M_PI;
+ Vec6 y = Vec6::Zero();
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), y.data()));
+ Vec6 gtY;
+ gtY << Vec3(1.0, 2.0, 0.0),
+ Vec3(std::sqrt(2.0) / 2.0, std::sqrt(2.0) / 2.0, 0.0);
+ EXPECT_LT((y - gtY).norm(), kTolerance);
+ }
+}
+
+TEST(LineManifold, NormalFunctionTest) {
+ LineManifold<3> manifold;
+ EXPECT_EQ(manifold.AmbientSize(), 6);
+ EXPECT_EQ(manifold.TangentSize(), 4);
+
+ Vector zero_tangent = Vector::Zero(manifold.TangentSize());
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+ Vector x = Vector::Random(manifold.AmbientSize());
+ Vector y = Vector::Random(manifold.AmbientSize());
+ Vector delta = Vector::Random(manifold.TangentSize());
+
+ if (x.tail<3>().norm() == 0.0) {
+ continue;
+ }
+
+ x.tail<3>().normalize();
+ manifold.Plus(x.data(), delta.data(), y.data());
+
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+ }
+}
+
+TEST(LineManifold, NormalFunctionTestDynamic) {
+ LineManifold<ceres::DYNAMIC> manifold(3);
+ EXPECT_EQ(manifold.AmbientSize(), 6);
+ EXPECT_EQ(manifold.TangentSize(), 4);
+
+ Vector zero_tangent = Vector::Zero(manifold.TangentSize());
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+ Vector x = Vector::Random(manifold.AmbientSize());
+ Vector y = Vector::Random(manifold.AmbientSize());
+ Vector delta = Vector::Random(manifold.TangentSize());
+
+ if (x.tail<3>().norm() == 0.0) {
+ continue;
+ }
+
+ x.tail<3>().normalize();
+ manifold.Plus(x.data(), delta.data(), y.data());
+
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+ }
+}
+
} // namespace internal
} // namespace ceres
