navlie.bspline¶
A B-Spline on SE(3), useful for generating simulated trajectories in 3D space and interoceptive measurements (like IMU measurements) along the trajectory.
This is essentially a Python implementation of the C++ code found in OpenVINS here: https://github.com/rpng/open_vins/blob/master/ov_core/src/sim/BsplineSE3.h
The core idea is to utilize cubic B-splines to interpolate between a set of control point poses. Denoting \(\mathbf{T}_{ab} \in SE(3)\) as the vehicle pose, the B-Spline is parameterized by an evenly spaced set of control points \(\mathbf{T}_{ab_i}\). The value of the spline curve at any time \(t\) is purely a function of four bounding control points, such that
\[\mathbf{T}_{ab} (t) = \exp \left( \tilde{B}_{0, 4} (t) \log \left(\mathbf{T}_{ab_0} \right) \right) \prod_{n=1}^3 \exp \left( \tilde{B}_{i, 4} \log \left(\mathbf{T}_{ab_{i-1}}^{-1} \mathbf{T}_{ab_i} \right) \right)\]
where \(\tilde{B}_{i, 4}\) are the cumulative basis functions. For detailed derivations for the key equations behind the B-Spline simulator, see
E. Mueggler, G. Gallego, H. Rebecq, and D. Scaramuzza. Continuous-Time Visual-Inertial Odometry for Event Cameras. IEEE Transactions on Robotics, pages 1–16, 2018.
Classes
|
Creates a B-Spline on SE(3) from a list of SE3State poses. |