auto_differentiation.h

Go to the documentation of this file.

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// 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.
//
// 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
// HOLDER 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.
 
#ifndef G2O_AUTO_DIFFERENTIATION_H
#define G2O_AUTO_DIFFERENTIATION_H
 
#include <algorithm>
#include <cassert>
#include <type_traits>
 
#include "eigen_types.h"
#include "g2o/autodiff/autodiff.h"
#include "g2o/stuff/misc.h"
#include "g2o_core_api.h"
 
namespace g2o {
 
template <typename Edge>
struct EstimateAccessor {
  template <int k>
  EIGEN_STRONG_INLINE double* data(Edge* that) {
    return const_cast<double*>(that->template vertexXn<k>()->estimate().data());
  }
};
 
template <typename Edge>
class EstimateAccessorGet {
 public:
  template <int k>
  EIGEN_STRONG_INLINE double* data(Edge* that) {
    auto& buffer = std::get<k>(_estimateBuffer);
    buffer.resize(that->template vertexDimension<k>());
    double* rawBuffer = const_cast<double*>(buffer.data());
    bool gotData = that->template vertexXn<k>()->getEstimateData(rawBuffer);
    assert(gotData && "Called getEstimateData, but seems unimplmented");
    return gotData ? rawBuffer : nullptr;
  }
 
 protected:
  template <typename>
  struct BufferType;
  template <std::size_t... Ints>
  struct BufferType<std::index_sequence<Ints...>> {
    using type =
        std::tuple<VectorN<Edge::template VertexXnType<Ints>::Dimension>...>;
  };
 
  using Buffer = typename BufferType<
      std::make_index_sequence<Edge::_nr_of_vertices>>::type;
  Buffer _estimateBuffer;
};
 
template <typename Edge, typename EstimateAccess = EstimateAccessor<Edge>>
class AutoDifferentiation {
 public:
  template <int EdgeDimension, int VertexDimension>
  using ADJacobianType =
      typename Eigen::Matrix<double, EdgeDimension, VertexDimension,
                             Eigen::RowMajor>;
 
  static void computeError(Edge* that) {
    static_assert(Edge::Dimension > 0,
                  "Dynamically sized edges are not supported");
    computeErrorNs(that, std::make_index_sequence<Edge::_nr_of_vertices>());
  }
 
  static void linearize(Edge* that) {
    static_assert(Edge::Dimension > 0,
                  "Dynamically sized edges are not supported");
    linearizeOplusNs(that, std::make_index_sequence<Edge::_nr_of_vertices>());
  }
 
 protected:
  template <std::size_t... Ints>
  static void computeErrorNs(Edge* that, std::index_sequence<Ints...>) {
    static_assert(
        std::min({Edge::template VertexXnType<Ints>::Dimension...}) > 0,
        "Dynamically sized vertices are not supported");
    EstimateAccess estimateAccess;
    (*that)(estimateAccess.template data<Ints>(that)..., that->errorData());
  }
 
  template <std::size_t... Ints>
  static void linearizeOplusNs(Edge* that, std::index_sequence<Ints...>) {
    static_assert(
        std::min({Edge::template VertexXnType<Ints>::Dimension...}) > 0,
        "Dynamically sized vertices are not supported");
    // all vertices are fixed, no need to compute anything here
    if (that->allVerticesFixed()) {
      (void(that->template jacobianOplusXn<Ints>().setZero()), ...);
      return;
    }
 
    // tuple containing the Jacobians
    std::tuple<ADJacobianType<Edge::Dimension,
                              Edge::template VertexXnType<Ints>::Dimension>...>
        ad_jacobians;
 
    // setting up the pointer to the parameters and the Jacobians for calling
    // AD.
    EstimateAccess estimateAccess;
    double* parameters[] = {estimateAccess.template data<Ints>(that)...};
    // double* parameters[] = { /* trivial case would be */
    //     const_cast<double*>(that->template
    //     vertexXn<Ints>()->estimate().data())...};
 
    // pointers to the Jacobians, set to NULL if vertex is fixed to skip
    // computation
    double* jacobians[] = {
        that->template vertexXn<Ints>()->fixed()
            ? nullptr
            : const_cast<double*>(std::get<Ints>(ad_jacobians).data())...};
    // Calls the automatic differentiation for evaluation of the Jacobians.
    double errorValue[Edge::Dimension];
    using AutoDiffDims = ceres::internal::StaticParameterDims<
        Edge::template VertexXnType<Ints>::Dimension...>;
    bool diffState =
        ceres::internal::AutoDifferentiate<Edge::Dimension, AutoDiffDims, Edge,
                                           double>(
            *that, parameters, Edge::Dimension, errorValue, jacobians);
 
    assert(diffState && "Error during Automatic Differentiation");
    if (!diffState) {  // something went wrong during AD
      (void(std::get<Ints>(ad_jacobians).setZero()), ...);
      return;
    }
    // copy over the Jacobians (convert row-major -> column-major) for non-fixed
    // vertices
    (void(that->template vertexXn<Ints>()->fixed()
              ? (that->template jacobianOplusXn<Ints>().setZero(), 0)
              : (assign(that->template jacobianOplusXn<Ints>(),
                        std::get<Ints>(ad_jacobians)),
                 0)),
     ...);
  }
 
  template <typename A, typename B>
  static EIGEN_STRONG_INLINE void assign(const Eigen::MatrixBase<A>& a,
                                         const Eigen::MatrixBase<B>& b) {
    Eigen::MatrixBase<A>& aux = const_cast<Eigen::MatrixBase<A>&>(a);
    aux = b;
  }
};
 
}  // namespace g2o
 
// helper macros for fine-grained integration into own types
#define G2O_MAKE_AUTO_AD_COMPUTEERROR                                      \
  void computeError() override {                                           \
    g2o::AutoDifferentiation<                                              \
        std::remove_reference<decltype(*this)>::type>::computeError(this); \
  }
#define G2O_MAKE_AUTO_AD_LINEARIZEOPLUS                                 \
  void linearizeOplus() override {                                      \
    g2o::AutoDifferentiation<                                           \
        std::remove_reference<decltype(*this)>::type>::linearize(this); \
  }
 
#define G2O_MAKE_AUTO_AD_FUNCTIONS \
  G2O_MAKE_AUTO_AD_COMPUTEERROR    \
  G2O_MAKE_AUTO_AD_LINEARIZEOPLUS
 
// helper macros for fine-grained integration into own types using
// EstimateAccessorGet
#define G2O_MAKE_AUTO_AD_COMPUTEERROR_BY_GET                               \
  void computeError() override {                                           \
    using EdgeType = std::remove_reference<decltype(*this)>::type;         \
    g2o::AutoDifferentiation<                                              \
        EdgeType, g2o::EstimateAccessorGet<EdgeType>>::computeError(this); \
  }
 
#define G2O_MAKE_AUTO_AD_LINEARIZEOPLUS_BY_GET                          \
  void linearizeOplus() override {                                      \
    using EdgeType = std::remove_reference<decltype(*this)>::type;      \
    g2o::AutoDifferentiation<                                           \
        EdgeType, g2o::EstimateAccessorGet<EdgeType>>::linearize(this); \
  }
 
#define G2O_MAKE_AUTO_AD_FUNCTIONS_BY_GET \
  G2O_MAKE_AUTO_AD_COMPUTEERROR_BY_GET    \
  G2O_MAKE_AUTO_AD_LINEARIZEOPLUS_BY_GET
 
#endif