ov_core::BsplineSE3 class

B-Spline which performs interpolation over SE(3) manifold.

Contents

This class implements the b-spline functionality that allows for interpolation over the $\mathbb{SE}(3)$ manifold. This is based off of the derivations from Continuous-Time Visual-Inertial Odometry for Event Cameras and A Spline-Based Trajectory Representation for Sensor Fusion and Rolling Shutter Cameras with some additional derivations being available in these notes. The use of b-splines for $\mathbb{SE}(3)$ interpolation has the following properties:

  1. Local control, allowing the system to function online as well as in batch
  2. $C^2$ -continuity to enable inertial predictions and calculations
  3. Good approximation of minimal torque trajectories
  4. A parameterization of rigid-body motion devoid of singularities

The key idea is to convert a set of trajectory points into a continuous-time uniform cubic cumulative b-spline. As compared to standard b-spline representations, the cumulative form ensures local continuity which is needed for on-manifold interpolation. We leverage the cubic b-spline to ensure $C^2$ -continuity to ensure that we can calculate accelerations at any point along the trajectory. The general equations are the following

\begin{align*} {}^{w}_{s}\mathbf{T}(u(t)) &= {}^{w}_{i-1}\mathbf{T}~\mathbf{A}_0~\mathbf{A}_1~\mathbf{A}_2 \\ \empty {}^{w}_{s}\dot{\mathbf{T}}(u(t)) &= {}^{w}_{i-1}\mathbf{T} \Big( \dot{\mathbf{A}}_0~\mathbf{A}_1~\mathbf{A}_2 + \mathbf{A}_0~\dot{\mathbf{A}}_1~\mathbf{A}_2 + \mathbf{A}_0~\mathbf{A}_1~\dot{\mathbf{A}}_2 \Big) \\ \empty {}^{w}_{s}\ddot{\mathbf{T}}(u(t)) &= {}^{w}_{i-1}\mathbf{T} \Big( \ddot{\mathbf{A}}_0~\mathbf{A}_1~\mathbf{A}_2 + \mathbf{A}_0~\ddot{\mathbf{A}}_1~\mathbf{A}_2 + \mathbf{A}_0~\mathbf{A}_1~\ddot{\mathbf{A}}_2 \nonumber\\ &\hspace{4cm} + 2\dot{\mathbf{A}}_0\dot{\mathbf{A}}_1\mathbf{A}_2 + 2\mathbf{A}_0\dot{\mathbf{A}}_1\dot{\mathbf{A}}_2 + 2\dot{\mathbf{A}}_0\mathbf{A}_1\dot{\mathbf{A}}_2 \Big) \\[1em] \empty {}^{i-1}_{i}\mathbf{\Omega} &= \mathrm{log}\big( {}^{w}_{i-1}\mathbf{T}^{-1}~{}^{w}_{i}\mathbf{T} \big) \\ \mathbf{A}_j &= \mathrm{exp}\Big({B}_j(u(t))~{}^{i-1+j}_{i+j}\mathbf{\Omega} \Big) \\ \dot{\mathbf{A}}_j &= \dot{B}_j(u(t)) ~{}^{i-1+j}_{i+j}\mathbf{\Omega}^\wedge ~\mathbf{A}_j \\ \ddot{\mathbf{A}}_j &= \dot{B}_j(u(t)) ~{}^{i-1+j}_{i+j}\mathbf{\Omega}^\wedge ~\dot{\mathbf{A}}_j + \ddot{B}_j(u(t)) ~{}^{i-1+j}_{i+j}\mathbf{\Omega}^\wedge ~\mathbf{A}_j \\[1em] \empty {B}_0(u(t)) &= \frac{1}{3!}~(5+3u-3u^2+u^3) \\ {B}_1(u(t)) &= \frac{1}{3!}~(1+3u+3u^2-2u^3) \\ {B}_2(u(t)) &= \frac{1}{3!}~(u^3) \\[1em] \empty \dot{{B}}_0(u(t)) &= \frac{1}{3!}~\frac{1}{\Delta t}~(3-6u+3u^2) \\ \dot{{B}}_1(u(t)) &= \frac{1}{3!}~\frac{1}{\Delta t}~(3+6u-6u^2) \\ \dot{{B}}_2(u(t)) &= \frac{1}{3!}~\frac{1}{\Delta t}~(3u^2) \\[1em] \empty \ddot{{B}}_0(u(t)) &= \frac{1}{3!}~\frac{1}{\Delta t^2}~(-6+6u) \\ \ddot{{B}}_1(u(t)) &= \frac{1}{3!}~\frac{1}{\Delta t^2}~(6-12u) \\ \ddot{{B}}_2(u(t)) &= \frac{1}{3!}~\frac{1}{\Delta t^2}~(6u) \end{align*}

where $u(t_s)=(t_s-t_i)/\Delta t=(t_s-t_i)/(t_{i+1}-t_i)$ is used for all values of u. Note that one needs to ensure that they use the SE(3) matrix expodential, logorithm, and hat operation for all above equations. The indexes correspond to the the two poses that are older and two poses that are newer then the current time we want to get (i.e. i-1 and i are less than s, while i+1 and i+2 are both greater than time s). Some additional derivations are available in these notes.

Constructors, destructors, conversion operators

BsplineSE3()
Default constructor.

Public functions

void feed_trajectory(std::vector<Eigen::VectorXd> traj_points)
Will feed in a series of poses that we will then convert into control points.
auto get_pose(double timestamp, Eigen::Matrix3d& R_GtoI, Eigen::Vector3d& p_IinG) -> bool
Gets the orientation and position at a given timestamp.
auto get_velocity(double timestamp, Eigen::Matrix3d& R_GtoI, Eigen::Vector3d& p_IinG, Eigen::Vector3d& w_IinI, Eigen::Vector3d& v_IinG) -> bool
Gets the angular and linear velocity at a given timestamp.
auto get_acceleration(double timestamp, Eigen::Matrix3d& R_GtoI, Eigen::Vector3d& p_IinG, Eigen::Vector3d& w_IinI, Eigen::Vector3d& v_IinG, Eigen::Vector3d& alpha_IinI, Eigen::Vector3d& a_IinG) -> bool
Gets the angular and linear acceleration at a given timestamp.
auto get_start_time() -> double
Returns the simulation start time that we should start simulating from.

Protected types

using AlignedEigenMat4d = std::map<double, Eigen::Matrix4d, std::less<double>, Eigen::aligned_allocator<std::pair<const double, Eigen::Matrix4d>>>
Type defintion of our aligned eigen4d matrix: https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html.

Protected static functions

static auto find_bounding_poses(const double timestamp, const AlignedEigenMat4d& poses, double& t0, Eigen::Matrix4d& pose0, double& t1, Eigen::Matrix4d& pose1) -> bool
Will find the two bounding poses for a given timestamp.
static auto find_bounding_control_points(const double timestamp, const AlignedEigenMat4d& poses, double& t0, Eigen::Matrix4d& pose0, double& t1, Eigen::Matrix4d& pose1, double& t2, Eigen::Matrix4d& pose2, double& t3, Eigen::Matrix4d& pose3) -> bool
Will find two older poses and two newer poses for the current timestamp.

Protected variables

double dt
Uniform sampling time for our control points.
double timestamp_start
Start time of the system.
AlignedEigenMat4d control_points
Our control SE3 control poses (R_ItoG, p_IinG)

Function documentation

void ov_core::BsplineSE3::feed_trajectory(std::vector<Eigen::VectorXd> traj_points)

Will feed in a series of poses that we will then convert into control points.

Parameters
traj_points Trajectory poses that we will convert into control points (timestamp(s), q_GtoI, p_IinG)

Our control points need to be uniformly spaced over the trajectory, thus given a trajectory we will uniformly sample based on the average spacing between the pose points specified.

bool ov_core::BsplineSE3::get_pose(double timestamp, Eigen::Matrix3d& R_GtoI, Eigen::Vector3d& p_IinG)

Gets the orientation and position at a given timestamp.

Parameters
timestamp Desired time to get the pose at
R_GtoI SO(3) orientation of the pose in the global frame
p_IinG Position of the pose in the global
Returns False if we can't find it

bool ov_core::BsplineSE3::get_velocity(double timestamp, Eigen::Matrix3d& R_GtoI, Eigen::Vector3d& p_IinG, Eigen::Vector3d& w_IinI, Eigen::Vector3d& v_IinG)

Gets the angular and linear velocity at a given timestamp.

Parameters
timestamp Desired time to get the pose at
R_GtoI SO(3) orientation of the pose in the global frame
p_IinG Position of the pose in the global
w_IinI Angular velocity in the inertial frame
v_IinG Linear velocity in the global frame
Returns False if we can't find it

bool ov_core::BsplineSE3::get_acceleration(double timestamp, Eigen::Matrix3d& R_GtoI, Eigen::Vector3d& p_IinG, Eigen::Vector3d& w_IinI, Eigen::Vector3d& v_IinG, Eigen::Vector3d& alpha_IinI, Eigen::Vector3d& a_IinG)

Gets the angular and linear acceleration at a given timestamp.

Parameters
timestamp Desired time to get the pose at
R_GtoI SO(3) orientation of the pose in the global frame
p_IinG Position of the pose in the global
w_IinI Angular velocity in the inertial frame
v_IinG Linear velocity in the global frame
alpha_IinI Angular acceleration in the inertial frame
a_IinG Linear acceleration in the global frame
Returns False if we can't find it

static bool ov_core::BsplineSE3::find_bounding_poses(const double timestamp, const AlignedEigenMat4d& poses, double& t0, Eigen::Matrix4d& pose0, double& t1, Eigen::Matrix4d& pose1) protected

Will find the two bounding poses for a given timestamp.

Parameters
timestamp Desired timestamp we want to get two bounding poses of
poses Map of poses and timestamps
t0 Timestamp of the first pose
pose0 SE(3) pose of the first pose
t1 Timestamp of the second pose
pose1 SE(3) pose of the second pose
Returns False if we are unable to find bounding poses

This will loop through the passed map of poses and find two bounding poses. If there are no bounding poses then this will return false.

static bool ov_core::BsplineSE3::find_bounding_control_points(const double timestamp, const AlignedEigenMat4d& poses, double& t0, Eigen::Matrix4d& pose0, double& t1, Eigen::Matrix4d& pose1, double& t2, Eigen::Matrix4d& pose2, double& t3, Eigen::Matrix4d& pose3) protected

Will find two older poses and two newer poses for the current timestamp.

Parameters
timestamp Desired timestamp we want to get four bounding poses of
poses Map of poses and timestamps
t0 Timestamp of the first pose
pose0 SE(3) pose of the first pose
t1 Timestamp of the second pose
pose1 SE(3) pose of the second pose
t2 Timestamp of the third pose
pose2 SE(3) pose of the third pose
t3 Timestamp of the fourth pose
pose3 SE(3) pose of the fourth pose
Returns False if we are unable to find bounding poses