AdaptAutoDiff.h

Go to the documentation of this file.

/* ----------------------------------------------------------------------------
 
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
 
 * See LICENSE for the license information
 
 * -------------------------------------------------------------------------- */

 
#pragma once
 
#include <ceres/autodiff_cost_function.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/VectorSpace.h>
 
#include <stdexcept>
 
namespace gtsam {

template <typename FUNCTOR, int M, int N1, int N2>
class AdaptAutoDiff {
  typedef Eigen::Matrix<double, M, N1, Eigen::RowMajor> RowMajor1;
  typedef Eigen::Matrix<double, M, N2, Eigen::RowMajor> RowMajor2;
 
  typedef Eigen::Matrix<double, M, 1> VectorT;
  typedef Eigen::Matrix<double, N1, 1> Vector1;
  typedef Eigen::Matrix<double, N2, 1> Vector2;
 
  FUNCTOR f;
 
 public:
  VectorT operator()(const Vector1& v1, const Vector2& v2,
                     OptionalJacobian<M, N1> H1 = {},
                     OptionalJacobian<M, N2> H2 = {}) {
    bool success;
    VectorT result;
 
    if (H1 || H2) {
      // Get derivatives with AutoDiff
      const double* parameters[] = {v1.data(), v2.data()};
      double rowMajor1[M * N1] = {}, rowMajor2[M * N2] = {};  // on the stack
      double* jacobians[] = {rowMajor1, rowMajor2};
      ceres::AutoDiffCostFunction<FUNCTOR, M, N1, N2> autoDiff(
          &f, ceres::DO_NOT_TAKE_OWNERSHIP);
      success = autoDiff.Evaluate(parameters, result.data(), jacobians);
 
      // Convert from row-major to columnn-major
      // TODO: if this is a bottleneck (probably not!) fix Autodiff to be
      // Column-Major
      if (H1) *H1 = Eigen::Map<RowMajor1>(rowMajor1);
      if (H2) *H2 = Eigen::Map<RowMajor2>(rowMajor2);
 
    } else {
      // Apply the mapping, to get result
      success = f(v1.data(), v2.data(), result.data());
    }
    if (!success)
      throw std::runtime_error(
          "AdaptAutoDiff: function call resulted in failure");
    return result;
  }
};
 
}  // namespace gtsam