blob: 2dca8f3e3f2d1ea8ce7e5422f2511d790f2a8653 [file] [log] [blame]
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2024 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_FIRST_ORDER_FUNCTION_H_
#define CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_
#include <memory>
#include <type_traits>
#include "absl/container/fixed_array.h"
#include "ceres/first_order_function.h"
#include "ceres/internal/eigen.h"
#include "ceres/jet.h"
#include "ceres/types.h"
namespace ceres {
// Create FirstOrderFunctions as needed by the GradientProblem
// framework, with gradients computed via automatic
// differentiation. For more information on automatic differentiation,
// see the wikipedia article at
// http://en.wikipedia.org/wiki/Automatic_differentiation
//
// To get an auto differentiated function, you must define a class
// with a templated operator() (a functor) that computes the cost
// function in terms of the template parameter T. The autodiff
// framework substitutes appropriate "jet" objects for T in order to
// compute the derivative when necessary, but this is hidden, and you
// should write the function as if T were a scalar type (e.g. a
// double-precision floating point number).
//
// The function must write the computed value in the last argument
// (the only non-const one) and return true to indicate
// success.
//
// For example, consider a scalar error e = x'y - a, where both x and y are
// two-dimensional column vector parameters, the prime sign indicates
// transposition, and a is a constant.
//
// To write an auto-differentiable FirstOrderFunction for the above model, first
// define the object
//
// class QuadraticCostFunctor {
// public:
// explicit QuadraticCostFunctor(double a) : a_(a) {}
// template <typename T>
// bool operator()(const T* const xy, T* cost) const {
// const T* const x = xy;
// const T* const y = xy + 2;
// *cost = x[0] * y[0] + x[1] * y[1] - T(a_);
// return true;
// }
//
// private:
// double a_;
// };
//
// Note that in the declaration of operator() the input parameters xy come
// first, and are passed as const pointers to arrays of T. The
// output is the last parameter.
//
// Then given this class definition, the auto differentiated FirstOrderFunction
// for it can be constructed as follows.
//
// FirstOrderFunction* function =
// new AutoDiffFirstOrderFunction<QuadraticCostFunctor, 4>(
// new QuadraticCostFunctor(1.0)));
//
// In the instantiation above, the template parameters following
// "QuadraticCostFunctor", "4", describe the functor as computing a
// 1-dimensional output from a four dimensional vector.
//
// WARNING: Since the functor will get instantiated with different types for
// T, you must convert from other numeric types to T before mixing
// computations with other variables of type T. In the example above, this is
// seen where instead of using a_ directly, a_ is wrapped with T(a_).
template <typename FirstOrderFunctor, int kNumParameters>
class AutoDiffFirstOrderFunction final : public FirstOrderFunction {
public:
// Takes ownership of functor.
explicit AutoDiffFirstOrderFunction(FirstOrderFunctor* functor)
: AutoDiffFirstOrderFunction{
std::unique_ptr<FirstOrderFunctor>{functor}} {}
explicit AutoDiffFirstOrderFunction(
std::unique_ptr<FirstOrderFunctor> functor)
: functor_(std::move(functor)) {
static_assert(kNumParameters > 0, "kNumParameters must be positive");
}
template <class... Args,
std::enable_if_t<std::is_constructible_v<FirstOrderFunctor,
Args&&...>>* = nullptr>
explicit AutoDiffFirstOrderFunction(Args&&... args)
: AutoDiffFirstOrderFunction{
std::make_unique<FirstOrderFunctor>(std::forward<Args>(args)...)} {}
bool Evaluate(const double* const parameters,
double* cost,
double* gradient) const override {
if (gradient == nullptr) {
return (*functor_)(parameters, cost);
}
using JetT = Jet<double, kNumParameters>;
absl::FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(kNumParameters);
for (int i = 0; i < kNumParameters; ++i) {
x[i].a = parameters[i];
x[i].v.setZero();
x[i].v[i] = 1.0;
}
JetT output;
output.a = kImpossibleValue;
output.v.setConstant(kImpossibleValue);
if (!(*functor_)(x.data(), &output)) {
return false;
}
*cost = output.a;
VectorRef(gradient, kNumParameters) = output.v;
return true;
}
int NumParameters() const override { return kNumParameters; }
const FirstOrderFunctor& functor() const { return *functor_; }
private:
std::unique_ptr<FirstOrderFunctor> functor_;
};
} // namespace ceres
#endif // CERES_PUBLIC_AUTODIFF_FIRST_ORDER_FUNCTION_H_