diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 592022fa..6396469d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -245,6 +245,46 @@ jobs: working-directory: ${{runner.workspace}}/build run: make test + cppad: + needs: [build-ubuntu] + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Setup + run: | + sudo apt update + sudo apt install -y libeigen3-dev + mkdir ${{runner.workspace}}/build + # Install cppad + # Get cppad from source + # as we need + # github.com/coin-or/CppAD/commit/a1d19b7fd484637b7352f3d0369bc50242f8e267 + - name: Checkout cppad + uses: actions/checkout@v2 + with: + repository: coin-or/CppAD + ref: master + path: 'cppad' + - name: Setup cppad + run: mkdir ${{runner.workspace}}/build_cppad + - name: Configure CMake cppad + working-directory: ${{runner.workspace}}/build_cppad + run: cmake $GITHUB_WORKSPACE/cppad -Dinclude_adolc=OFF -Dinclude_eigen=OFF -Dinclude_ipopt=OFF -Dinclude_cppadcg=OFF + - name: Install cppad + working-directory: ${{runner.workspace}}/build_cppad + run: sudo cmake --build . --target install + # Build/test manif cppad + - name: Configure CMake + working-directory: ${{runner.workspace}}/build + run: cmake $GITHUB_WORKSPACE -DBUILD_TESTING=ON + - name: Build + working-directory: ${{runner.workspace}}/build + run: make -j2 + - name: Test + working-directory: ${{runner.workspace}}/build + run: make test + pybind11-pip: needs: [build-ubuntu, build-mac] strategy: diff --git a/README.md b/README.md index 79c7f7f1..dd007011 100644 --- a/README.md +++ b/README.md @@ -133,8 +133,9 @@ local perturbation on the tangent space, many non-linear solvers (e.g. [Ceres][ceres]) expect functions to be differentiated with respect to the underlying representation vector of the group element (e.g. with respect to quaternion vector for `SO3`). -For this reason, **manif** is compliant with the auto-differentiation libraries -[`ceres::Jet`][ceres-jet] and [`autodiff::Dual`][autodiff]. +For this reason, **manif** is compliant with the common auto-differentiation types +[`ceres::Jet`][ceres-jet], +[`autodiff::Dual`][autodiff] and [`CppAD::AD`][cppad] (forward/reverse). ## Documentation @@ -180,6 +181,8 @@ Want to contribute? Great! Check out our [contribution guidelines](CONTRIBUTING. [ceres]: http://ceres-solver.org/ [ceres-jet]: http://ceres-solver.org/automatic_derivatives.html#dual-numbers-jets [autodiff]: https://autodiff.github.io/ +[cppad]: https://coin-or.github.io/CppAD/doc/cppad.htm + [crtp]: https://en.wikipedia.org/wiki/Curiously_recurring_template_pattern [manif-repo]: https://github.com/artivis/manif.git diff --git a/cmake/modules/FindCppAD.cmake b/cmake/modules/FindCppAD.cmake new file mode 100644 index 00000000..0229ea6b --- /dev/null +++ b/cmake/modules/FindCppAD.cmake @@ -0,0 +1,10 @@ +# Try to find CppAD +# Once done this will define +# +# CPPAD_FOUND - system has CppAD lib with correct version +# CPPAD_INCLUDE_DIR - the CppAD include directory + +find_path(CPPAD_INCLUDE_DIR NAMES cppad/cppad.hpp) +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(CppAD DEFAULT_MSG CPPAD_INCLUDE_DIR) +mark_as_advanced(CPPAD_INCLUDE_DIR) \ No newline at end of file diff --git a/docs/pages/cpp/cppad.md b/docs/pages/cpp/cppad.md new file mode 100644 index 00000000..34baa6f7 --- /dev/null +++ b/docs/pages/cpp/cppad.md @@ -0,0 +1,157 @@ +# CppAD + +- [CppAD](#cppad) + - [Jacobians](#jacobians) + +The **manif** package differentiates Jacobians with respect to a +local perturbation on the tangent space. +These Jacobians map tangent spaces, as described in [this paper][jsola18]. + +However, many non-linear solvers +(e.g. [Ceres][ceres]) expect functions to be differentiated with respect to the +underlying representation vector of the group element +(e.g. with respect to quaternion vector for `SO3`). + +For this reason **manif** is compliant with the +[`CppAD::AD`][cppad] (forward/reverse) auto-differentiation type. + +For reference of the size of the Jacobians returned when using [`CppAD::AD`][cppad], +**manif** implements rotations in the following way: + +- SO(2) and SE(2): as a complex number with `real = cos(theta)` and `imag = sin(theta)` values. +- SO(3), SE(3) and SE_2(3): as a unit quaternion, using the underlying `Eigen::Quaternion` type. + +Therefore, the respective Jacobian sizes using [`autodiff::dual`][autodiff] are as follows: + +- ℝ(n) : size n +- SO(2) : size 2 +- SO(3) : size 4 +- SE(2) : size 4 +- SE(3) : size 7 +- SE_2(3): size 10 + +## Jacobians + +Considering, + +![x][latex2] a group element (e.g. S3), +![omega][latex3] the vector tangent to the group at ![x][latex4], +![f(x)][latex5] an error function, + +one is interested in expressing the Taylor series of the error function, + +![f(x(+)omega)][latex6] + +Therefore we have to compute the Jacobian + +![J_e_omega][latex7] + +the **Jacobian of** ![f(x)][latex8] **with respect to a perturbation on the tangent space**, +so that the state update happens on the manifold tangent space. + +In some optimization frameworks, the computation of this Jacobian is decoupled +in two folds as explained hereafter. + +Using the **CppAD** library, +evaluating a function and its Jacobians may be, + +```cpp +using Scalar = double; +using Ad = CppAD::AD; +using AdFun = CppAD::ADFun; +using VectorXs = Eigen::Matrix; +using VectorXad = Eigen::Matrix; + +using LieGroup = manif::SE3; +using Tangent = typename LieGroup::Tangent; + +// The variable block +VectorXad variables(LieGroup::RepSize + LieGroup::RepSize); +VectorXad variables_out(Tangent::RepSize); + +... // Some initialization and such + +// Map to manipulate variables as manif objects +Eigen::Map xi(variables.data()), xj(variables.data()+LieGroup::RepSize); +Eigen::Map e(variables_out.data()); + +// declare independent variables and start taping +CppAD::Independent(variables); + +// Call manif ominus +e = xi - xj; + +// create f: xi, xj -> e and stop taping +AdFun ad_fun(variables, variables_out); + +// Evaluate the Jacobians +VectorXs jacobians = ad_fun.Jacobian(variables.template cast()); + +// Map the Jacobian as a matrice +Eigen::Map< + Eigen::Matrix +> J_e_xixj(jacobians.data()); + +// With Jacobians as blocks +// +// J_e_xi = J_e_xixj.block(0, 0, LieGroup::DoF, LieGroup::RepSize) +// J_e_xj = J_e_xixj.block(0, LieGroup::RepSize, LieGroup::DoF, LieGroup::RepSize) +``` + +It produces Jacobians of the form, + +![J_e_x(+)omega][latex10] + +We thus then need to compute the Jacobian that will map to the tangent space - +often called local-parameterization. +A convenience function is provided in **manif** to do so as follow: + +```cpp +Eigen::MatrixXd J_xi_lp = cppadLocalParameterizationJacobian(xi); +Eigen::MatrixXd J_xj_lp = cppadLocalParameterizationJacobian(xj); +``` + +This function computes the ![x(+)omega][latex11] operation's +Jacobian evaluated for ![omega=0][latex13] thus providing the Jacobian, + +![J_x(+)w_w][latex14] + +Once both the cost function and local-parameterization's Jacobians are evaluated, +they can be compose as, + +![J_e_w = J_e_x(+)omega * J_x(+)w_w][latex15] + +Voila. + +The intermediate Jacobians (2-3) that some solver requires are **not** available in `manif` +since the library provides directly the final Jacobian `(1)`. + +However, **manif** is compliant with [`CppAD::AD`][cppad] +auto-differentiation type to compute (2-3). + +[//]: # (URLs) + +[jsola18]: http://arxiv.org/abs/1812.01537 + +[ceres]: http://ceres-solver.org/ +[ceres-costfunction]: http://ceres-solver.org/nnls_modeling.html#costfunction +[ceres-localparam]: http://ceres-solver.org/nnls_modeling.html#localparameterization +[ceres-jet]: http://ceres-solver.org/automatic_derivatives.html#dual-numbers-jets +[cppad]: https://coin-or.github.io/CppAD/doc/cppad.htm + +[latex1]: https://latex.codecogs.com/svg.latex?SO^3 +[latex2]: https://latex.codecogs.com/svg.latex?\bf&space;x +[latex3]: https://latex.codecogs.com/svg.latex?\omega +[latex4]: https://latex.codecogs.com/svg.latex?\bf&space;x +[latex5]: https://latex.codecogs.com/svg.latex?{\bf&space;e}=f({\bf&space;x}) +[latex6]: https://latex.codecogs.com/svg.latex?f({\bf&space;x\oplus\omega})\approx{\bf&space;e}+{\bf&space;J}_{\omega}^{e}~\omega&space;. +[latex7]: https://latex.codecogs.com/svg.latex?{\bf&space;J}_{\omega}^{e}=\frac{\delta{\bf&space;e}}{\delta{\bf&space;x}}=\frac{\delta&space;f({\bf&space;x})}{\delta{\bf&space;x}}=\lim_{\omega\to0}\frac{f({\bf&space;x}\oplus\omega)\ominus&space;f({\bf&space;x})}{\omega},&space;(1) +[latex8]: https://latex.codecogs.com/svg.latex?f({\bf&space;x}) +[latex9]: https://latex.codecogs.com/svg.latex?{\bf&space;e}=f({\bf&space;x}) +[latex10]: https://latex.codecogs.com/svg.latex?{\bf&space;J}_{{\bf&space;x}\oplus\omega}^{e}=\frac{\delta{\bf&space;e}}{\delta({\bf&space;x}\oplus\omega)}=\lim_{\mathbf&space;h\to0}\frac{&space;f({\bf&space;x}+\mathbf&space;h)-f({\bf&space;x})}{\mathbf&space;h}.&space;(2) +[latex11]: https://latex.codecogs.com/svg.latex?{\bf&space;x}\oplus\mathbf\omega +[latex12]: https://latex.codecogs.com/svg.latex?\mathbf\omega +[latex13]: https://latex.codecogs.com/svg.latex?\omega=0 +[latex14]: https://latex.codecogs.com/svg.latex?{\bf&space;J}_{\omega}^{{\bf&space;x}\oplus\omega}=\frac{\delta({\bf&space;x}\oplus\omega)}{\delta\omega}=\lim_{\delta\omega\to0}\frac{{\bf&space;x}\oplus(\omega+\delta\omega)-{\bf&space;x}\oplus\mathbf\omega}{\delta\omega}=\lim_{\delta\omega\to0}\frac{{\bf&space;x}\oplus\delta\omega-{\bf&space;x}}{\delta\omega}.&space;(3) +[latex15]: https://latex.codecogs.com/svg.latex?{\bf&space;J}_{\omega}^{e}={\bf&space;J}_{{\bf&space;x}\oplus\omega}^{e}\times{\bf&space;J}_{\omega}^{{\bf&space;x}\oplus\omega}.&space;(4) +[latex16]: https://latex.codecogs.com/svg.latex?{\bf&space;x}\oplus\mathbf\omega diff --git a/include/manif/SE2.h b/include/manif/SE2.h index c0195140..8c72a385 100644 --- a/include/manif/SE2.h +++ b/include/manif/SE2.h @@ -4,6 +4,7 @@ #include "manif/impl/macro.h" #include "manif/impl/lie_group_base.h" #include "manif/impl/tangent_base.h" +#include "manif/impl/conditional_op.h" #include "manif/impl/se2/SE2_properties.h" #include "manif/impl/se2/SE2_base.h" diff --git a/include/manif/SE3.h b/include/manif/SE3.h index 0910462c..9b604324 100644 --- a/include/manif/SE3.h +++ b/include/manif/SE3.h @@ -4,6 +4,7 @@ #include "manif/impl/macro.h" #include "manif/impl/lie_group_base.h" #include "manif/impl/tangent_base.h" +#include "manif/impl/conditional_op.h" #include "manif/impl/se3/SE3_properties.h" #include "manif/impl/se3/SE3_base.h" diff --git a/include/manif/SE_2_3.h b/include/manif/SE_2_3.h index 8cdf7d35..69336555 100644 --- a/include/manif/SE_2_3.h +++ b/include/manif/SE_2_3.h @@ -4,6 +4,7 @@ #include "manif/impl/macro.h" #include "manif/impl/lie_group_base.h" #include "manif/impl/tangent_base.h" +#include "manif/impl/conditional_op.h" #include "manif/impl/se_2_3/SE_2_3_properties.h" #include "manif/impl/se_2_3/SE_2_3_base.h" diff --git a/include/manif/SO3.h b/include/manif/SO3.h index 6e2100fb..41b8a33d 100644 --- a/include/manif/SO3.h +++ b/include/manif/SO3.h @@ -4,6 +4,7 @@ #include "manif/impl/macro.h" #include "manif/impl/lie_group_base.h" #include "manif/impl/tangent_base.h" +#include "manif/impl/conditional_op.h" #include "manif/impl/so3/SO3_properties.h" #include "manif/impl/so3/SO3_base.h" diff --git a/include/manif/constants.h b/include/manif/constants.h index 6381a68c..6e4db84e 100644 --- a/include/manif/constants.h +++ b/include/manif/constants.h @@ -73,6 +73,11 @@ struct Constants static constexpr float to_deg = float(180.0 / MANIF_PI); }; +constexpr float Constants::eps; +constexpr float Constants::eps_sqrt; +constexpr float Constants::to_rad; +constexpr float Constants::to_deg; + } /* namespace manif */ #endif /* _MANIF_MANIF_CONSTANTS_H_ */ diff --git a/include/manif/cppad/conditional_op.h b/include/manif/cppad/conditional_op.h new file mode 100644 index 00000000..beac2cde --- /dev/null +++ b/include/manif/cppad/conditional_op.h @@ -0,0 +1,50 @@ +#ifndef _MANIF_MANIF_CPPAD_CONDITIONAL_OP_H_ +#define _MANIF_MANIF_CPPAD_CONDITIONAL_OP_H_ + +namespace manif { +namespace internal { + +template +struct CondOpLtHelper> { + using Scalar = CppAD::AD<_Base>; + + static Scalar eval( + const Scalar& lhs, const Scalar& rhs, const Scalar& vt, const Scalar& vf + ) { + return CppAD::CondExpLt(lhs, rhs, vt, vf); + } +}; + +template +struct CondOpGtHelper> { + using Scalar = CppAD::AD<_Base>; + + static Scalar eval( + const Scalar& lhs, const Scalar& rhs, const Scalar& vt, const Scalar& vf + ) { + return CppAD::CondExpGt(lhs, rhs, vt, vf); + } + + template + static Eigen::Matrix eval( + const Scalar& lhs, + const Scalar& rhs, + const Eigen::MatrixBase& vt, + const Eigen::MatrixBase& vf + ) { + using Scalar = typename Derived::Scalar; + static constexpr unsigned int Size = Derived::Rows; + Eigen::Matrix ret; + + for (int i=0; i +struct Constants> { + static const CppAD::AD eps; +}; + +const CppAD::AD +Constants>::eps = CppAD::AD(1e-6); + +/// @brief Specialize Constants traits for the double-based CppAD::AD type +template <> +struct Constants> { + static const CppAD::AD eps; +}; + +const CppAD::AD +Constants>::eps = CppAD::AD(1e-14); + +} // namespace manif + +#endif // _MANIF_MANIF_CPPAD_CONSTANTS_H_ \ No newline at end of file diff --git a/include/manif/cppad/cppad.h b/include/manif/cppad/cppad.h new file mode 100644 index 00000000..cdc8bedb --- /dev/null +++ b/include/manif/cppad/cppad.h @@ -0,0 +1,18 @@ +#ifndef _MANIF_MANIF_CPPAD_CPPAD_H_ +#define _MANIF_MANIF_CPPAD_CPPAD_H_ + +#include + +#include "manif/cppad/constants.h" +#include "manif/cppad/eigen.h" +#include "manif/cppad/local_parameterization.h" +#include "manif/cppad/conditional_op.h" + +namespace manif { +namespace internal { +template +struct is_ad> : std::integral_constant { }; +} // namespace internal +} // namespace manif + +#endif // _MANIF_MANIF_CPPAD_CPPAD_H_ \ No newline at end of file diff --git a/include/manif/cppad/eigen.h b/include/manif/cppad/eigen.h new file mode 100644 index 00000000..37ecc5a5 --- /dev/null +++ b/include/manif/cppad/eigen.h @@ -0,0 +1,28 @@ +#ifndef _MANIF_MANIF_CPPAD_EIGEN_H_ +#define _MANIF_MANIF_CPPAD_EIGEN_H_ + +namespace Eigen { +namespace internal { + +/// @note Eigen cast specialization +/// enables Eigen::MatrixXd = MatrixXad.cast(); +template +struct cast_impl, T> { + EIGEN_DEVICE_FUNC static inline T run(const CppAD::AD& x) { + return CppAD::Value(x); + } +}; + +/// @note Eigen cast specialization to prevents nesting AD +/// e.g. CppAD::AD> +template +struct cast_impl, CppAD::AD> { + EIGEN_DEVICE_FUNC static inline T run(const CppAD::AD& x) { + return x; + } +}; + +} // namespace internal +} // namespace Eigen + +#endif // _MANIF_MANIF_CPPAD_EIGEN_H_ \ No newline at end of file diff --git a/include/manif/cppad/local_parameterization.h b/include/manif/cppad/local_parameterization.h new file mode 100644 index 00000000..2a227110 --- /dev/null +++ b/include/manif/cppad/local_parameterization.h @@ -0,0 +1,78 @@ +#ifndef _MANIF_MANIF_CPPAD_LOCAL_PARAMETRIZATION_H_ +#define _MANIF_MANIF_CPPAD_LOCAL_PARAMETRIZATION_H_ + +namespace manif { + +enum class AutoDifferentiation : char {Aut, For, Rev}; + +/** + * @brief Compute the local parameterization Jacobian + * using CppAD auto differentiation. + * + * @tparam Derived + * @param _state + * @param m + * @return Jacobian + */ +template +Eigen::Matrix +cppadLocalParameterizationJacobian( + const manif::LieGroupBase& _state, + const AutoDifferentiation m = AutoDifferentiation::Aut +) { + using Scalar = typename Derived::Scalar; + using Ad = CppAD::AD; + + using LieGroup = typename Derived::template LieGroupTemplate; + using Tangent = typename Derived::Tangent::template TangentTemplate; + + using Jacobian = Eigen::Matrix< + Scalar, + Derived::RepSize, + Derived::DoF, + (Derived::DoF>1)? Eigen::RowMajor : Eigen::ColMajor + >; + + using MatrixXad = Eigen::Matrix; + + const LieGroup state = _state.coeffs().template cast(); + MatrixXad delta = MatrixXad::Zero(Derived::DoF); + + MatrixXad state_plus_delta(Derived::RepSize); + + CppAD::Independent(delta); + + Eigen::Map(state_plus_delta.data()) = + state + Eigen::Map(delta.data()); + + CppAD::ADFun ad_fun(delta, state_plus_delta); + + MANIF_ASSERT(state.coeffs().isApprox(state_plus_delta)); + + Eigen::Matrix jac; + + switch (m) { + case AutoDifferentiation::Aut: + jac = ad_fun.Jacobian(delta.template cast().eval()); + break; + case AutoDifferentiation::For: + jac.resize(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, delta.template cast().eval(), jac); + break; + case AutoDifferentiation::Rev: + jac.resize(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, delta.template cast().eval(), jac); + break; + default: + MANIF_THROW("Unknown auto differentiation mode."); + break; + } + + MANIF_ASSERT(LieGroup::RepSize * LieGroup::DoF == jac.size()); + + return Eigen::Map(jac.data()); +} + +} // namespace manif + +#endif // _MANIF_MANIF_CPPAD_LOCAL_PARAMETRIZATION_H_ diff --git a/include/manif/impl/conditional_op.h b/include/manif/impl/conditional_op.h new file mode 100644 index 00000000..b9ae5b37 --- /dev/null +++ b/include/manif/impl/conditional_op.h @@ -0,0 +1,81 @@ +#ifndef _MANIF_MANIF_IMPL_CONDITIONAL_OP_H_ +#define _MANIF_MANIF_IMPL_CONDITIONAL_OP_H_ + +namespace manif { +namespace internal { + +/// @brief Lower than conditional trait class +template +struct CondOpLtHelper { + static _Scalar eval( + const _Scalar lhs, const _Scalar rhs, const _Scalar& vt, const _Scalar& vf + ) { + return (lhs < rhs) ? vt : vf; + } + + template + static ResultType eval( + const _Scalar lhs, const _Scalar rhs, const ResultType& vt, const ResultType& vf + ) { + return (lhs < rhs) ? vt : vf; + } +}; + +/// @brief Greater than conditional trait class +template +struct CondOpGtHelper { + static _Scalar eval( + const _Scalar lhs, const _Scalar rhs, const _Scalar& vt, const _Scalar& vf + ) { + return (lhs > rhs) ? vt : vf; + } + + template + static ResultType eval( + const _Scalar lhs, const _Scalar rhs, const ResultType& vt, const ResultType& vf + ) { + return (lhs > rhs) ? vt : vf; + } +}; + +} // namespace internal + +/** + * @brief Lower than conditional helper function + * @note (lhs < rhs)? true_value : false_value + */ +template +auto if_lt(_Scalar&& a, Args&&... args) +-> decltype( + internal::CondOpLtHelper<_Scalar>::eval( + std::forward<_Scalar>(a), std::forward(args)... + ) + ) +{ + return internal::CondOpLtHelper<_Scalar>::eval( + std::forward<_Scalar>(a), std::forward(args)... + ); +} + +/** + * @brief Greater than conditional helper function + * @note (lhs > rhs)? true_value : false_value + */ +template +auto if_gt(_Scalar&& a, Args&&... args) +-> decltype( + internal::CondOpGtHelper<_Scalar>::eval( + std::forward<_Scalar>(a), std::forward(args)... + ) + ) +{ + return internal::CondOpGtHelper<_Scalar>::eval( + std::forward<_Scalar>(a), std::forward(args)... + ); +} + +// @todo? if_le / if_ge / if_eq + +} // namespace manif + +#endif // _MANIF_MANIF_IMPL_CONDITIONAL_OP_H_ \ No newline at end of file diff --git a/include/manif/impl/se2/SE2Tangent_base.h b/include/manif/impl/se2/SE2Tangent_base.h index 50149db0..6b95d837 100644 --- a/include/manif/impl/se2/SE2Tangent_base.h +++ b/include/manif/impl/se2/SE2Tangent_base.h @@ -128,21 +128,21 @@ SE2TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const const Scalar sin_theta = sin(theta); const Scalar theta_sq = theta * theta; - Scalar A, // sin_theta_by_theta - B; // one_minus_cos_theta_by_theta - - if (theta_sq < Constants::eps) - { - // Taylor approximation - A = Scalar(1) - Scalar(1. / 6.) * theta_sq; - B = Scalar(.5) * theta - Scalar(1. / 24.) * theta * theta_sq; - } - else - { - // Euler - A = sin_theta / theta; - B = (Scalar(1) - cos_theta) / theta; - } + // sin_theta_by_theta + const Scalar A = if_lt( + theta_sq, + Constants::eps, + Scalar(1) - Scalar(1. / 6.) * theta_sq, // Taylor approximation + sin_theta / theta // Euler + ); + + // one_minus_cos_theta_by_theta + const Scalar B = if_lt( + theta_sq, + Constants::eps, + Scalar(.5) * theta - Scalar(1. / 24.) * theta * theta_sq, // Taylor approximation + (Scalar(1) - cos_theta) / theta // Euler + ); if (J_m_t) { diff --git a/include/manif/impl/se2/SE2_base.h b/include/manif/impl/se2/SE2_base.h index fbd56e58..475fd216 100644 --- a/include/manif/impl/se2/SE2_base.h +++ b/include/manif/impl/se2/SE2_base.h @@ -220,25 +220,25 @@ SE2Base<_Derived>::log(OptJacobianRef J_t_m) const using std::sin; const Scalar theta = angle(); - const Scalar cos_theta = coeffs()[2]; - const Scalar sin_theta = coeffs()[3]; + const Scalar cos_theta = real(); + const Scalar sin_theta = imag(); const Scalar theta_sq = theta * theta; - Scalar A, // sin_theta_by_theta - B; // one_minus_cos_theta_by_theta - - if (theta_sq < Constants::eps) - { - // Taylor approximation - A = Scalar(1) - Scalar(1. / 6.) * theta_sq; - B = Scalar(.5) * theta - Scalar(1. / 24.) * theta * theta_sq; - } - else - { - // Euler - A = sin_theta / theta; - B = (Scalar(1) - cos_theta) / theta; - } + // sin_theta_by_theta + Scalar A = if_lt( + theta_sq, + Constants::eps, + Scalar(1) - Scalar(1. / 6.) * theta_sq, // Taylor approximation + sin_theta / theta // Euler + ); + + // one_minus_cos_theta_by_theta + Scalar B = if_lt( + theta_sq, + Constants::eps, + Scalar(.5) * theta - Scalar(1. / 24.) * theta * theta_sq, // Taylor approximation + (Scalar(1) - cos_theta) / theta // Euler + ); const Scalar den = Scalar(1) / (A*A + B*B); diff --git a/include/manif/impl/so3/SO3Tangent_base.h b/include/manif/impl/so3/SO3Tangent_base.h index c14939d1..a780bd98 100644 --- a/include/manif/impl/so3/SO3Tangent_base.h +++ b/include/manif/impl/so3/SO3Tangent_base.h @@ -124,31 +124,31 @@ SO3TangentBase<_Derived>::exp(OptJacobianRef J_m_t) const const DataType& theta_vec = coeffs(); const Scalar theta_sq = theta_vec.squaredNorm(); + const Scalar theta = sqrt(theta_sq); - if (theta_sq > Constants::eps) + if (J_m_t) { - const Scalar theta = sqrt(theta_sq); - if (J_m_t) - { - const LieAlg W = hat(); + const LieAlg W = hat(); + + J_m_t->setIdentity(); - J_m_t->setIdentity(); + if (theta_sq > Constants::eps) + { J_m_t->noalias() -= (Scalar(1.0) - cos(theta)) / theta_sq * W; J_m_t->noalias() += (theta - sin(theta)) / (theta_sq * theta) * W * W; } - - return LieGroup( Eigen::AngleAxis(theta, theta_vec.normalized()) ); - } - else - { - if (J_m_t) + else { - J_m_t->setIdentity(); - J_m_t->noalias() -= Scalar(0.5) * hat(); + J_m_t->noalias() -= Scalar(0.5) * W; } - - return LieGroup(x()/Scalar(2), y()/Scalar(2), z()/Scalar(2), Scalar(1)); } + + return LieGroup(if_gt( + theta_sq, + Constants::eps, + typename LieGroup::DataType(Eigen::Quaternion(Eigen::AngleAxis(theta, theta_vec/theta)).coeffs()), + typename LieGroup::DataType(x()/Scalar(2), y()/Scalar(2), z()/Scalar(2), Scalar(1)) + )); } template diff --git a/include/manif/impl/so3/SO3_base.h b/include/manif/impl/so3/SO3_base.h index 303c22c9..85a2ecbd 100644 --- a/include/manif/impl/so3/SO3_base.h +++ b/include/manif/impl/so3/SO3_base.h @@ -187,52 +187,38 @@ SO3Base<_Derived>::log(OptJacobianRef J_t_m) const using std::sqrt; using std::atan2; - Tangent tan; - Scalar log_coeff; - const Scalar sin_angle_squared = coeffs().template head<3>().squaredNorm(); - if (sin_angle_squared > Constants::eps) - { - const Scalar sin_angle = sqrt(sin_angle_squared); - const Scalar cos_angle = w(); - - /** @note If (cos_angle < 0) then angle >= pi/2 , - * means : angle for angle_axis vector >= pi (== 2*angle) - * |-> results in correct rotation but not a normalized angle_axis vector - * - * In that case we observe that 2 * angle ~ 2 * angle - 2 * pi, - * which is equivalent saying - * - * angle - pi = atan(sin(angle - pi), cos(angle - pi)) - * = atan(-sin(angle), -cos(angle)) - */ - const Scalar two_angle = Scalar(2.0) * ((cos_angle < Scalar(0.0)) ? - Scalar(atan2(-sin_angle, -cos_angle)) : - Scalar(atan2( sin_angle, cos_angle))); - - log_coeff = two_angle / sin_angle; - } - else - { - // small-angle approximation - log_coeff = Scalar(2.0); - } - tan = Tangent(coeffs().template head<3>() * log_coeff); - -// using std::atan2; -// Scalar n = coeffs().template head<3>().norm(); -// Scalar angle(0); -// typename Tangent::DataType axis(1,0,0); -// if (n::eps) -// n = coeffs().template head<3>().stableNorm(); -// if (n > Scalar(0)) -// { -// angle = Scalar(2)*atan2(n, w()); -// axis = coeffs().template head<3>() / n; -// } - -// tan = Tangent(axis*angle); + Scalar log_coeff = if_gt( + sin_angle_squared, + Constants::eps, + [=]() -> Scalar { // specify ret type to force autodiff to eval expr + const Scalar sin_angle = sqrt(sin_angle_squared); + const Scalar cos_angle = w(); + + /** @note If (cos_angle < 0) then angle >= pi/2 , + * means : angle for angle_axis vector >= pi (== 2*angle) + * |-> results in correct rotation but not a normalized angle_axis vector + * + * In that case we observe that 2 * angle ~ 2 * angle - 2 * pi, + * which is equivalent saying + * + * angle - pi = atan(sin(angle - pi), cos(angle - pi)) + * = atan(-sin(angle), -cos(angle)) + */ + const Scalar two_angle = Scalar(2.0) * if_lt( + cos_angle, + Scalar(0), + Scalar(atan2(-sin_angle, -cos_angle)), + Scalar(atan2( sin_angle, cos_angle)) + ); + + return two_angle / sin_angle; + }(), + Scalar(2.0) + ); + + Tangent tan(coeffs().template head<3>() * log_coeff); if (J_t_m) { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0598f421..961b86f2 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.8) +cmake_minimum_required(VERSION 3.5.1) find_package(GTest QUIET) @@ -95,6 +95,22 @@ else() message(STATUS "Could not find autodiff, autodiff tests will not be built.") endif() +find_package(CppAD QUIET) + +if(NOT CPPAD_FOUND) + list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake/modules) + find_package(CppAD QUIET) +endif() + +if(CPPAD_FOUND) + + message(STATUS "Found CppAD, building CppAD tests.") + add_subdirectory(cppad) + +else() + message(STATUS "Could not find CppAD, CppAD tests will not be built.") +endif() + # Set required C++11 flag set_property(TARGET ${CXX_11_TEST_TARGETS} PROPERTY CXX_STANDARD 11) set_property(TARGET ${CXX_11_TEST_TARGETS} PROPERTY CXX_STANDARD_REQUIRED ON) diff --git a/test/cppad/CMakeLists.txt b/test/cppad/CMakeLists.txt new file mode 100644 index 00000000..2505a64e --- /dev/null +++ b/test/cppad/CMakeLists.txt @@ -0,0 +1,18 @@ +# cppad tests + +manif_add_gtest(gtest_cppad gtest_cppad.cpp) + +set(CXX_11_TEST_TARGETS_CPPAD + gtest_cppad +) + +foreach(target ${CXX_11_TEST_TARGETS_CPPAD}) + target_include_directories(${target} SYSTEM PRIVATE ${CPPAD_INCLUDE_DIR}) +endforeach() + +set(CXX_11_TEST_TARGETS + ${CXX_11_TEST_TARGETS} + ${CXX_11_TEST_TARGETS_CPPAD} + + PARENT_SCOPE +) diff --git a/test/cppad/gtest_common_tester_cppad.h b/test/cppad/gtest_common_tester_cppad.h new file mode 100644 index 00000000..3bd89f0c --- /dev/null +++ b/test/cppad/gtest_common_tester_cppad.h @@ -0,0 +1,1337 @@ +#ifndef _MANIF_MANIF_TEST_CPPAD_COMMON_TESTER_CPPAD_H_ +#define _MANIF_MANIF_TEST_CPPAD_COMMON_TESTER_CPPAD_H_ + +#include "../gtest_manif_utils.h" + +#define CPPAD_REVERSE 1 + +#define MANIF_TEST_CPPAD(manifold) \ + using TEST_##manifold##_JACOBIANS_CPPAD_TESTER = CommonTesterCppAD; \ + INSTANTIATE_TEST_CASE_P( \ + TEST_##manifold##_JACOBIANS_CPPAD_TESTS, \ + TEST_##manifold##_JACOBIANS_CPPAD_TESTER, \ + ::testing::Values( \ + std::make_tuple( \ + manifold::Identity(), \ + manifold::Identity(), \ + manifold::Tangent::Zero(), \ + manifold::Tangent::Zero() \ + ), \ + std::make_tuple( \ + (manifold::Tangent::Random()*1e-8).exp(), \ + (manifold::Tangent::Random()*1e-8).exp(), \ + manifold::Tangent::Random()*1e-8, \ + manifold::Tangent::Random()*1e-8 \ + ), \ + std::make_tuple( \ + manifold::Random(), \ + manifold::Random(), \ + manifold::Tangent::Random(), \ + manifold::Tangent::Random() \ + ) \ + )); \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_RMINUS_JACOBIAN) \ + { evalRminusJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_LMINUS_JACOBIAN) \ + { evalLminusJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_RPLUS_JACOBIAN) \ + { evalRplusJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_LPLUS_JACOBIAN) \ + { evalLplusJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_LOG_JACOBIAN) \ + { evalLogJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_EXP_JACOBIAN) \ + { evalExpJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_INVERSE_JACOBIAN) \ + { evalInverseJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_COMPOSE_JACOBIAN) \ + { evalComposeJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_BETWEEN_JACOBIAN) \ + { evalBetweenJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_ACT_JACOBIAN) \ + { evalActJac(); } \ + TEST_P(TEST_##manifold##_JACOBIANS_CPPAD_TESTER, TEST_##manifold##_CPPAD_TAPE_JACOBIAN) \ + { evalTape(); } + +#define __MANIF_MAKE_TEST_CPPAD(manifold, type) \ + using manifold##type = manifold; \ + MANIF_TEST_CPPAD(manifold##type) + +#define __MANIF_MAKE_TEST_CPPAD_ALL_TYPES(manifold) \ + __MANIF_MAKE_TEST_CPPAD(manifold, double) + + // float-based test are a little too flaky ~[1e-2, 1e-6] + // __MANIF_MAKE_TEST_CPPAD(manifold, float) + +#define MANIF_TEST_CPPAD_ALL \ + using namespace manif; \ + __MANIF_MAKE_TEST_CPPAD_ALL_TYPES(R2) \ + __MANIF_MAKE_TEST_CPPAD_ALL_TYPES(R5) \ + __MANIF_MAKE_TEST_CPPAD_ALL_TYPES(SO2) \ + __MANIF_MAKE_TEST_CPPAD_ALL_TYPES(SE2) \ + __MANIF_MAKE_TEST_CPPAD_ALL_TYPES(SO3) \ + __MANIF_MAKE_TEST_CPPAD_ALL_TYPES(SE3) \ + __MANIF_MAKE_TEST_CPPAD_ALL_TYPES(SE_2_3) + + +template +class CppADLocalReParameterizationFunctor { + using LieGroup = _LieGroup; + using Scalar = typename _LieGroup::Scalar; + using Tangent = typename _LieGroup::Tangent; + + template + using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>; + + template + using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>; + + using Ad = CppAD::AD; + + using Vs = Eigen::Matrix; + +public: + + enum class Mode : char {Aut, For, Rev}; + + using MatrixXad = Eigen::Matrix; + using Jacobian = Eigen::Matrix< + Scalar, LieGroup::DoF, LieGroup::RepSize, Eigen::RowMajor + >; + + CppADLocalReParameterizationFunctor() = default; + virtual ~CppADLocalReParameterizationFunctor() = default; + + template + bool operator()( + T* state_raw, T* state_other_raw, T* delta_raw + ) const { + const Eigen::Map> state(state_raw); + const Eigen::Map> state_other(state_other_raw); + Eigen::Map> delta(delta_raw); + + delta = state - state_other; + + return true; + } + + template + Jacobian ComputeJacobian( + Eigen::Ref> _state + ) { + MatrixXad state = _state.template cast(); + MatrixXad state_other = state; + MatrixXad delta(LieGroup::DoF); + + CppAD::Independent(state); + + this->template operator()(state.data(), state_other.data(), delta.data()); + + CppAD::ADFun ad_fun(state, delta); + + MANIF_ASSERT(delta.isZero(manif::Constants::eps)); + + Eigen::Matrix jac; + + switch (M) { + case Mode::Aut: + jac = ad_fun.Jacobian(state.template cast().eval()); + break; + case Mode::For: + jac.resize(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, state.template cast().eval(), jac); + break; + case Mode::Rev: + jac.resize(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, state.template cast().eval(), jac); + break; + default: + MANIF_THROW("Unknown auto differentiation mode."); + break; + } + + MANIF_ASSERT(LieGroup::RepSize * LieGroup::DoF == jac.size()); + + return Eigen::Map(jac.data()); + } +}; + +template +class CommonTesterCppAD : public testing::TestWithParam> { + using LieGroup = _LieGroup; + using Scalar = typename LieGroup::Scalar; + using Tangent = typename LieGroup::Tangent; + using Jacobian = typename LieGroup::Jacobian; + + template + using LieGroupTemplate = typename LieGroup::template LieGroupTemplate<_Scalar>; + + template + using TangentTemplate = typename Tangent::template TangentTemplate<_Scalar>; + + using Ad = CppAD::AD; + using AdFun = CppAD::ADFun; + + using LocalReParameterization = CppADLocalReParameterizationFunctor; + + using LPJacobian = Eigen::Matrix< + Scalar, + LieGroup::RepSize, + LieGroup::DoF, + (LieGroup::DoF>1)? Eigen::RowMajor : Eigen::ColMajor + >; + + using VectorXs = Eigen::Matrix; + using VectorXad = Eigen::Matrix; + + const LieGroup& getState() const { + return std::get<0>(this->GetParam()); + } + + const LieGroup& getStateOther() const { + return std::get<1>(this->GetParam()); + } + + const Tangent& getDelta() const { + return std::get<2>(this->GetParam()); + } + + const Tangent& getDeltaOther() const { + return std::get<3>(this->GetParam()); + } + +public: + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + CommonTesterCppAD() = default; + ~CommonTesterCppAD() = default; + + void evalRminusJac() { + // The variable block. + // @note CppAD doesn't like fixed size vector, + // we thus work with a Eigen dynamic one... + VectorXad variables(LieGroup::RepSize + LieGroup::RepSize); + + VectorXad delta_vec(Tangent::RepSize); + + Eigen::Map> + state(variables.data()), + state_other(variables.data()+LieGroup::RepSize); + Eigen::Map> delta(delta_vec.data()); + + state = getState().template cast(); + state_other = getStateOther().template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + delta = state.rminus(state_other); + + // create f: state, state_other -> delta + // and stop taping + AdFun ad_fun(variables, delta_vec); + + // Compute analytic Jacobians + Jacobian J_d_s, J_d_so; + EXPECT_MANIF_NEAR( + getState().rminus(getStateOther(), J_d_s, J_d_so), + delta.template cast(), + tol_ + ); + + VectorXs variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state, state_other + + // Forward + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::DoF * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::For + ); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_d_s, adJ.block(0, 0, LieGroup::DoF, LieGroup::RepSize) * lpJ_s_0, tol_ + ); + EXPECT_EIGEN_NEAR( + J_d_so, + adJ.block(0, LieGroup::RepSize, LieGroup::DoF, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + + // Reverse + if (CPPAD_REVERSE) { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::DoF * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::Rev + ); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_d_s, adJ.block(0, 0, LieGroup::DoF, LieGroup::RepSize) * lpJ_s_0, tol_ + ); + EXPECT_EIGEN_NEAR( + J_d_so, + adJ.block(0, LieGroup::RepSize, LieGroup::DoF, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + } + + void evalLminusJac() { + // The variable block. + // @note CppAD doesn't like fixed size vector, + // we thus work with a Eigen dynamic one... + VectorXad variables(LieGroup::RepSize + LieGroup::RepSize); + + VectorXad delta_vec(Tangent::RepSize); + + Eigen::Map> + state(variables.data()), + state_other(variables.data()+LieGroup::RepSize); + Eigen::Map> delta(delta_vec.data()); + + state = getState().template cast(); + state_other = getStateOther().template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + delta = state.lminus(state_other); + + // create f: state, state_other -> delta + // and stop taping + AdFun ad_fun(variables, delta_vec); + + // Compute analytic Jacobians + Jacobian J_d_s, J_d_so; + EXPECT_MANIF_NEAR( + getState().lminus(getStateOther(), J_d_s, J_d_so), + delta.template cast(), + tol_ + ); + + VectorXs variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state, state_other + + // Forward + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::DoF * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::For + ); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_d_s, adJ.block(0, 0, LieGroup::DoF, LieGroup::RepSize) * lpJ_s_0, tol_ + ); + EXPECT_EIGEN_NEAR( + J_d_so, + adJ.block(0, LieGroup::RepSize, LieGroup::DoF, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + + // Reverse + if (CPPAD_REVERSE) { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::DoF * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::Rev + ); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_d_s, adJ.block(0, 0, LieGroup::DoF, LieGroup::RepSize) * lpJ_s_0, tol_ + ); + EXPECT_EIGEN_NEAR( + J_d_so, + adJ.block(0, LieGroup::RepSize, LieGroup::DoF, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + } + + void evalRplusJac() { + VectorXad variables(LieGroup::RepSize + LieGroup::DoF); + + VectorXad variables_out(LieGroup::RepSize); + + Eigen::Map> state(variables.data()); + Eigen::Map> delta(variables.data()+LieGroup::RepSize); + Eigen::Map> state_out(variables_out.data()); + + state = getState().template cast(); + delta = getDelta().template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + state_out = state.rplus(delta); + + // create f: state, delta -> state_out + // and stop taping + AdFun ad_fun(variables, variables_out); + + // Compute analytic Jacobians + Jacobian J_so_s, J_so_d; + EXPECT_MANIF_NEAR( + getState().rplus(getDelta(), J_so_s, J_so_d), + state_out.template cast(), + tol_ + ); + + VectorXs variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state, state_other + + // Forward + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ((LieGroup::RepSize + LieGroup::DoF) * LieGroup::RepSize, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_so_s, + lrpJ * adJ.block(0, 0, LieGroup::RepSize, LieGroup::RepSize) * lpJ_s_0, + tol_ + ); + EXPECT_EIGEN_NEAR( + J_so_d, + lrpJ * adJ.block(0, LieGroup::RepSize, LieGroup::RepSize, LieGroup::DoF), + tol_ + ); + } + + // Reverse + if (CPPAD_REVERSE) { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ((LieGroup::RepSize + LieGroup::DoF) * LieGroup::RepSize, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_so_s, + lrpJ * adJ.block(0, 0, LieGroup::RepSize, LieGroup::RepSize) * lpJ_s_0, + tol_ + ); + EXPECT_EIGEN_NEAR( + J_so_d, + lrpJ * adJ.block(0, LieGroup::RepSize, LieGroup::RepSize, LieGroup::DoF), + tol_ + ); + } + } + + void evalLplusJac() { + VectorXad variables(LieGroup::RepSize + LieGroup::DoF); + + VectorXad variables_out(LieGroup::RepSize); + + Eigen::Map> state(variables.data()); + Eigen::Map> delta(variables.data()+LieGroup::RepSize); + Eigen::Map> state_out(variables_out.data()); + + state = getState().template cast(); + delta = getDelta().template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + state_out = state.lplus(delta); + + // create f: state, delta -> state_out + // and stop taping + AdFun ad_fun(variables, variables_out); + + // Compute analytic Jacobians + Jacobian J_so_s, J_so_d; + EXPECT_MANIF_NEAR( + getState().lplus(getDelta(), J_so_s, J_so_d), + state_out.template cast(), + tol_ + ); + + VectorXs variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state, state_other + + // Forward + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ((LieGroup::RepSize + LieGroup::DoF) * LieGroup::RepSize, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_so_s, + lrpJ * adJ.block(0, 0, LieGroup::RepSize, LieGroup::RepSize) * lpJ_s_0, + tol_ + ); + EXPECT_EIGEN_NEAR( + J_so_d, + lrpJ * adJ.block(0, LieGroup::RepSize, LieGroup::RepSize, LieGroup::DoF), + tol_ + ); + } + + // Reverse + if (CPPAD_REVERSE) { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ((LieGroup::RepSize + LieGroup::DoF) * LieGroup::RepSize, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_so_s, + lrpJ * adJ.block(0, 0, LieGroup::RepSize, LieGroup::RepSize) * lpJ_s_0, + tol_ + ); + EXPECT_EIGEN_NEAR( + J_so_d, + lrpJ * adJ.block(0, LieGroup::RepSize, LieGroup::RepSize, LieGroup::DoF), + tol_ + ); + } + } + + void evalLogJac() { + VectorXad variables(LieGroup::RepSize); + VectorXad delta_vec(Tangent::RepSize); + + Eigen::Map> state(variables.data()); + Eigen::Map> delta(delta_vec.data()); + + state = getState().template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + delta = state.log(); + + // create f: state -> delta and stop taping + AdFun ad_fun(variables, delta_vec); + + // Compute analytic Jacobians + Jacobian J_d_s; + EXPECT_MANIF_NEAR(getState().log(J_d_s), delta.template cast(), tol_); + + auto variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state + + // Forward + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::DoF * LieGroup::RepSize, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + // Retrieve the block Jacobians + Eigen::Map> adJ_d_s(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR(J_d_s, adJ_d_s * lpJ, tol_); + } + + // Reverse + if (CPPAD_REVERSE) { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::DoF * LieGroup::RepSize, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + // Retrieve the block Jacobians + Eigen::Map> adJ_d_s(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR(J_d_s, adJ_d_s * lpJ, tol_); + } + } + + void evalExpJac() { + VectorXad variables(Tangent::RepSize); + VectorXad variables_out(LieGroup::RepSize); + + Eigen::Map> delta(variables.data()); + Eigen::Map> state_out(variables_out.data()); + + delta = getDelta().template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + state_out = delta.exp(); + + // create f: state -> delta and stop taping + AdFun ad_fun(variables, variables_out); + + // Compute analytic Jacobians + Jacobian J_x_d; + EXPECT_MANIF_NEAR( + getDelta().exp(J_x_d), state_out.template cast(), tol_ + ); + + auto variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::RepSize * LieGroup::DoF, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the block Jacobians + Eigen::Map1)? Eigen::RowMajor : Eigen::ColMajor + >> adJ_x_d(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR(J_x_d, lrpJ * adJ_x_d, tol_); + } + + // @todo Error detected by false result for + // size_t(x.size()) == f.Domain() + + if (CPPAD_REVERSE) { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::RepSize * LieGroup::DoF, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the block Jacobians + Eigen::Map1)? Eigen::RowMajor : Eigen::ColMajor + >> adJ_x_d(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR(J_x_d, lrpJ * adJ_x_d, tol_); + } + } + + void evalInverseJac() { + VectorXad variables(LieGroup::RepSize); + VectorXad state_out_vec(LieGroup::RepSize); + + Eigen::Map> state(variables.data()); + Eigen::Map> state_out(state_out_vec.data()); + + state = getState().template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + state_out = state.inverse(); + + // create f: state -> delta and stop taping + AdFun ad_fun(variables, state_out_vec); + + // Compute analytic Jacobians + Jacobian J_d_s; + EXPECT_MANIF_NEAR( + getState().inverse(J_d_s), state_out.template cast(), tol_ + ); + + // Compute AD Jacobians at this state + { + auto variables_s = variables.template cast().eval(); + + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::RepSize * LieGroup::RepSize, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + variables_s = state_out_vec.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_s); + + // Retrieve the block Jacobians + Eigen::Map> adJ_d_s(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR(J_d_s, lrpJ * adJ_d_s * lpJ, tol_); + } + + if (CPPAD_REVERSE) { + auto variables_s = variables.template cast().eval(); + + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::RepSize * LieGroup::RepSize, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + variables_s = state_out_vec.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_s); + + // Retrieve the block Jacobians + Eigen::Map> adJ_d_s(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR(J_d_s, lrpJ * adJ_d_s * lpJ, tol_); + } + } + + void evalComposeJac() { + VectorXad variables(LieGroup::RepSize + LieGroup::RepSize); + + VectorXad variables_out(LieGroup::RepSize); + + Eigen::Map> + state(variables.data()), + state_other(variables.data()+LieGroup::RepSize), + state_out(variables_out.data()); + + state = getState().template cast(); + state_other = getStateOther().template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + state_out = state.compose(state_other); + + // create f: state, state_other -> delta + // and stop taping + AdFun ad_fun(variables, variables_out); + + // Compute analytic Jacobians + Jacobian J_cs_s, J_cs_so; + EXPECT_MANIF_NEAR( + getState().compose(getStateOther(), J_cs_s, J_cs_so), + state_out.template cast(), + tol_ + ); + + VectorXs variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state, state_other + + // Forward + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::RepSize * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::For + ); + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_cs_s, + lrpJ * adJ.block(0, 0, LieGroup::RepSize, LieGroup::RepSize) * lpJ_s_0, + tol_ + ); + EXPECT_EIGEN_NEAR( + J_cs_so, + lrpJ * adJ.block(0, LieGroup::RepSize, LieGroup::RepSize, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + + // Reverse + if (CPPAD_REVERSE) { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::RepSize * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::Rev + ); + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_cs_s, + lrpJ * adJ.block(0, 0, LieGroup::RepSize, LieGroup::RepSize) * lpJ_s_0, + tol_ + ); + EXPECT_EIGEN_NEAR( + J_cs_so, + lrpJ * adJ.block(0, LieGroup::RepSize, LieGroup::RepSize, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + } + + void evalBetweenJac() { + VectorXad variables(LieGroup::RepSize + LieGroup::RepSize); + + VectorXad variables_out(LieGroup::RepSize); + + Eigen::Map> + state(variables.data()), + state_other(variables.data()+LieGroup::RepSize), + state_out(variables_out.data()); + + state = getState().template cast(); + state_other = getStateOther().template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + state_out = state.between(state_other); + + // create f: state, state_other -> delta + // and stop taping + AdFun ad_fun(variables, variables_out); + + // Compute analytic Jacobians + Jacobian J_cs_s, J_cs_so; + EXPECT_MANIF_NEAR( + getState().between(getStateOther(), J_cs_s, J_cs_so), + state_out.template cast(), + tol_ + ); + + VectorXs variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state, state_other + + // Forward + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::RepSize * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::For + ); + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_cs_s, + lrpJ * adJ.block(0, 0, LieGroup::RepSize, LieGroup::RepSize) * lpJ_s_0, + tol_ + ); + EXPECT_EIGEN_NEAR( + J_cs_so, + lrpJ * adJ.block(0, LieGroup::RepSize, LieGroup::RepSize, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + + // Reverse + if (CPPAD_REVERSE) { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::RepSize * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::Rev + ); + + VectorXs variables_out_s = variables_out.template cast(); + + typename LocalReParameterization::Jacobian lrpJ = LocalReParameterization( + ).template ComputeJacobian(variables_out_s); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_cs_s, + lrpJ * adJ.block(0, 0, LieGroup::RepSize, LieGroup::RepSize) * lpJ_s_0, + tol_ + ); + EXPECT_EIGEN_NEAR( + J_cs_so, + lrpJ * adJ.block(0, LieGroup::RepSize, LieGroup::RepSize, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + } + + void evalActJac() { + VectorXad variables(LieGroup::RepSize + LieGroup::Dim); + + VectorXad variables_out(LieGroup::Dim); + + Eigen::Matrix vector_s; + vector_s.setRandom(); + + Eigen::Map> state(variables.data()); + Eigen::Map> + vector(variables.data()+LieGroup::RepSize), + vector_out(variables_out.data()); + + state = getState().template cast(); + vector = vector_s.template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + vector_out = state.act(vector); + + // create f: state, vec -> vec_out + // and stop taping + AdFun ad_fun(variables, variables_out); + + // Compute analytic Jacobians + Eigen::Matrix J_cs_s; + Eigen::Matrix J_cs_so; + EXPECT_EIGEN_NEAR( + getState().act(vector_s, J_cs_s, J_cs_so), + vector_out.template cast(), + tol_ + ); + + VectorXs variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state, state_other + + // Forward + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ((LieGroup::RepSize + LieGroup::Dim) * LieGroup::Dim, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_cs_s, adJ.block(0, 0, LieGroup::Dim, LieGroup::RepSize) * lpJ_s_0, tol_ + ); + EXPECT_EIGEN_NEAR( + J_cs_so, adJ.block(0, LieGroup::RepSize, LieGroup::Dim, LieGroup::Dim), tol_ + ); + } + + // Reverse + if (CPPAD_REVERSE) { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ((LieGroup::RepSize + LieGroup::Dim) * LieGroup::Dim, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_cs_s, adJ.block(0, 0, LieGroup::Dim, LieGroup::RepSize) * lpJ_s_0, tol_ + ); + EXPECT_EIGEN_NEAR( + J_cs_so, adJ.block(0, LieGroup::RepSize, LieGroup::Dim, LieGroup::Dim), tol_ + ); + } + } + + void evalTape() { + // We evaluate here the jacs of a dummy, complex expression + // to make sure that cppad's tape holds on. + + LieGroup sstate = getState(); + LieGroup sstate_other = getStateOther(); + + VectorXad variables(LieGroup::RepSize + LieGroup::RepSize); + + VectorXad delta_vec(Tangent::RepSize); + + Eigen::Map> + state(variables.data()), + state_other(variables.data()+LieGroup::RepSize); + Eigen::Map> delta(delta_vec.data()); + + state = sstate.template cast(); + state_other = sstate_other.template cast(); + + // declare independent variables and start taping + CppAD::Independent(variables); + + delta = state.rminus(state).exp().rminus(state_other).exp().log(); + + // create f: state, state_other -> delta + // and stop taping + AdFun ad_fun(variables, delta_vec); + + // Compute analytic Jacobians ds/dd, dso/dd + Jacobian J_I_slhs, J_I_srhs, J_exp_I, J_rm_exp, J_rm_so, J_exprm_rm, J_d_exprm; + EXPECT_MANIF_NEAR( + sstate.rminus( + sstate, J_I_slhs, J_I_srhs + ).exp(J_exp_I).rminus( + sstate_other, J_rm_exp, J_rm_so + ).exp(J_exprm_rm).log(J_d_exprm), + delta.template cast(), + tol_ + ); + + Jacobian J_d_rm = J_d_exprm * J_exprm_rm; + Jacobian J_d_I = J_d_rm * J_rm_exp * J_exp_I; + Jacobian J_d_s = J_d_I * J_I_slhs + J_d_I * J_I_srhs; + Jacobian J_d_so = J_d_rm * J_rm_so; + + VectorXs variables_s = variables.template cast().eval(); + + // Compute AD Jacobians at this state, state_other + + // Forward + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianFor(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::DoF * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::For + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::For + ); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_d_s, adJ.block(0, 0, LieGroup::DoF, LieGroup::RepSize) * lpJ_s_0, tol_ + ); + EXPECT_EIGEN_NEAR( + J_d_so, + adJ.block(0, LieGroup::RepSize, LieGroup::DoF, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + + // Reverse + { + VectorXs jac(ad_fun.Domain()*ad_fun.Range()); + CppAD::JacobianRev(ad_fun, variables_s, jac); + + EXPECT_EQ(LieGroup::DoF * LieGroup::RepSize * 2, jac.size()); + + // Compute the local parameterization Jacobian + // that maps to the local tangent space. + LPJacobian lpJ_s_0 = cppadLocalParameterizationJacobian( + getState(), manif::AutoDifferentiation::Rev + ); + + LPJacobian lpJ_so_0 = cppadLocalParameterizationJacobian( + getStateOther(), manif::AutoDifferentiation::Rev + ); + + // Retrieve the respective state, state_other block Jacobians + // @note CppAD seem to have a RowMajor layout + Eigen::Map> adJ(jac.data()); + + // Compare Analytic vs AD. + EXPECT_EIGEN_NEAR( + J_d_s, adJ.block(0, 0, LieGroup::DoF, LieGroup::RepSize) * lpJ_s_0, tol_ + ); + EXPECT_EIGEN_NEAR( + J_d_so, + adJ.block(0, LieGroup::RepSize, LieGroup::DoF, LieGroup::RepSize) * lpJ_so_0, + tol_ + ); + } + } + +protected: + + Scalar tol_ = (std::is_same::value)? 1e-3 : 1e-8; +}; + +#endif // _MANIF_MANIF_TEST_CPPAD_COMMON_TESTER_CPPAD_H_ \ No newline at end of file diff --git a/test/cppad/gtest_cppad.cpp b/test/cppad/gtest_cppad.cpp new file mode 100644 index 00000000..205b837e --- /dev/null +++ b/test/cppad/gtest_cppad.cpp @@ -0,0 +1,10 @@ +#include "manif/manif.h" +#include "manif/cppad/cppad.h" + +#include "gtest_common_tester_cppad.h" + +using namespace manif; + +MANIF_TEST_CPPAD_ALL; + +MANIF_RUN_ALL_TEST; diff --git a/test/gtest/CMakeLists.txt.in b/test/gtest/CMakeLists.txt.in index 452b73e3..351b13cf 100644 --- a/test/gtest/CMakeLists.txt.in +++ b/test/gtest/CMakeLists.txt.in @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.2) +cmake_minimum_required(VERSION 3.5.1) project(googletest-download NONE)