ov_core namespace

Core algorithms for Open VINS.

Contents

This has the core algorithms that all projects within the Open VINS ecosystem leverage. The purpose is to allow for the reuse of code that could be shared between different localization systems (i.e. msckf-based, batch-based, etc.). These algorithms are the foundation which is necessary before we can even write an estimator that can perform localization. The key components of the ov_core codebase are the following:

  • Closed-form preintegration [3]
  • 3d feature initialization
  • Inertial state initialization
  • Visual-inertial simulator and SE(3) b-spline
  • KLT, descriptor, aruco, and simulation feature trackers
  • Groundtruth dataset reader
  • Quaternion and other manifold math operations
  • Generic type system and their implementations

Please take a look at classes that we offer for the user to leverage as each has its own documentation. If you are looking for the estimator please take a look at the ov_msckf project which leverages these algorithms.

Classes

class BsplineSE3
B-Spline which performs interpolation over SE(3) manifold.
class CpiBase
Base class for continuous preintegration integrators.
class CpiV1
Model 1 of continuous preintegration.
class CpiV2
Model 2 of continuous preintegration.
class DatasetReader
Helper functions to read in dataset files.
class Feature
Sparse feature class used to collect measurements.
class FeatureDatabase
Database containing features we are currently tracking.
class FeatureInitializer
Class that triangulates feature.
struct FeatureInitializerOptions
Struct which stores all our feature initializer options.
class FeatureRepresentation
Class has useful feature representation types.
class Grider_DOG
Does Difference of Gaussian (DoG) in a grid pattern.
class Grider_FAST
Extracts FAST features in a grid pattern.
class IMU
Derived Type class that implements an IMU state.
class InertialInitializer
Initializer for visual-inertial system.
class JPLQuat
Derived Type class that implements JPL quaternion.
class Landmark
Type that implements a persistant SLAM feature.
class PoseJPL
Derived Type class that implements a 6 d.o.f pose.
class Simulator
Master simulator class that will create artificial measurements for our visual-inertial algorithms.
class TrackAruco
Tracking of OpenCV Aruoc tags.
class TrackBase
Visual feature tracking base class.
class TrackDescriptor
Descriptor-based visual tracking.
class TrackKLT
KLT tracking of features.
class TrackSIM
Simulated tracker for when we already have uv measurements!
class Type
Base class for estimated variables.
class Vec
Derived Type class that implements vector variables.

Functions

auto rot_2_quat(const Eigen::Matrix<double, 3, 3>& rot) -> Eigen::Matrix<double, 4, 1>
Returns a JPL quaternion from a rotation matrix.
auto skew_x(const Eigen::Matrix<double, 3, 1>& w) -> Eigen::Matrix<double, 3, 3>
Skew-symmetric matrix from a given 3x1 vector.
auto quat_2_Rot(const Eigen::Matrix<double, 4, 1>& q) -> Eigen::Matrix<double, 3, 3>
Converts JPL quaterion to SO(3) rotation matrix.
auto quat_multiply(const Eigen::Matrix<double, 4, 1>& q, const Eigen::Matrix<double, 4, 1>& p) -> Eigen::Matrix<double, 4, 1>
Multiply two JPL quaternions.
auto vee(const Eigen::Matrix<double, 3, 3>& w_x) -> Eigen::Matrix<double, 3, 1>
Returns vector portion of skew-symmetric.
auto exp_so3(const Eigen::Matrix<double, 3, 1>& w) -> Eigen::Matrix<double, 3, 3>
SO(3) matrix exponential.
auto log_so3(const Eigen::Matrix<double, 3, 3>& R) -> Eigen::Matrix<double, 3, 1>
SO(3) matrix logarithm.
auto exp_se3(Eigen::Matrix<double, 6, 1> vec) -> Eigen::Matrix4d
SE(3) matrix exponential function.
auto log_se3(Eigen::Matrix4d mat) -> Eigen::Matrix<double, 6, 1>
SE(3) matrix logarithm.
auto hat_se3(const Eigen::Matrix<double, 6, 1>& vec) -> Eigen::Matrix4d
Hat operator for R^6 -> Lie Algebra se(3)
auto Inv_se3(const Eigen::Matrix4d& T) -> Eigen::Matrix4d
SE(3) matrix analytical inverse.
auto Inv(Eigen::Matrix<double, 4, 1> q) -> Eigen::Matrix<double, 4, 1>
JPL Quaternion inverse.
auto Omega(Eigen::Matrix<double, 3, 1> w) -> Eigen::Matrix<double, 4, 4>
Integrated quaternion from angular velocity.
auto quatnorm(Eigen::Matrix<double, 4, 1> q_t) -> Eigen::Matrix<double, 4, 1>
Normalizes a quaternion to make sure it is unit norm.
auto Jl_so3(Eigen::Matrix<double, 3, 1> w) -> Eigen::Matrix<double, 3, 3>
Computes left Jacobian of SO(3)
auto Jr_so3(Eigen::Matrix<double, 3, 1> w) -> Eigen::Matrix<double, 3, 3>
Computes right Jacobian of SO(3)

Function documentation

Eigen::Matrix<double, 4, 1> ov_core::rot_2_quat(const Eigen::Matrix<double, 3, 3>& rot)

Returns a JPL quaternion from a rotation matrix.

Parameters
rot in 3x3 rotation matrix
Returns 4x1 quaternion

This is based on the equation 74 in Indirect Kalman Filter for 3D Attitude Estimation. In the implementation, we have 4 statements so that we avoid a division by zero and instead always divide by the largest diagonal element. This all comes from the definition of a rotation matrix, using the diagonal elements and an off-diagonal.

\begin{align*} \mathbf{R}(\bar{q})= \begin{bmatrix} q_1^2-q_2^2-q_3^2+q_4^2 & 2(q_1q_2+q_3q_4) & 2(q_1q_3-q_2q_4) \\ 2(q_1q_2-q_3q_4) & -q_2^2+q_2^2-q_3^2+q_4^2 & 2(q_2q_3+q_1q_4) \\ 2(q_1q_3+q_2q_4) & 2(q_2q_3-q_1q_4) & -q_1^2-q_2^2+q_3^2+q_4^2 \end{bmatrix} \end{align*}

Eigen::Matrix<double, 3, 3> ov_core::skew_x(const Eigen::Matrix<double, 3, 1>& w)

Skew-symmetric matrix from a given 3x1 vector.

Parameters
w in 3x1 vector to be made a skew-symmetric
Returns 3x3 skew-symmetric matrix

This is based on equation 6 in Indirect Kalman Filter for 3D Attitude Estimation:

\begin{align*} \lfloor\mathbf{v}\times\rfloor = \begin{bmatrix} 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 \end{bmatrix} \end{align*}

Eigen::Matrix<double, 3, 3> ov_core::quat_2_Rot(const Eigen::Matrix<double, 4, 1>& q)

Converts JPL quaterion to SO(3) rotation matrix.

Parameters
q in JPL quaternion
Returns 3x3 SO(3) rotation matrix

This is based on equation 62 in Indirect Kalman Filter for 3D Attitude Estimation:

\begin{align*} \mathbf{R} = (2q_4^2-1)\mathbf{I}_3-2q_4\lfloor\mathbf{q}\times\rfloor+2\mathbf{q}^\top\mathbf{q} \end{align*}

Eigen::Matrix<double, 4, 1> ov_core::quat_multiply(const Eigen::Matrix<double, 4, 1>& q, const Eigen::Matrix<double, 4, 1>& p)

Multiply two JPL quaternions.

Parameters
q in First JPL quaternion
p in Second JPL quaternion
Returns 4x1 resulting p*q quaternion

This is based on equation 9 in Indirect Kalman Filter for 3D Attitude Estimation. We also enforce that the quaternion is unique by having q_4 be greater than zero.

\begin{align*} \bar{q}\otimes\bar{p}= \mathcal{L}(\bar{q})\bar{p}= \begin{bmatrix} q_4\mathbf{I}_3+\lfloor\mathbf{q}\times\rfloor & \mathbf{q} \\ -\mathbf{q}^\top & q_4 \end{bmatrix} \begin{bmatrix} \mathbf{p} \\ p_4 \end{bmatrix} \end{align*}

Eigen::Matrix<double, 3, 1> ov_core::vee(const Eigen::Matrix<double, 3, 3>& w_x)

Returns vector portion of skew-symmetric.

Parameters
w_x in skew-symmetric matrix
Returns 3x1 vector portion of skew

See skew_x() for details.

Eigen::Matrix<double, 3, 3> ov_core::exp_so3(const Eigen::Matrix<double, 3, 1>& w)

SO(3) matrix exponential.

Parameters
w in 3x1 vector we will take the exponential of
Returns SO(3) rotation matrix

SO(3) matrix exponential mapping from the vector to SO(3) lie group. This formula ends up being the Rodrigues formula. This definition was taken from "Lie Groups for 2D and 3D Transformations" by Ethan Eade equation 15. http://ethaneade.com/lie.pdf

\begin{align*} \exp\colon\mathfrak{so}(3)&\to SO(3) \\ \exp(\mathbf{v}) &= \mathbf{I} +\frac{\sin{\theta}}{\theta}\lfloor\mathbf{v}\times\rfloor +\frac{1-\cos{\theta}}{\theta^2}\lfloor\mathbf{v}\times\rfloor^2 \\ \mathrm{where}&\quad \theta^2 = \mathbf{v}^\top\mathbf{v} \end{align*}

Eigen::Matrix<double, 3, 1> ov_core::log_so3(const Eigen::Matrix<double, 3, 3>& R)

SO(3) matrix logarithm.

Parameters
R in 3x3 SO(3) rotation matrix
Returns 3x1 in the se(3) space [omegax, omegay, omegaz]

This definition was taken from "Lie Groups for 2D and 3D Transformations" by Ethan Eade equation 17 & 18. http://ethaneade.com/lie.pdf

\begin{align*} \theta &= \textrm{arccos}(0.5(\textrm{trace}(\mathbf{R})-1)) \\ \lfloor\mathbf{v}\times\rfloor &= \frac{\theta}{2\sin{\theta}}(\mathbf{R}-\mathbf{R}^\top) \end{align*}

Eigen::Matrix4d ov_core::exp_se3(Eigen::Matrix<double, 6, 1> vec)

SE(3) matrix exponential function.

Parameters
vec 6x1 in the se(3) space [omega, u]
Returns 4x4 SE(3) matrix

Equation is from Ethan Eade's reference: http://ethaneade.com/lie.pdf

\begin{align*} \exp([\boldsymbol\omega,\mathbf u])&=\begin{bmatrix} \mathbf R & \mathbf V \mathbf u \\ \mathbf 0 & 1 \end{bmatrix} \\[1em] \mathbf R &= \mathbf I + A \lfloor \boldsymbol\omega \times\rfloor + B \lfloor \boldsymbol\omega \times\rfloor^2 \\ \mathbf V &= \mathbf I + B \lfloor \boldsymbol\omega \times\rfloor + C \lfloor \boldsymbol\omega \times\rfloor^2 \end{align*}

where we have the following definitions

\begin{align*} \theta &= \sqrt{\boldsymbol\omega^\top\boldsymbol\omega} \\ A &= \sin\theta/\theta \\ B &= (1-\cos\theta)/\theta^2 \\ C &= (1-A)/\theta^2 \end{align*}

Eigen::Matrix<double, 6, 1> ov_core::log_se3(Eigen::Matrix4d mat)

SE(3) matrix logarithm.

Parameters
mat 4x4 SE(3) matrix
Returns 6x1 in the se(3) space [omega, u]

Equation is from Ethan Eade's reference: http://ethaneade.com/lie.pdf

\begin{align*} \boldsymbol\omega &=\mathrm{skew\_offdiags}\Big(\frac{\theta}{2\sin\theta}(\mathbf R - \mathbf R^\top)\Big) \\ \mathbf u &= \mathbf V^{-1}\mathbf t \end{align*}

where we have the following definitions

\begin{align*} \theta &= \mathrm{arccos}((\mathrm{tr}(\mathbf R)-1)/2) \\ \mathbf V^{-1} &= \mathbf I - \frac{1}{2} \lfloor \boldsymbol\omega \times\rfloor + \frac{1}{\theta^2}\Big(1-\frac{A}{2B}\Big)\lfloor \boldsymbol\omega \times\rfloor^2 \end{align*}

Eigen::Matrix4d ov_core::hat_se3(const Eigen::Matrix<double, 6, 1>& vec)

Hat operator for R^6 -> Lie Algebra se(3)

Parameters
vec 6x1 in the se(3) space [omega, u]
Returns Lie algebra se(3) 4x4 matrix
\begin{align*} \boldsymbol\Omega^{\wedge} = \begin{bmatrix} \lfloor \boldsymbol\omega \times\rfloor & \mathbf u \\ \mathbf 0 & 0 \end{bmatrix} \end{align*}

Eigen::Matrix4d ov_core::Inv_se3(const Eigen::Matrix4d& T)

SE(3) matrix analytical inverse.

Parameters
T in SE(3) matrix
Returns inversed SE(3) matrix

It seems that using the .inverse() function is not a good way. This should be used in all cases we need the inverse instead of numerical inverse. https://github.com/rpng/open_vins/issues/12

\begin{align*} \mathbf{T}^{-1} = \begin{bmatrix} \mathbf{R}^\top & -\mathbf{R}^\top\mathbf{p} \\ \mathbf{0} & 1 \end{bmatrix} \end{align*}

Eigen::Matrix<double, 4, 1> ov_core::Inv(Eigen::Matrix<double, 4, 1> q)

JPL Quaternion inverse.

Parameters
q in quaternion we want to change
Returns inversed quaternion

See equation 21 in Indirect Kalman Filter for 3D Attitude Estimation.

\begin{align*} \bar{q}^{-1} = \begin{bmatrix} -\mathbf{q} \\ q_4 \end{bmatrix} \end{align*}

Eigen::Matrix<double, 4, 4> ov_core::Omega(Eigen::Matrix<double, 3, 1> w)

Integrated quaternion from angular velocity.

See equation (48) of trawny tech report Indirect Kalman Filter for 3D Attitude Estimation.

Eigen::Matrix<double, 4, 1> ov_core::quatnorm(Eigen::Matrix<double, 4, 1> q_t)

Normalizes a quaternion to make sure it is unit norm.

Parameters
q_t Quaternion to normalized
Returns Normalized quaterion

Eigen::Matrix<double, 3, 3> ov_core::Jl_so3(Eigen::Matrix<double, 3, 1> w)

Computes left Jacobian of SO(3)

Parameters
w axis-angle
Returns The left Jacobian of SO(3)

The left Jacobian of SO(3) is defined equation (7.77b) in State Estimation for Robotics by Timothy D. Barfoot. Specifically it is the following (with $\theta=|\boldsymbol\theta|$ and $\mathbf a=\boldsymbol\theta/|\boldsymbol\theta|$ ):

\begin{align*} J_l(\boldsymbol\theta) = \frac{\sin\theta}{\theta}\mathbf I + \Big(1-\frac{\sin\theta}{\theta}\Big)\mathbf a \mathbf a^\top + \frac{1-\cos\theta}{\theta}\lfloor \mathbf a \times\rfloor \end{align*}

Eigen::Matrix<double, 3, 3> ov_core::Jr_so3(Eigen::Matrix<double, 3, 1> w)

Computes right Jacobian of SO(3)

Parameters
w axis-angle
Returns The right Jacobian of SO(3)

The right Jacobian of SO(3) is related to the left by Jl(-w)=Jr(w). See equation (7.87) in State Estimation for Robotics by Timothy D. Barfoot. See Jl_so3() for the definition of the left Jacobian of SO(3).