Add AutoDiffManifold
AutoDiffManifold allows the user to define a templated
functor that implements the Plus and Minus operations
on the Manifold and will compute the Jacobians needed
to define the Manifold object using automatic differentiation.
Change-Id: Ibd073c25847389308ca1ab66e6f5fe78aae77205
diff --git a/include/ceres/autodiff_manifold.h b/include/ceres/autodiff_manifold.h
new file mode 100644
index 0000000..fb5f459
--- /dev/null
+++ b/include/ceres/autodiff_manifold.h
@@ -0,0 +1,259 @@
+// 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: sameeragarwal@google.com (Sameer Agarwal)
+
+#ifndef CERES_PUBLIC_AUTODIFF_MANIFOLD_H_
+#define CERES_PUBLIC_AUTODIFF_MANIFOLD_H_
+
+#include <memory>
+
+#include "ceres/internal/autodiff.h"
+#include "ceres/manifold.h"
+
+namespace ceres {
+
+// Create a Manifold with Jacobians computed via automatic differentiation. For
+// more information on manifolds, see include/ceres/manifold.h
+//
+// To get an auto differentiated manifold, you must define a class/struct with
+// templated Plus and Minus functions that computes
+//
+// x_plus_delta = Plus(x, delta);
+// y_minus_x = Minus(y, x);
+//
+// Where, x, y and x_plus_y are vectors on the manifold in the ambient space (so
+// they are kAmbientSize vectors) and and delta, y_minus_x are vectors in the
+// tangent space (so they are kTangentSize vectors).
+//
+// The Functor should have the signatures:
+//
+// struct Functor {
+// template <typename T>
+// bool Plus(const T* x, const T* delta, T* x_plus_delta) const;
+//
+// template <typename T>
+// bool Minus(const T* y, const T* x, T* y_minus_x) const;
+// };
+//
+// Observe that the Plus and Minus operations are templated on the parameter T.
+// The autodiff framework substitutes appropriate "Jet" objects for T in order
+// to compute the derivative when necessary. This is the same mechanism that is
+// used to compute derivatives when using AutoDiffCostFunction.
+//
+// Plus and Minus should return true if the computation is successful and false
+// otherwise, in which case the result will not be used.
+//
+// Given this Functor, the corresponding Manifold can be constructed as:
+//
+// AutoDiffManifold<Functor, kAmbientSize, kTangentSize> manifold;
+//
+// As a concrete example consider the case of Quaternions. Quaternions form a
+// three dimensional manifold embedded in R^4, i.e. they have an ambient
+// dimension of 4 and their tangent space has dimension 3. The following Functor
+// (taken from autodiff_manifold_test.cc) defines the Plus and Minus operations
+// on the Quaternion manifold:
+//
+// NOTE: The following is only used for illustration purposes. Ceres Solver
+// ships with optimized production grade QuaternionManifold implementation. See
+// manifold.h.
+//
+// This functor assumes that the quaternions are laid out as [w,x,y,z] in
+// memory, i.e. the real or scalar part is the first coordinate.
+//
+// struct QuaternionFunctor {
+// template <typename T>
+// bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
+// const T squared_norm_delta =
+// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
+//
+// T q_delta[4];
+// if (squared_norm_delta > T(0.0)) {
+// T norm_delta = sqrt(squared_norm_delta);
+// const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
+// q_delta[0] = cos(norm_delta);
+// q_delta[1] = sin_delta_by_delta * delta[0];
+// q_delta[2] = sin_delta_by_delta * delta[1];
+// q_delta[3] = sin_delta_by_delta * delta[2];
+// } else {
+// // We do not just use q_delta = [1,0,0,0] here because that is a
+// // constant and when used for automatic differentiation will
+// // lead to a zero derivative. Instead we take a first order
+// // approximation and evaluate it at zero.
+// q_delta[0] = T(1.0);
+// q_delta[1] = delta[0];
+// q_delta[2] = delta[1];
+// q_delta[3] = delta[2];
+// }
+//
+// QuaternionProduct(q_delta, x, x_plus_delta);
+// return true;
+// }
+//
+// template <typename T>
+// bool Minus(const T* y, const T* x, T* y_minus_x) const {
+// T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
+// T ambient_y_minus_x[4];
+// QuaternionProduct(y, minus_x, ambient_y_minus_x);
+// T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] +
+// ambient_y_minus_x[2] * ambient_y_minus_x[2] +
+// ambient_y_minus_x[3] * ambient_y_minus_x[3]);
+// if (u_norm > 0.0) {
+// T theta = atan2(u_norm, ambient_y_minus_x[0]);
+// y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
+// y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;
+// y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm;
+// } else {
+// // We do not use [0,0,0] here because even though the value part is
+// // a constant, the derivative part is not.
+// y_minus_x[0] = ambient_y_minus_x[1];
+// y_minus_x[1] = ambient_y_minus_x[2];
+// y_minus_x[2] = ambient_y_minus_x[3];
+// }
+// return true;
+// }
+// };
+//
+// Then given this struct, the auto differentiated Quaternion Manifold can now
+// be constructed as
+//
+// Manifold* manifold = new AutoDiffManifold<QuaternionFunctor, 4, 3>;
+
+template <typename Functor, int kAmbientSize, int kTangentSize>
+class AutoDiffManifold : public Manifold {
+ public:
+ AutoDiffManifold() : functor_(std::make_unique<Functor>()) {}
+
+ // Takes ownership of functor.
+ explicit AutoDiffManifold(Functor* functor) : functor_(functor) {}
+
+ ~AutoDiffManifold() override {}
+
+ int AmbientSize() const override { return kAmbientSize; }
+ int TangentSize() const override { return kTangentSize; }
+
+ bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const override {
+ return functor_->Plus(x, delta, x_plus_delta);
+ }
+
+ bool PlusJacobian(const double* x, double* jacobian) const override;
+
+ bool Minus(const double* y,
+ const double* x,
+ double* y_minus_x) const override {
+ return functor_->Minus(y, x, y_minus_x);
+ }
+
+ bool MinusJacobian(const double* x, double* jacobian) const override;
+
+ private:
+ std::unique_ptr<Functor> functor_;
+};
+
+namespace internal {
+
+// The following two helper structs are needed to interface the Plus and Minus
+// methods of the ManifoldFunctor with the automatic differentiation which
+// expects a Functor with operator().
+template <typename Functor>
+struct PlusWrapper {
+ PlusWrapper(const Functor& functor) : functor(functor) {}
+ template <typename T>
+ bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
+ return functor.Plus(x, delta, x_plus_delta);
+ }
+ const Functor& functor;
+};
+
+template <typename Functor>
+struct MinusWrapper {
+ MinusWrapper(const Functor& functor) : functor(functor) {}
+ template <typename T>
+ bool operator()(const T* y, const T* x, T* y_minus_x) const {
+ return functor.Minus(y, x, y_minus_x);
+ }
+ const Functor& functor;
+};
+} // namespace internal
+
+template <typename Functor, int kAmbientSize, int kTangentSize>
+bool AutoDiffManifold<Functor, kAmbientSize, kTangentSize>::PlusJacobian(
+ const double* x, double* jacobian) const {
+ double zero_delta[kTangentSize];
+ for (int i = 0; i < kTangentSize; ++i) {
+ zero_delta[i] = 0.0;
+ }
+
+ double x_plus_delta[kAmbientSize];
+ for (int i = 0; i < kAmbientSize; ++i) {
+ x_plus_delta[i] = 0.0;
+ }
+
+ const double* parameter_ptrs[2] = {x, zero_delta};
+
+ // PlusJacobian is D_2 Plus(x,0) so we only need to compute the Jacobian
+ // w.r.t. the second argument.
+ double* jacobian_ptrs[2] = {nullptr, jacobian};
+ return internal::AutoDifferentiate<
+ kAmbientSize,
+ internal::StaticParameterDims<kAmbientSize, kTangentSize>>(
+ internal::PlusWrapper(*functor_),
+ parameter_ptrs,
+ kAmbientSize,
+ x_plus_delta,
+ jacobian_ptrs);
+}
+
+template <typename Functor, int kAmbientSize, int kTangentSize>
+bool AutoDiffManifold<Functor, kAmbientSize, kTangentSize>::MinusJacobian(
+ const double* x, double* jacobian) const {
+ double y_minus_x[kTangentSize];
+ for (int i = 0; i < kTangentSize; ++i) {
+ y_minus_x[i] = 0.0;
+ }
+
+ const double* parameter_ptrs[2] = {x, x};
+
+ // MinusJacobian is D_1 Minus(x,x), so we only need to compute the Jacobian
+ // w.r.t. the first argument.
+ double* jacobian_ptrs[2] = {jacobian, nullptr};
+ return internal::AutoDifferentiate<
+ kTangentSize,
+ internal::StaticParameterDims<kAmbientSize, kAmbientSize>>(
+ internal::MinusWrapper(*functor_),
+ parameter_ptrs,
+ kAmbientSize,
+ y_minus_x,
+ jacobian_ptrs);
+}
+
+} // namespace ceres
+
+#endif // CERES_PUBLIC_AUTODIFF_MANIFOLD_H_
diff --git a/include/ceres/ceres.h b/include/ceres/ceres.h
index 86a84ab..a070c96 100644
--- a/include/ceres/ceres.h
+++ b/include/ceres/ceres.h
@@ -37,6 +37,7 @@
#include "ceres/autodiff_cost_function.h"
#include "ceres/autodiff_first_order_function.h"
#include "ceres/autodiff_local_parameterization.h"
+#include "ceres/autodiff_manifold.h"
#include "ceres/conditioned_cost_function.h"
#include "ceres/context.h"
#include "ceres/cost_function.h"
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index 3d39458..56e88cb 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt
@@ -435,6 +435,7 @@
ceres_test(autodiff_first_order_function)
ceres_test(autodiff_cost_function)
ceres_test(autodiff_local_parameterization)
+ ceres_test(autodiff_manifold)
ceres_test(block_jacobi_preconditioner)
ceres_test(block_random_access_dense_matrix)
ceres_test(block_random_access_diagonal_matrix)
diff --git a/internal/ceres/autodiff_manifold_test.cc b/internal/ceres/autodiff_manifold_test.cc
new file mode 100644
index 0000000..f204d5a
--- /dev/null
+++ b/internal/ceres/autodiff_manifold_test.cc
@@ -0,0 +1,292 @@
+// 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: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "ceres/autodiff_manifold.h"
+
+#include <cmath>
+
+#include "ceres/manifold.h"
+#include "ceres/manifold_test_utils.h"
+#include "ceres/rotation.h"
+#include "gtest/gtest.h"
+
+namespace ceres {
+namespace internal {
+
+constexpr int kNumTrials = 1000;
+constexpr double kTolerance = 1e-9;
+
+struct EuclideanFunctor {
+ template <typename T>
+ bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
+ for (int i = 0; i < 3; ++i) {
+ x_plus_delta[i] = x[i] + delta[i];
+ }
+ return true;
+ }
+
+ template <typename T>
+ bool Minus(const T* y, const T* x, T* y_minus_x) const {
+ for (int i = 0; i < 3; ++i) {
+ y_minus_x[i] = y[i] - x[i];
+ }
+ return true;
+ }
+};
+
+TEST(AutoDiffLManifoldTest, EuclideanManifold) {
+ AutoDiffManifold<EuclideanFunctor, 3, 3> manifold;
+ EXPECT_EQ(manifold.AmbientSize(), 3);
+ EXPECT_EQ(manifold.TangentSize(), 3);
+
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+ const Vector x = Vector::Random(manifold.AmbientSize());
+ const Vector y = Vector::Random(manifold.AmbientSize());
+ Vector delta = Vector::Random(manifold.TangentSize());
+ Vector x_plus_delta = Vector::Zero(manifold.AmbientSize());
+
+ manifold.Plus(x.data(), delta.data(), x_plus_delta.data());
+ EXPECT_NEAR((x_plus_delta - x - delta).norm() / (x + delta).norm(),
+ 0.0,
+ kTolerance);
+
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+ }
+}
+
+struct ScaledFunctor {
+ explicit ScaledFunctor(const double s) : s_(s) {}
+
+ template <typename T>
+ bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
+ for (int i = 0; i < 3; ++i) {
+ x_plus_delta[i] = x[i] + s_ * delta[i];
+ }
+ return true;
+ }
+
+ template <typename T>
+ bool Minus(const T* y, const T* x, T* y_minus_x) const {
+ for (int i = 0; i < 3; ++i) {
+ y_minus_x[i] = (y[i] - x[i]) / s_;
+ }
+ return true;
+ }
+
+ double s_;
+};
+
+TEST(AutoDiffManifoldTest, ScaledManifold) {
+ constexpr double kScale = 1.2342;
+ AutoDiffManifold<ScaledFunctor, 3, 3> manifold(new ScaledFunctor(kScale));
+ EXPECT_EQ(manifold.AmbientSize(), 3);
+ EXPECT_EQ(manifold.TangentSize(), 3);
+
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+ const Vector x = Vector::Random(manifold.AmbientSize());
+ const Vector y = Vector::Random(manifold.AmbientSize());
+ Vector delta = Vector::Random(manifold.TangentSize());
+ Vector x_plus_delta = Vector::Zero(manifold.AmbientSize());
+
+ manifold.Plus(x.data(), delta.data(), x_plus_delta.data());
+ EXPECT_NEAR((x_plus_delta - x - delta * kScale).norm() /
+ (x + delta * kScale).norm(),
+ 0.0,
+ kTolerance);
+
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+ }
+}
+
+// Templated functor that implements the Plus and Minus operations on the
+// Quaternion manifold.
+struct QuaternionFunctor {
+ template <typename T>
+ bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
+ const T squared_norm_delta =
+ delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
+
+ T q_delta[4];
+ if (squared_norm_delta > T(0.0)) {
+ T norm_delta = sqrt(squared_norm_delta);
+ const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
+ q_delta[0] = cos(norm_delta);
+ q_delta[1] = sin_delta_by_delta * delta[0];
+ q_delta[2] = sin_delta_by_delta * delta[1];
+ q_delta[3] = sin_delta_by_delta * delta[2];
+ } else {
+ // We do not just use q_delta = [1,0,0,0] here because that is a
+ // constant and when used for automatic differentiation will
+ // lead to a zero derivative. Instead we take a first order
+ // approximation and evaluate it at zero.
+ q_delta[0] = T(1.0);
+ q_delta[1] = delta[0];
+ q_delta[2] = delta[1];
+ q_delta[3] = delta[2];
+ }
+
+ QuaternionProduct(q_delta, x, x_plus_delta);
+ return true;
+ }
+
+ template <typename T>
+ bool Minus(const T* y, const T* x, T* y_minus_x) const {
+ T minus_x[4] = {x[0], -x[1], -x[2], -x[3]};
+ T ambient_y_minus_x[4];
+ QuaternionProduct(y, minus_x, ambient_y_minus_x);
+ T u_norm = sqrt(ambient_y_minus_x[1] * ambient_y_minus_x[1] +
+ ambient_y_minus_x[2] * ambient_y_minus_x[2] +
+ ambient_y_minus_x[3] * ambient_y_minus_x[3]);
+ if (u_norm > 0.0) {
+ T theta = atan2(u_norm, ambient_y_minus_x[0]);
+ y_minus_x[0] = theta * ambient_y_minus_x[1] / u_norm;
+ y_minus_x[1] = theta * ambient_y_minus_x[2] / u_norm;
+ y_minus_x[2] = theta * ambient_y_minus_x[3] / u_norm;
+ } else {
+ // We do not use [0,0,0] here because even though the value part is
+ // a constant, the derivative part is not.
+ y_minus_x[0] = ambient_y_minus_x[1];
+ y_minus_x[1] = ambient_y_minus_x[2];
+ y_minus_x[2] = ambient_y_minus_x[3];
+ }
+ return true;
+ }
+};
+
+TEST(AutoDiffManifoldTest, QuaternionPlusPiBy2) {
+ AutoDiffManifold<QuaternionFunctor, 4, 3> manifold;
+
+ Vector x = Vector::Zero(4);
+ x[0] = 1.0;
+
+ for (int i = 0; i < 3; ++i) {
+ Vector delta = Vector::Zero(3);
+ delta[i] = M_PI / 2;
+ Vector x_plus_delta = Vector::Zero(4);
+ EXPECT_TRUE(manifold.Plus(x.data(), delta.data(), x_plus_delta.data()));
+
+ // Expect that the element corresponding to pi/2 is +/- 1. All other
+ // elements should be zero.
+ for (int j = 0; j < 4; ++j) {
+ if (i == (j - 1)) {
+ EXPECT_LT(std::abs(x_plus_delta[j]) - 1,
+ std::numeric_limits<double>::epsilon())
+ << "\ndelta = " << delta.transpose()
+ << "\nx_plus_delta = " << x_plus_delta.transpose()
+ << "\n expected the " << j
+ << "th element of x_plus_delta to be +/- 1.";
+ } else {
+ EXPECT_LT(std::abs(x_plus_delta[j]),
+ std::numeric_limits<double>::epsilon())
+ << "\ndelta = " << delta.transpose()
+ << "\nx_plus_delta = " << x_plus_delta.transpose()
+ << "\n expected the " << j << "th element of x_plus_delta to be 0.";
+ }
+ }
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(
+ manifold, x, delta, x_plus_delta, kTolerance);
+ }
+}
+
+// Compute the expected value of Quaternion::Plus via functions in rotation.h
+// and compares it to the one computed by Quaternion::Plus.
+MATCHER_P2(QuaternionPlusIsCorrectAt, x, delta, "") {
+ // This multiplication by 2 is needed because AngleAxisToQuaternion uses
+ // |delta|/2 as the angle of rotation where as in the implementation of
+ // Quaternion for historical reasons we use |delta|.
+ const Vector two_delta = delta * 2;
+ Vector delta_q(4);
+ AngleAxisToQuaternion(two_delta.data(), delta_q.data());
+
+ Vector expected(4);
+ QuaternionProduct(delta_q.data(), x.data(), expected.data());
+ Vector actual(4);
+ EXPECT_TRUE(arg.Plus(x.data(), delta.data(), actual.data()));
+
+ const double n = (actual - expected).norm();
+ const double d = expected.norm();
+ const double diffnorm = n / d;
+ if (diffnorm > kTolerance) {
+ *result_listener << "\nx: " << x.transpose()
+ << "\ndelta: " << delta.transpose()
+ << "\nexpected: " << expected.transpose()
+ << "\nactual: " << actual.transpose()
+ << "\ndiff: " << (expected - actual).transpose()
+ << "\ndiffnorm : " << diffnorm;
+ return false;
+ }
+ return true;
+}
+
+Vector RandomQuaternion() {
+ Vector x = Vector::Random(4);
+ x.normalize();
+ return x;
+}
+
+TEST(AutoDiffManifoldTest, QuaternionGenericDelta) {
+ AutoDiffManifold<QuaternionFunctor, 4, 3> manifold;
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+ const Vector x = RandomQuaternion();
+ const Vector y = RandomQuaternion();
+ Vector delta = Vector::Random(3);
+ EXPECT_THAT(manifold, QuaternionPlusIsCorrectAt(x, delta));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+ }
+}
+
+TEST(AutoDiffManifoldTest, QuaternionSmallDelta) {
+ AutoDiffManifold<QuaternionFunctor, 4, 3> manifold;
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+ const Vector x = RandomQuaternion();
+ const Vector y = RandomQuaternion();
+ Vector delta = Vector::Random(3);
+ delta.normalize();
+ delta *= 1e-6;
+ EXPECT_THAT(manifold, QuaternionPlusIsCorrectAt(x, delta));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+ }
+}
+
+TEST(AutoDiffManifold, QuaternionDeltaJustBelowPi) {
+ AutoDiffManifold<QuaternionFunctor, 4, 3> manifold;
+ for (int trial = 0; trial < kNumTrials; ++trial) {
+ const Vector x = RandomQuaternion();
+ const Vector y = RandomQuaternion();
+ Vector delta = Vector::Random(3);
+ delta.normalize();
+ delta *= (M_PI - 1e-6);
+ EXPECT_THAT(manifold, QuaternionPlusIsCorrectAt(x, delta));
+ EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
+ }
+}
+
+} // namespace internal
+} // namespace ceres
