Cubic B-Spline interpolation of SE(3)

spline.h

#pragma once

#include <sophus/se3.hpp>

// follows spline fusion paper
// modified from minimal_ceres_sophus and kontiki

// forward declaration
namespace ceres
{
template <typename Scalar, int N> class Jet;
}

// clang-format off
const Eigen::Matrix4d C = (Eigen::Matrix4d() <<
        6. / 6., 0., 0., 0.,
        5. / 6., 3. / 6., -3. / 6., 1. / 6.,
        1. / 6., 3. / 6., 3. / 6., -2. / 6.,
        0., 0., 0., 1. / 6.).finished();
// clang-format on

template <typename T> class CubicSpline
{
  public:
    typedef Sophus::SE3<T> SE3Type;
    typedef Sophus::Matrix4<T> SE3DerivType;
    typedef Eigen::Map<SE3Type> KnotMap;

    CubicSpline(const double dt = 1., const double t0 = 0.)
        : dt_(dt), t0_(t0){};

    void add_knot(T *data) { knots_.push_back(data); }

    KnotMap get_knot(size_t k) const { return knots_[k]; }

    // evaluate pose and derivatives
    void evaluate(T t, SE3Type &P, SE3DerivType &P_prim,
                  SE3DerivType &P_bis) const;

  protected:
    static int floor_(double x) { return static_cast<int>(std::floor(x)); }
    template <typename Scalar, int N>
    static int floor_(const ceres::Jet<Scalar, N> &x)
    {
        return static_cast<int>(std::floor(x.a));
    };

    const double dt_; // time interval
    const double t0_; // t0
    std::vector<T *> knots_;
};

template <typename T>
void CubicSpline<T>::evaluate(T t, SE3Type &P, SE3DerivType &P_prim,
                              SE3DerivType &P_bis) const
{
    typedef Sophus::Matrix4<T> Mat4;
    typedef Sophus::Vector4<T> Vec4;

    assert(knots_.size() >= 4);
    assert(t >= T(t0_ + dt_));
    assert(t < (t0_ + (dt_ * (knots_.size() - 2))));

    // Spline normalized time (offset aware)
    T s = (t - T(t0_)) / T(dt_);
    int i = floor_(s);
    T u = s - T(i);
    int i0 = i - 1;

    // cumulative basis and time derivatives
    T u2(u * u), u3(u2 * u), dt_inv(1 / dt_);
    Mat4 CT = C.cast<T>();
    Vec4 B = CT * Vec4{T(1), u, u2, u3};
    Vec4 Bd1 = CT * Vec4{T(0), T(1), T(2) * u, T(3) * u2} * dt_inv;
    Vec4 Bd2 = CT * Vec4{T(0), T(0), T(2), T(6) * u} * dt_inv * dt_inv;

    KnotMap P0(knots_[i0]);
    P = P0; // formula 4

    Mat4 A[3], Ad1[3], Ad2[3];

    for (int j : {1, 2, 3})
    {
        KnotMap knot1 = KnotMap(knots_[i0 + j - 1]);
        KnotMap knot2 = KnotMap(knots_[i0 + j]);
        typename SE3Type::Tangent omega = (knot1.inverse() * knot2).log();
        Mat4 omega_hat = SE3Type::hat(omega);
        SE3Type Aj = SE3Type::exp(B[j] * omega);
        P = P * Aj; // formula 4
        Mat4 Ajm = Aj.matrix();
        Mat4 Ajd1 = Ajm * omega_hat * Bd1[j];
        Mat4 Ajd2 = Ajd1 * omega_hat * Bd1[j] + //
                   Ajm * omega_hat * Bd2[j];
        A[j - 1] = Ajm;
        Ad1[j - 1] = Ajd1;
        Ad2[j - 1] = Ajd2;
    }

    // clang-format off
    // formula 5, 6
    Mat4 M1 = Ad1[0] * A[1] * A[2] +
              A[0] * Ad1[1] * A[2] +
              A[0] * A[1] * Ad1[2];
    Mat4 M2 = Ad2[0] * A[1] * A[2] +
              A[0] * Ad2[1] * A[2] +
              A[0] * A[1] * Ad2[2] +
              T(2.) * Ad1[0] * Ad1[1] * A[2] +
              T(2.) * Ad1[0] * A[1] * Ad1[2] +
              T(2.) * A[0] * Ad1[1] * Ad1[2];
    // clang-format on

    P_prim = P0.matrix() * M1;
    P_bis = P0.matrix() * M2;
    // velocity = P_prim.col(3).head(3);
    // acceleration = P_bis.col(3).head(3);
    // angular_velocity see kontiki
}