Skip to content

Instantly share code, notes, and snippets.

@scturtle scturtle/spline.h
Created Sep 21, 2018

Embed
What would you like to do?
Cubic B-Spline interpolation of SE(3)
#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
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.