95 lines
2.7 KiB
C++
95 lines
2.7 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file AdaptAutoDiff.h
|
|
* @date October 22, 2014
|
|
* @author Frank Dellaert
|
|
* @brief Adaptor for Ceres style auto-differentiated functions
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam_unstable/nonlinear/ceres_autodiff.h>
|
|
#include <gtsam/base/Manifold.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/// Adapt ceres-style autodiff
|
|
template<typename F, typename T, typename A1, typename A2>
|
|
class AdaptAutoDiff {
|
|
|
|
static const int N = traits::dimension<T>::value;
|
|
static const int M1 = traits::dimension<A1>::value;
|
|
static const int M2 = traits::dimension<A2>::value;
|
|
|
|
typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
|
|
typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;
|
|
|
|
typedef Canonical<T> CanonicalT;
|
|
typedef Canonical<A1> Canonical1;
|
|
typedef Canonical<A2> Canonical2;
|
|
|
|
typedef typename CanonicalT::vector VectorT;
|
|
typedef typename Canonical1::vector Vector1;
|
|
typedef typename Canonical2::vector Vector2;
|
|
|
|
// Instantiate function and charts
|
|
CanonicalT chartT;
|
|
Canonical1 chart1;
|
|
Canonical2 chart2;
|
|
F f;
|
|
|
|
public:
|
|
|
|
typedef Eigen::Matrix<double, N, M1> JacobianTA1;
|
|
typedef Eigen::Matrix<double, N, M2> JacobianTA2;
|
|
|
|
T operator()(const A1& a1, const A2& a2, boost::optional<JacobianTA1&> H1 =
|
|
boost::none, boost::optional<JacobianTA2&> H2 = boost::none) {
|
|
|
|
using ceres::internal::AutoDiff;
|
|
|
|
// Make arguments
|
|
Vector1 v1 = chart1.local(a1);
|
|
Vector2 v2 = chart2.local(a2);
|
|
|
|
bool success;
|
|
VectorT result;
|
|
|
|
if (H1 || H2) {
|
|
|
|
// Get derivatives with AutoDiff
|
|
double *parameters[] = { v1.data(), v2.data() };
|
|
double rowMajor1[N * M1], rowMajor2[N * M2]; // om the stack
|
|
double *jacobians[] = { rowMajor1, rowMajor2 };
|
|
success = AutoDiff<F, double, 9, 3>::Differentiate(f, parameters, 2,
|
|
result.data(), jacobians);
|
|
|
|
// Convert from row-major to columnn-major
|
|
// TODO: if this is a bottleneck (probably not!) fix Autodiff to be Column-Major
|
|
*H1 = Eigen::Map<RowMajor1>(rowMajor1);
|
|
*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 chartT.retract(result);
|
|
}
|
|
|
|
};
|
|
|
|
}
|