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