ov_core namespace

Core algorithms for OpenVINS.

Contents

This has the core algorithms that all projects within the OpenVINS 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:

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. If you are looking for the different types please take a look at the ov_type namespace for the ones we have.

Classes

class BsplineSE3
B-Spline which performs interpolation over SE(3) manifold.
class CamBase
Base pinhole camera model class.
class CamEqui
Fisheye / equadistant model pinhole camera model class.
struct CameraData
Struct for a collection of camera measurements.
class CamRadtan
Radial-tangential / Brown–Conrady model pinhole camera model class.
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 FeatureHelper
Contains some nice helper functions for features.
class FeatureInitializer
Class that triangulates feature.
struct FeatureInitializerOptions
Struct which stores all our feature initializer options.
class Grider_FAST
Extracts FAST features in a grid pattern.
class Grider_GRID
Extracts FAST features in a grid pattern.
struct ImuData
Struct for a single imu measurement (time, wm, am)
class Printer
Printer for open_vins that allows for various levels of printing to be done.
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 YamlParser
Helper class to do OpenCV yaml parsing from both file and ROS.

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(const Eigen::Matrix<double, 3, 1>& w) -> Eigen::Matrix<double, 3, 3>
Computes left Jacobian of SO(3)
auto Jr_so3(const Eigen::Matrix<double, 3, 1>& w) -> Eigen::Matrix<double, 3, 3>
Computes right Jacobian of SO(3)
auto rot2rpy(const Eigen::Matrix<double, 3, 3>& rot) -> Eigen::Matrix<double, 3, 1>
Gets roll, pitch, yaw of argument rotation (in that order).
auto rot_x(double t) -> Eigen::Matrix<double, 3, 3>
Construct rotation matrix from given roll.
auto rot_y(double t) -> Eigen::Matrix<double, 3, 3>
Construct rotation matrix from given pitch.
auto rot_z(double t) -> Eigen::Matrix<double, 3, 3>
Construct rotation matrix from given yaw.

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}\mathbf{q}^\top \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 q*p 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 in R(3) 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 R(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*}

This function is based on the GTSAM one as the original implementation was a bit unstable. See the following:

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

SE(3) matrix exponential function.

Parameters
vec 6x1 in the R(6) 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 R(6) 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*}

This function is based on the GTSAM one as the original implementation was a bit unstable. See the following:

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 R(6) 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.

Parameters
w Angular velocity
Returns The matrix $\boldsymbol{\Omega}$

See equation (48) of trawny tech report Indirect Kalman Filter for 3D Attitude Estimation. This matrix is derived in Section 1.5 of the report by finding the Quaterion Time Derivative.

\begin{align*} \boldsymbol{\Omega}(\boldsymbol{\omega}) &= \begin{bmatrix} -\lfloor{\boldsymbol{\omega}} \rfloor & \boldsymbol{\omega} \\ -\boldsymbol{\omega}^\top & 0 \end{bmatrix} \end{align*}

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(const 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 [3]. 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(const 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 [3]. See Jl_so3() for the definition of the left Jacobian of SO(3).

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

Gets roll, pitch, yaw of argument rotation (in that order).

Parameters
rot SO(3) rotation matrix
Returns roll, pitch, yaw values (in that order)

To recover the matrix: R_input = R_z(yaw)*R_y(pitch)*R_x(roll) If you are interested in how to compute Jacobians checkout this report: http://mars.cs.umn.edu/tr/reports/Trawny05c.pdf

Eigen::Matrix<double, 3, 3> ov_core::rot_x(double t)

Construct rotation matrix from given roll.

Parameters
t roll angle
Returns SO(3) rotation matrix
\begin{align*} \mathbf{R}_x(t) &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(t) & -\sin(t) \\ 0 & \sin(t) & \cos(t) \end{bmatrix} \end{align*}

Eigen::Matrix<double, 3, 3> ov_core::rot_y(double t)

Construct rotation matrix from given pitch.

Parameters
t pitch angle
Returns SO(3) rotation matrix
\begin{align*} \mathbf{R}_y(t) &= \begin{bmatrix} \cos(t) & 0 & \sin(t) \\ 0 & 1 & 0 \\ -\sin(t) & 0 & \cos(t) \end{bmatrix} \end{align*}

Eigen::Matrix<double, 3, 3> ov_core::rot_z(double t)

Construct rotation matrix from given yaw.

Parameters
t yaw angle
Returns SO(3) rotation matrix
\begin{align*} \mathbf{R}_z(t) &= \begin{bmatrix} \cos(t) & -\sin(t) & 0 \\ \sin(t) & \cos(t) & 0 \\ 0 & 0 & 1 \end{bmatrix} \end{align*}