gtsam  4.1.0
gtsam
AdaptAutoDiff.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
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
gtsam::AdaptAutoDiff
The AdaptAutoDiff class uses ceres-style autodiff to adapt a ceres-style Function evaluation,...
Definition: AdaptAutoDiff.h:38
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition: OptionalJacobian.h:39
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
OptionalJacobian.h
Special class for optional Jacobian arguments.