Add support for CppAD

.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:

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<Scalar>`][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

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)

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<Scalar>`][cppad] (forward/reverse) auto-differentiation type.
+
+For reference of the size of the Jacobians returned when using [`CppAD::AD<Scalar>`][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<Scalar>;
+using AdFun = CppAD::ADFun<Scalar>;
+using VectorXs = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
+using VectorXad = Eigen::Matrix<Ad, Eigen::Dynamic, 1>;
+
+using LieGroup = manif::SE3<Ad>;
+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<LieGroup> xi(variables.data()), xj(variables.data()+LieGroup::RepSize);
+Eigen::Map<Tangent> 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<Scalar>());
+
+// Map the Jacobian as a matrice
+Eigen::Map<
+  Eigen::Matrix<Scalar, LieGroup::DoF, LieGroup::RepSize*2, Eigen::RowMajor>
+> 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<Scalar>`][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&amp;space;x
+[latex3]: https://latex.codecogs.com/svg.latex?\omega
+[latex4]: https://latex.codecogs.com/svg.latex?\bf&amp;space;x
+[latex5]: https://latex.codecogs.com/svg.latex?{\bf&amp;space;e}=f({\bf&amp;space;x})
+[latex6]: https://latex.codecogs.com/svg.latex?f({\bf&amp;space;x\oplus\omega})\approx{\bf&amp;space;e}+{\bf&amp;space;J}_{\omega}^{e}~\omega&amp;space;.
+[latex7]: https://latex.codecogs.com/svg.latex?{\bf&amp;space;J}_{\omega}^{e}=\frac{\delta{\bf&amp;space;e}}{\delta{\bf&amp;space;x}}=\frac{\delta&amp;space;f({\bf&amp;space;x})}{\delta{\bf&amp;space;x}}=\lim_{\omega\to0}\frac{f({\bf&amp;space;x}\oplus\omega)\ominus&amp;space;f({\bf&amp;space;x})}{\omega},&amp;space;(1)
+[latex8]: https://latex.codecogs.com/svg.latex?f({\bf&amp;space;x})
+[latex9]: https://latex.codecogs.com/svg.latex?{\bf&amp;space;e}=f({\bf&amp;space;x})
+[latex10]: https://latex.codecogs.com/svg.latex?{\bf&amp;space;J}_{{\bf&amp;space;x}\oplus\omega}^{e}=\frac{\delta{\bf&amp;space;e}}{\delta({\bf&amp;space;x}\oplus\omega)}=\lim_{\mathbf&amp;space;h\to0}\frac{&amp;space;f({\bf&amp;space;x}+\mathbf&amp;space;h)-f({\bf&amp;space;x})}{\mathbf&amp;space;h}.&amp;space;(2)
+[latex11]: https://latex.codecogs.com/svg.latex?{\bf&amp;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&amp;space;J}_{\omega}^{{\bf&amp;space;x}\oplus\omega}=\frac{\delta({\bf&amp;space;x}\oplus\omega)}{\delta\omega}=\lim_{\delta\omega\to0}\frac{{\bf&amp;space;x}\oplus(\omega+\delta\omega)-{\bf&amp;space;x}\oplus\mathbf\omega}{\delta\omega}=\lim_{\delta\omega\to0}\frac{{\bf&amp;space;x}\oplus\delta\omega-{\bf&amp;space;x}}{\delta\omega}.&amp;space;(3)
+[latex15]: https://latex.codecogs.com/svg.latex?{\bf&amp;space;J}_{\omega}^{e}={\bf&amp;space;J}_{{\bf&amp;space;x}\oplus\omega}^{e}\times{\bf&amp;space;J}_{\omega}^{{\bf&amp;space;x}\oplus\omega}.&amp;space;(4)
+[latex16]: https://latex.codecogs.com/svg.latex?{\bf&amp;space;x}\oplus\mathbf\omega

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"

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"

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"

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"

include/manif/constants.h

@@ -73,6 +73,11 @@ struct Constants<float>
   static constexpr float to_deg = float(180.0 / MANIF_PI);
 };
 
+constexpr float Constants<float>::eps;
+constexpr float Constants<float>::eps_sqrt;
+constexpr float Constants<float>::to_rad;
+constexpr float Constants<float>::to_deg;
+
 } /* namespace manif  */
 
 #endif /* _MANIF_MANIF_CONSTANTS_H_ */

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 <typename _Base>
+struct CondOpLtHelper<CppAD::AD<_Base>> {
+  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 <typename _Base>
+struct CondOpGtHelper<CppAD::AD<_Base>> {
+  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 <typename Derived>
+  static Eigen::Matrix<typename Derived::Scalar, Derived::Rows, 1> eval(
+    const Scalar& lhs,
+    const Scalar& rhs,
+    const Eigen::MatrixBase<Derived>& vt,
+    const Eigen::MatrixBase<Derived>& vf
+  ) {
+    using Scalar = typename Derived::Scalar;
+    static constexpr unsigned int Size = Derived::Rows;
+    Eigen::Matrix<Scalar, Size, 1> ret;
+
+    for (int i=0; i<Size; ++i) {
+      ret[i] = CppAD::CondExpGt(lhs, rhs, vt[i], vf[i]);
+    }
+
+    return ret;
+  }
+};
+
+} // namespace internal
+} // namespace manif
+
+#endif // _MANIF_MANIF_CPPAD_CONDITIONAL_OP_H_

include/manif/cppad/constants.h

@@ -0,0 +1,26 @@
+#ifndef _MANIF_MANIF_CPPAD_CONSTANTS_H_
+#define _MANIF_MANIF_CPPAD_CONSTANTS_H_
+
+namespace manif {
+
+/// @brief Specialize Constants traits for the float-based CppAD::AD type
+template <>
+struct Constants<CppAD::AD<float>> {
+  static const CppAD::AD<float> eps;
+};
+
+const CppAD::AD<float>
+Constants<CppAD::AD<float>>::eps = CppAD::AD<float>(1e-6);
+
+/// @brief Specialize Constants traits for the double-based CppAD::AD type
+template <>
+struct Constants<CppAD::AD<double>> {
+  static const CppAD::AD<double> eps;
+};
+
+const CppAD::AD<double>
+Constants<CppAD::AD<double>>::eps = CppAD::AD<double>(1e-14);
+
+} // namespace manif
+
+#endif // _MANIF_MANIF_CPPAD_CONSTANTS_H_

include/manif/cppad/cppad.h

@@ -0,0 +1,18 @@
+#ifndef _MANIF_MANIF_CPPAD_CPPAD_H_
+#define _MANIF_MANIF_CPPAD_CPPAD_H_
+
+#include <cppad/cppad.hpp>
+
+#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 <typename Scalar>
+struct is_ad<CppAD::AD<Scalar>> : std::integral_constant<bool, true> { };
+} // namespace internal
+} // namespace manif
+
+#endif // _MANIF_MANIF_CPPAD_CPPAD_H_