gtsam 4.1.1 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
28namespace gtsam {
29
37template <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
Special class for optional Jacobian arguments.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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,...