gtsam  4.0.0 gtsam
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10  * -------------------------------------------------------------------------- */
11
19 #pragma once
20
21 #include <gtsam/base/VectorSpace.h>
23 #include <gtsam/3rdparty/ceres/autodiff.h>
24
25 #include <boost/static_assert.hpp>
26 #include <boost/type_traits/is_base_of.hpp>
27
28 namespace gtsam {
29
37 template <typename FUNCTOR, int M, int N1, int N2>
39  typedef Eigen::Matrix<double, M, N1, Eigen::RowMajor> RowMajor1;
40  typedef Eigen::Matrix<double, M, N2, Eigen::RowMajor> RowMajor2;
41
42  typedef Eigen::Matrix<double, M, 1> VectorT;
43  typedef Eigen::Matrix<double, N1, 1> Vector1;
44  typedef Eigen::Matrix<double, N2, 1> Vector2;
45
46  FUNCTOR f;
47
48  public:
49  VectorT operator()(const Vector1& v1, const Vector2& v2,
50  OptionalJacobian<M, N1> H1 = boost::none,
51  OptionalJacobian<M, N2> H2 = boost::none) {
52  using ceres::internal::AutoDiff;
53
54  bool success;
55  VectorT result;
56
57  if (H1 || H2) {
58  // Get derivatives with AutoDiff
59  const double* parameters[] = {v1.data(), v2.data()};
60  double rowMajor1[M * N1], rowMajor2[M * N2]; // on the stack
61  double* jacobians[] = {rowMajor1, rowMajor2};
62  success = AutoDiff<FUNCTOR, double, N1, N2>::Differentiate(
63  f, parameters, M, result.data(), jacobians);
64
65  // Convert from row-major to columnn-major
66  // TODO: if this is a bottleneck (probably not!) fix Autodiff to be
67  // Column-Major
68  if (H1) *H1 = Eigen::Map<RowMajor1>(rowMajor1);
69  if (H2) *H2 = Eigen::Map<RowMajor2>(rowMajor2);
70
71  } else {
72  // Apply the mapping, to get result
73  success = f(v1.data(), v2.data(), result.data());
74  }
75  if (!success)
76  throw std::runtime_error(
77  "AdaptAutoDiff: function call resulted in failure");
78  return result;
79  }
80 };
81
82 } // namespace gtsam
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
The AdaptAutoDiff class uses ceres-style autodiff to adapt a ceres-style Function evaluation,...