bspline.hpp

// SPDX-License-Identifier: MIT
// Copyright (c) 2021  Kenji Koide (k.koide@aist.go.jp)
 
#pragma once
 
#include <gtsam/slam/expressions.h>
 
namespace gtsam_points {
 
gtsam::Pose3_ bspline(const gtsam::Pose3_& pose0, const gtsam::Pose3_& pose1, const gtsam::Pose3_& pose2, const gtsam::Pose3_& pose3, const gtsam::Double_& t);
 
gtsam::Pose3_ bspline_se3(const gtsam::Pose3_& pose0, const gtsam::Pose3_& pose1, const gtsam::Pose3_& pose2, const gtsam::Pose3_& pose3, const gtsam::Double_& t);
 
gtsam::Rot3_ bspline_so3(const gtsam::Rot3_& rot0, const gtsam::Rot3_& rot1, const gtsam::Rot3_& rot2, const gtsam::Rot3_& rot3, const gtsam::Double_& t);
 
gtsam::Vector3_ bspline_trans(const gtsam::Vector3_& trans0, const gtsam::Vector3_& trans1, const gtsam::Vector3_& trans2, const gtsam::Vector3_& trans3, const gtsam::Double_& t);
 
gtsam::Vector3_
bspline_angular_vel(const gtsam::Rot3_& rot0, const gtsam::Rot3_& rot1, const gtsam::Rot3_& rot2, const gtsam::Rot3_& rot3, const gtsam::Double_& t, const double knot_interval);
 
gtsam::Vector3_ bspline_linear_vel(
  const gtsam::Vector3_& trans0,
  const gtsam::Vector3_& trans1,
  const gtsam::Vector3_& trans2,
  const gtsam::Vector3_& trans3,
  const gtsam::Double_& t,
  const double knot_interval);
 
gtsam::Vector3_ bspline_linear_acc(
  const gtsam::Vector3_& trans0,
  const gtsam::Vector3_& trans1,
  const gtsam::Vector3_& trans2,
  const gtsam::Vector3_& trans3,
  const gtsam::Double_& t,
  const double knot_interval);
 
gtsam::Vector6_ bspline_imu(
  const gtsam::Pose3_ pose0,
  const gtsam::Pose3_ pose1,
  const gtsam::Pose3_ pose2,
  const gtsam::Pose3_ pose3,
  const gtsam::Double_& t,
  const double knot_interval,
  const gtsam::Vector3& g);
 
// Utility functions
inline gtsam::Pose3_ bspline(const gtsam::Key key1, const gtsam::Double_& t) {
  return bspline(gtsam::Pose3_(key1 - 1), gtsam::Pose3_(key1), gtsam::Pose3_(key1 + 1), gtsam::Pose3_(key1 + 2), t);
}
 
inline gtsam::Pose3_ bspline_se3(const gtsam::Key key1, const gtsam::Double_& t) {
  return bspline_se3(gtsam::Pose3_(key1 - 1), gtsam::Pose3_(key1), gtsam::Pose3_(key1 + 1), gtsam::Pose3_(key1 + 2), t);
}
 
inline gtsam::Vector6_ bspline_imu(const gtsam::Key key1, const gtsam::Double_& t, const double knot_interval, const gtsam::Vector3& g) {
  return bspline_imu(gtsam::Pose3_(key1 - 1), gtsam::Pose3_(key1), gtsam::Pose3_(key1 + 1), gtsam::Pose3_(key1 + 2), t, knot_interval, g);
}
 
}  // namespace gtsam_points