From f44e6f0187ca756c4b98e3f09e3361a4f7e0a96c Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 22 Oct 2014 11:41:04 +0200 Subject: [PATCH] Moved AdaptAutoDiff template in its own header file --- gtsam_unstable/nonlinear/AdaptAutoDiff.h | 94 +++++++++++++++++++ .../nonlinear/tests/testAdaptAutoDiff.cpp | 73 +------------- 2 files changed, 96 insertions(+), 71 deletions(-) create mode 100644 gtsam_unstable/nonlinear/AdaptAutoDiff.h diff --git a/gtsam_unstable/nonlinear/AdaptAutoDiff.h b/gtsam_unstable/nonlinear/AdaptAutoDiff.h new file mode 100644 index 000000000..fc1f98064 --- /dev/null +++ b/gtsam_unstable/nonlinear/AdaptAutoDiff.h @@ -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 +#include + +namespace gtsam { + +/// Adapt ceres-style autodiff +template +class AdaptAutoDiff { + + static const int N = traits::dimension::value; + static const int M1 = traits::dimension::value; + static const int M2 = traits::dimension::value; + + typedef Eigen::Matrix RowMajor1; + typedef Eigen::Matrix RowMajor2; + + typedef Canonical CanonicalT; + typedef Canonical Canonical1; + typedef Canonical 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 JacobianTA1; + typedef Eigen::Matrix JacobianTA2; + + T operator()(const A1& a1, const A2& a2, boost::optional H1 = + boost::none, boost::optional 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::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); + *H2 = Eigen::Map(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); + } + +}; + +} diff --git a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp index eb5245f35..2d2408a74 100644 --- a/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam_unstable/nonlinear/tests/testAdaptAutoDiff.cpp @@ -17,16 +17,16 @@ * @brief unit tests for Block Automatic Differentiation */ +#include +#include #include #include #include #include -#include #include #include #include -#include #include #undef CHECK @@ -164,75 +164,6 @@ TEST(Expression, AutoDiff2) { EXPECT(assert_equal(E2,H2,1e-8)); } -/* ************************************************************************* */ -// Adapt ceres-style autodiff -template -class AdaptAutoDiff { - - static const int N = traits::dimension::value; - static const int M1 = traits::dimension::value; - static const int M2 = traits::dimension::value; - - typedef Eigen::Matrix RowMajor1; - typedef Eigen::Matrix RowMajor2; - - typedef Canonical CanonicalT; - typedef Canonical Canonical1; - typedef Canonical 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 JacobianTA1; - typedef Eigen::Matrix JacobianTA2; - - T operator()(const A1& a1, const A2& a2, boost::optional H1 = - boost::none, boost::optional 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::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); - *H2 = Eigen::Map(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) {