# class

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

### Contents

This class implements the b-spline functionality that allows for interpolation over the 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 interpolation has the following properties:

- Local control, allowing the system to function online as well as in batch
- -continuity to enable inertial predictions and calculations
- Good approximation of minimal torque trajectories
- 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 -continuity to ensure that we can calculate accelerations at any point along the trajectory. The general equations are the following

where 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 |