Automatic Differentiation 'Dual' Type (#100)

* Dual numbers initial implementation

* Add test coverage, with partial derivative example

* Add Jacobian test, fix small issues

* Improve test to demonstrate non-square jacobian

* Better naming for collectReals/Derivatives

* Improve comments

* Potential GCC 4.8 bug workaround

* Add fallback workaround for non-IEEE float platforms

matrix/Dual.hpp

@@ -0,0 +1,355 @@
+/**
+ * @file Dual.hpp
+ *
+ * This is a dual number type for calculating automatic derivatives.
+ *
+ * Based roughly on the methods described in:
+ * Automatic Differentiation, C++ Templates and Photogrammetry, by Dan Piponi
+ * and
+ * Ceres Solver's excellent Jet.h
+ *
+ * @author Julian Kent <julian@auterion.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M>
+class Vector;
+
+template <typename Scalar, size_t N>
+struct Dual
+{
+    static constexpr size_t WIDTH = N;
+
+    Scalar value {};
+    Vector<Scalar, N> derivative;
+
+    Dual() = default;
+
+    explicit Dual(Scalar v, size_t inputDimension = 65535)
+    {
+        value = v;
+        if (inputDimension < N) {
+            derivative(inputDimension) = Scalar(1);
+        }
+    }
+
+    explicit Dual(Scalar v, const Vector<Scalar, N>& d) :
+        value(v), derivative(d)
+    {}
+
+    Dual<Scalar, N>& operator=(const Scalar& a)
+    {
+        derivative.setZero();
+        value = a;
+        return *this;
+    }
+
+    Dual<Scalar, N>& operator +=(const Dual<Scalar, N>& a)
+    {
+        return (*this = *this + a);
+    }
+
+    Dual<Scalar, N>& operator *=(const Dual<Scalar, N>& a)
+    {
+        return (*this = *this * a);
+    }
+
+    Dual<Scalar, N>& operator -=(const Dual<Scalar, N>& a)
+    {
+        return (*this = *this - a);
+    }
+
+    Dual<Scalar, N>& operator /=(const Dual<Scalar, N>& a)
+    {
+        return (*this = *this / a);
+    }
+
+    Dual<Scalar, N>& operator +=(Scalar a)
+    {
+        return (*this = *this + a);
+    }
+
+    Dual<Scalar, N>& operator -=(Scalar a)
+    {
+        return (*this = *this - a);
+    }
+
+    Dual<Scalar, N>& operator *=(Scalar a)
+    {
+        return (*this = *this * a);
+    }
+
+    Dual<Scalar, N>& operator /=(Scalar a)
+    {
+        return (*this = *this / a);
+    }
+
+};
+
+// operators
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator+(const Dual<Scalar, N>& a)
+{
+    return a;
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator-(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(-a.value, -a.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return Dual<Scalar, N>(a.value + b.value, a.derivative + b.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return a + (-b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator+(const Dual<Scalar, N>& a, Scalar b)
+{
+    return Dual<Scalar, N>(a.value + b, a.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator-(const Dual<Scalar, N>& a, Scalar b)
+{
+    return a + (-b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator+(Scalar a, const Dual<Scalar, N>& b)
+{
+    return Dual<Scalar, N>(a + b.value, b.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator-(Scalar a, const Dual<Scalar, N>& b)
+{
+    return a + (-b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return Dual<Scalar, N>(a.value * b.value, a.value * b.derivative + b.value * a.derivative);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator*(const Dual<Scalar, N>& a, Scalar b)
+{
+    return Dual<Scalar, N>(a.value * b, a.derivative * b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator*(Scalar a, const Dual<Scalar, N>& b)
+{
+    return b * a;
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    const Scalar inv_b_real = Scalar(1) / b.value;
+    return Dual<Scalar, N>(a.value * inv_b_real, a.derivative * inv_b_real -
+                           a.value * b.derivative * inv_b_real * inv_b_real);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator/(const Dual<Scalar, N>& a, Scalar b)
+{
+    return a * (Scalar(1) / b);
+}
+
+template <typename Scalar, size_t N>
+Dual<Scalar, N> operator/(Scalar a, const Dual<Scalar, N>& b)
+{
+    const Scalar inv_b_real = Scalar(1) / b.value;
+    return Dual<Scalar, N>(a * inv_b_real, (-inv_b_real * a * inv_b_real) * b.derivative);
+}
+
+// basic math
+
+// sqrt
+template <typename Scalar, size_t N>
+Dual<Scalar, N> sqrt(const Dual<Scalar, N>& a)
+{
+    Scalar real = sqrt(a.value);
+    return Dual<Scalar, N>(real, a.derivative * (Scalar(1) / (Scalar(2) * real)));
+}
+
+// abs
+template <typename Scalar, size_t N>
+Dual<Scalar, N> abs(const Dual<Scalar, N>& a)
+{
+    return a.value >= Scalar(0) ? a : -a;
+}
+
+// ceil
+template <typename Scalar, size_t N>
+Dual<Scalar, N> ceil(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(ceil(a.value));
+}
+
+// floor
+template <typename Scalar, size_t N>
+Dual<Scalar, N> floor(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(floor(a.value));
+}
+
+// fmod
+template <typename Scalar, size_t N>
+Dual<Scalar, N> fmod(const Dual<Scalar, N>& a, Scalar mod)
+{
+    return Dual<Scalar, N>(a.value - Scalar(size_t(a.value / mod)) * mod, a.derivative);
+}
+
+// max
+template <typename Scalar, size_t N>
+Dual<Scalar, N> max(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return a.value >= b.value ? a : b;
+}
+
+// min
+template <typename Scalar, size_t N>
+Dual<Scalar, N> min(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    return a.value < b.value ? a : b;
+}
+
+// isnan
+template <typename Scalar, size_t N>
+bool isnan(const Dual<Scalar, N>& a)
+{
+    return isnan(a.value);
+}
+
+// isfinite
+template <typename Scalar, size_t N>
+bool isfinite(const Dual<Scalar, N>& a)
+{
+    return isfinite(a.value);
+}
+
+// isinf
+template <typename Scalar, size_t N>
+bool isinf(const Dual<Scalar, N>& a)
+{
+    return isinf(a.value);
+}
+
+// trig
+
+// sin
+template <typename Scalar, size_t N>
+Dual<Scalar, N> sin(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(sin(a.value), cos(a.value) * a.derivative);
+}
+
+// cos
+template <typename Scalar, size_t N>
+Dual<Scalar, N> cos(const Dual<Scalar, N>& a)
+{
+    return Dual<Scalar, N>(cos(a.value), -sin(a.value) * a.derivative);
+}
+
+// tan
+template <typename Scalar, size_t N>
+Dual<Scalar, N> tan(const Dual<Scalar, N>& a)
+{
+    Scalar real = tan(a.value);
+    return Dual<Scalar, N>(real, (Scalar(1) + real * real) * a.derivative);
+}
+
+// asin
+template <typename Scalar, size_t N>
+Dual<Scalar, N> asin(const Dual<Scalar, N>& a)
+{
+    Scalar asin_d = Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
+    return Dual<Scalar, N>(asin(a.value), asin_d * a.derivative);
+}
+
+// acos
+template <typename Scalar, size_t N>
+Dual<Scalar, N> acos(const Dual<Scalar, N>& a)
+{
+    Scalar acos_d = -Scalar(1) / sqrt(Scalar(1) - a.value * a.value);
+    return Dual<Scalar, N>(acos(a.value), acos_d * a.derivative);
+}
+
+// atan
+template <typename Scalar, size_t N>
+Dual<Scalar, N> atan(const Dual<Scalar, N>& a)
+{
+    Scalar atan_d = Scalar(1) / (Scalar(1) + a.value * a.value);
+    return Dual<Scalar, N>(atan(a.value), atan_d * a.derivative);
+}
+
+// atan2
+template <typename Scalar, size_t N>
+Dual<Scalar, N> atan2(const Dual<Scalar, N>& a, const Dual<Scalar, N>& b)
+{
+    // derivative is equal to that of atan(a/b), so substitute a/b into atan and simplify
+    Scalar atan_d = Scalar(1) / (a.value * a.value + b.value * b.value);
+    return Dual<Scalar, N>(atan2(a.value, b.value), (a.derivative * b.value - a.value * b.derivative) * atan_d);
+}
+
+// retrieve the derivative elements of a vector of Duals into a matrix
+template <typename Scalar, size_t M, size_t N>
+Matrix<Scalar, M, N> collectDerivatives(const Matrix<Dual<Scalar, N>, M, 1>& input)
+{
+    Matrix<Scalar, M, N> jac;
+    for (size_t i = 0; i < M; i++) {
+        jac.row(i) = input(i, 0).derivative;
+    }
+    return jac;
+}
+
+// retrieve the real (non-derivative) elements of a matrix of Duals into an equally sized matrix
+template <typename Scalar, size_t M, size_t N, size_t D>
+Matrix<Scalar, M, N> collectReals(const Matrix<Dual<Scalar, D>, M, N>& input)
+{
+    Matrix<Scalar, M, N> r;
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            r(i,j) = input(i,j).value;
+        }
+    }
+    return r;
+}
+
+#if defined(SUPPORT_STDIOSTREAM)
+template<typename Type, size_t N>
+std::ostream& operator<<(std::ostream& os,
+                         const matrix::Dual<Type, N>& dual)
+{
+    os << "[";
+    os << std::setw(10) << dual.value << ";";
+    for (size_t j = 0; j < N; ++j) {
+        os << "\t";
+        os << std::setw(10) << static_cast<double>(dual.derivative(j));
+    }
+    os << "]";
+    return os;
+}
+#endif // defined(SUPPORT_STDIOSTREAM)
+
+}
+

matrix/Matrix.hpp

@@ -34,12 +34,23 @@ template<typename Type, size_t M, size_t N>
 class Matrix
 {
 
-    Type _data[M][N] {};
+    Type _data[M][N];
 
 public:
 
     // Constructors
-    Matrix() = default;
+    Matrix()
+    {
+#ifdef  __STDC_IEC_559__
+        memset(_data, 0, sizeof(_data)); //workaround for GCC 4.8 bug, don't use {} for array init
+#else
+        for (size_t i = 0; i < M; i++) {
+            for (size_t j = 0; j < N; j++) {
+                _data[i][j] = Type{};
+            }
+        }
+#endif
+    }
 
     explicit Matrix(const Type data_[M*N])
     {
@@ -558,7 +569,14 @@ Matrix<Type, M, N> operator*(Type scalar, const Matrix<Type, M, N> &other)
 template<typename Type, size_t  M, size_t N>
 bool isEqual(const Matrix<Type, M, N> &x,
              const Matrix<Type, M, N> &y, const Type eps=1e-4f) {
-    return x == y;
+    for (size_t i = 0; i < M; i++) {
+        for (size_t j = 0; j < N; j++) {
+            if (!isEqualF(x(i,j), y(i,j), eps)) {
+                return false;
+            }
+        }
+    }
+    return true;
 }
 
 #if defined(SUPPORT_STDIOSTREAM)
@@ -569,7 +587,7 @@ std::ostream& operator<<(std::ostream& os,
     for (size_t i = 0; i < M; ++i) {
         os << "[";
         for (size_t j = 0; j < N; ++j) {
-            os << std::setw(10) << static_cast<double>(matrix(i, j));
+            os << std::setw(10) << matrix(i, j);
             os << "\t";
         }
         os << "]" << std::endl;

matrix/Quaternion.hpp

@@ -395,11 +395,11 @@ public:
      * @param vec vector to rotate in frame 1 (typically body frame)
      * @return rotated vector in frame 2 (typically reference frame)
      */
-    Vector3f conjugate(const Vector3f &vec) {
-        Quaternion q = *this;
-        Quaternion v(0, vec(0), vec(1), vec(2));
+    Vector3<Type> conjugate(const Vector3<Type> &vec) const {
+        const Quaternion& q = *this;
+        Quaternion v(Type(0), vec(0), vec(1), vec(2));
         Quaternion res = q*v*q.inversed();
-        return Vector3f(res(1), res(2), res(3));
+        return Vector3<Type>(res(1), res(2), res(3));
     }
 
     /**
@@ -411,12 +411,12 @@ public:
      * @param vec vector to rotate in frame 2 (typically reference frame)
      * @return rotated vector in frame 1 (typically body frame)
      */
-    Vector3f conjugate_inversed(const Vector3f &vec) const
+    Vector3<Type> conjugate_inversed(const Vector3<Type> &vec) const
     {
-        Quaternion q = *this;
-        Quaternion v(0, vec(0), vec(1), vec(2));
+        const Quaternion& q = *this;
+        Quaternion v(Type(0), vec(0), vec(1), vec(2));
         Quaternion res = q.inversed()*v*q;
-        return Vector3f(res(1), res(2), res(3));
+        return Vector3<Type>(res(1), res(2), res(3));
     }
 
     /**

matrix/Vector.hpp

@@ -54,7 +54,7 @@ public:
 
     Type dot(const MatrixM1 & b) const {
         const Vector &a(*this);
-        Type r = 0;
+        Type r(0);
         for (size_t i = 0; i<M; i++) {
             r += a(i)*b(i,0);
         }
@@ -66,7 +66,7 @@ public:
         return a.dot(b);
     }
 
-    inline Vector operator*(float b) const {
+    inline Vector operator*(Type b) const {
         return Vector(MatrixM1::operator*(b));
     }
 

matrix/math.hpp

@@ -17,3 +17,4 @@
 #include "Quaternion.hpp"
 #include "AxisAngle.hpp"
 #include "LeastSquaresSolver.hpp"
+#include "Dual.hpp"