Public Member Functions | Protected Types | Static Protected Member Functions | Protected Attributes | List of all members
ov_core::BsplineSE3 Class Reference

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

#include <BsplineSE3.h>

Public Member Functions

 BsplineSE3 ()
 Default constructor. More...
 
void feed_trajectory (std::vector< Eigen::VectorXd > traj_points)
 Will feed in a series of poses that we will then convert into control points. More...
 
bool 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. More...
 
bool get_pose (double timestamp, Eigen::Matrix3d &R_GtoI, Eigen::Vector3d &p_IinG)
 Gets the orientation and position at a given timestamp. More...
 
double get_start_time ()
 Returns the simulation start time that we should start simulating from. More...
 
bool 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. More...
 

Protected Types

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

Static Protected Member Functions

static bool 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)
 Will find two older poses and two newer poses for the current timestamp. More...
 
static bool find_bounding_poses (const double timestamp, const AlignedEigenMat4d &poses, double &t0, Eigen::Matrix4d &pose0, double &t1, Eigen::Matrix4d &pose1)
 Will find the two bounding poses for a given timestamp. More...
 

Protected Attributes

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

Detailed Description

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

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.

Definition at line 104 of file BsplineSE3.h.

Member Typedef Documentation

◆ AlignedEigenMat4d

typedef std::map<double, Eigen::Matrix4d, std::less<double>, Eigen::aligned_allocator<std::pair<const double, Eigen::Matrix4d> > > ov_core::BsplineSE3::AlignedEigenMat4d
protected

Type defintion of our aligned eigen4d matrix: https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html.

Definition at line 168 of file BsplineSE3.h.

Constructor & Destructor Documentation

◆ BsplineSE3()

ov_core::BsplineSE3::BsplineSE3 ( )
inline

Default constructor.

Definition at line 110 of file BsplineSE3.h.

Member Function Documentation

◆ feed_trajectory()

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

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

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.

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

Definition at line 26 of file BsplineSE3.cpp.

◆ find_bounding_control_points()

bool 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 
)
staticprotected

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

Parameters
timestampDesired timestamp we want to get four bounding poses of
posesMap of poses and timestamps
t0Timestamp of the first pose
pose0SE(3) pose of the first pose
t1Timestamp of the second pose
pose1SE(3) pose of the second pose
t2Timestamp of the third pose
pose2SE(3) pose of the third pose
t3Timestamp of the fourth pose
pose3SE(3) pose of the fourth pose
Returns
False if we are unable to find bounding poses

Definition at line 301 of file BsplineSE3.cpp.

◆ find_bounding_poses()

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

Will find the two bounding poses for a given timestamp.

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.

Parameters
timestampDesired timestamp we want to get two bounding poses of
posesMap of poses and timestamps
t0Timestamp of the first pose
pose0SE(3) pose of the first pose
t1Timestamp of the second pose
pose1SE(3) pose of the second pose
Returns
False if we are unable to find bounding poses

Definition at line 249 of file BsplineSE3.cpp.

◆ get_acceleration()

bool 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
timestampDesired time to get the pose at
R_GtoISO(3) orientation of the pose in the global frame
p_IinGPosition of the pose in the global
w_IinIAngular velocity in the inertial frame
v_IinGLinear velocity in the global frame
alpha_IinIAngular acceleration in the inertial frame
a_IinGLinear acceleration in the global frame
Returns
False if we can't find it

Definition at line 180 of file BsplineSE3.cpp.

◆ get_pose()

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

Gets the orientation and position at a given timestamp.

Parameters
timestampDesired time to get the pose at
R_GtoISO(3) orientation of the pose in the global frame
p_IinGPosition of the pose in the global
Returns
False if we can't find it

Definition at line 92 of file BsplineSE3.cpp.

◆ get_start_time()

double ov_core::BsplineSE3::get_start_time ( )
inline

Returns the simulation start time that we should start simulating from.

Definition at line 157 of file BsplineSE3.h.

◆ get_velocity()

bool 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
timestampDesired time to get the pose at
R_GtoISO(3) orientation of the pose in the global frame
p_IinGPosition of the pose in the global
w_IinIAngular velocity in the inertial frame
v_IinGLinear velocity in the global frame
Returns
False if we can't find it

Definition at line 127 of file BsplineSE3.cpp.

Member Data Documentation

◆ control_points

AlignedEigenMat4d ov_core::BsplineSE3::control_points
protected

Our control SE3 control poses (R_ItoG, p_IinG)

Definition at line 171 of file BsplineSE3.h.

◆ dt

double ov_core::BsplineSE3::dt
protected

Uniform sampling time for our control points.

Definition at line 161 of file BsplineSE3.h.

◆ timestamp_start

double ov_core::BsplineSE3::timestamp_start
protected

Start time of the system.

Definition at line 164 of file BsplineSE3.h.

The documentation for this class was generated from the following files:

ov_core
Author(s): Patrick Geneva , Kevin Eckenhoff , Guoquan Huang
autogenerated on Mon Dec 16 2024 03:06:46