Moved AdaptAutoDiff template in its own header file
dellaert
parent
516bb4b0b1
commit
f44e6f0187
|
|
@ -0,0 +1,94 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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.apply(a1);
|
||||
Vector2 v2 = chart2.apply(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);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -17,16 +17,16 @@
|
|||
* @brief unit tests for Block Automatic Differentiation
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/nonlinear/AdaptAutoDiff.h>
|
||||
#include <gtsam_unstable/nonlinear/Expression.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam_unstable/nonlinear/Expression.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/LieScalar.h>
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ceres_autodiff.h>
|
||||
#include <gtsam_unstable/nonlinear/ceres_example.h>
|
||||
|
||||
#undef CHECK
|
||||
|
|
@ -164,75 +164,6 @@ TEST(Expression, AutoDiff2) {
|
|||
EXPECT(assert_equal(E2,H2,1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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.apply(a1);
|
||||
Vector2 v2 = chart2.apply(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);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Test AutoDiff wrapper Snavely
|
||||
TEST(Expression, AutoDiff3) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue