Core algorithms for OpenVINS. More...

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

class	CamBase
	Base pinhole camera model class. More...

class	CamEqui
	Fisheye / equadistant model pinhole camera model class. More...

struct	CameraData
	Struct for a collection of camera measurements. More...

class	CamRadtan
	Radial-tangential / Brown–Conrady model pinhole camera model class. More...

class	CpiBase
	Base class for continuous preintegration integrators. More...

class	CpiV1
	Model 1 of continuous preintegration. More...

class	CpiV2
	Model 2 of continuous preintegration. More...

class	DatasetReader
	Helper functions to read in dataset files. More...

class	Feature
	Sparse feature class used to collect measurements. More...

class	FeatureDatabase
	Database containing features we are currently tracking. More...

class	FeatureHelper
	Contains some nice helper functions for features. More...

class	FeatureInitializer
	Class that triangulates feature. More...

struct	FeatureInitializerOptions
	Struct which stores all our feature initializer options. More...

class	Grider_FAST
	Extracts FAST features in a grid pattern. More...

class	Grider_GRID
	Extracts FAST features in a grid pattern. More...

struct	ImuData
	Struct for a single imu measurement (time, wm, am) More...

class	LambdaBody

class	Printer
	Printer for open_vins that allows for various levels of printing to be done. More...

class	TrackAruco
	Tracking of OpenCV Aruoc tags. More...

class	TrackBase
	Visual feature tracking base class. More...

class	TrackDescriptor
	Descriptor-based visual tracking. More...

class	TrackKLT
	KLT tracking of features. More...

class	TrackSIM
	Simulated tracker for when we already have uv measurements! More...

class	YamlParser
	Helper class to do OpenCV yaml parsing from both file and ROS. More...

Functions
Eigen::Matrix4d	exp_se3 (Eigen::Matrix< double, 6, 1 > vec)
	SE(3) matrix exponential function. More...

Eigen::Matrix< double, 3, 3 >	exp_so3 (const Eigen::Matrix< double, 3, 1 > &w)
	SO(3) matrix exponential. More...

Eigen::Matrix4d	hat_se3 (const Eigen::Matrix< double, 6, 1 > &vec)
	Hat operator for R^6 -> Lie Algebra se(3) More...

Eigen::Matrix< double, 4, 1 >	Inv (Eigen::Matrix< double, 4, 1 > q)
	JPL Quaternion inverse. More...

Eigen::Matrix4d	Inv_se3 (const Eigen::Matrix4d &T)
	SE(3) matrix analytical inverse. More...

Eigen::Matrix< double, 3, 3 >	Jl_so3 (const Eigen::Matrix< double, 3, 1 > &w)
	Computes left Jacobian of SO(3) More...

Eigen::Matrix< double, 3, 3 >	Jr_so3 (const Eigen::Matrix< double, 3, 1 > &w)
	Computes right Jacobian of SO(3) More...

Eigen::Matrix< double, 6, 1 >	log_se3 (Eigen::Matrix4d mat)
	SE(3) matrix logarithm. More...

Eigen::Matrix< double, 3, 1 >	log_so3 (const Eigen::Matrix< double, 3, 3 > &R)
	SO(3) matrix logarithm. More...

Eigen::Matrix< double, 4, 4 >	Omega (Eigen::Matrix< double, 3, 1 > w)
	Integrated quaternion from angular velocity. More...

Eigen::Matrix< double, 3, 3 >	quat_2_Rot (const Eigen::Matrix< double, 4, 1 > &q)
	Converts JPL quaterion to SO(3) rotation matrix. More...

Eigen::Matrix< double, 4, 1 >	quat_multiply (const Eigen::Matrix< double, 4, 1 > &q, const Eigen::Matrix< double, 4, 1 > &p)
	Multiply two JPL quaternions. More...

Eigen::Matrix< double, 4, 1 >	quatnorm (Eigen::Matrix< double, 4, 1 > q_t)
	Normalizes a quaternion to make sure it is unit norm. More...

Eigen::Matrix< double, 3, 1 >	rot2rpy (const Eigen::Matrix< double, 3, 3 > &rot)
	Gets roll, pitch, yaw of argument rotation (in that order). More...

Eigen::Matrix< double, 4, 1 >	rot_2_quat (const Eigen::Matrix< double, 3, 3 > &rot)
	Returns a JPL quaternion from a rotation matrix. More...

Eigen::Matrix< double, 3, 3 >	rot_x (double t)
	Construct rotation matrix from given roll. More...

Eigen::Matrix< double, 3, 3 >	rot_y (double t)
	Construct rotation matrix from given pitch. More...

Eigen::Matrix< double, 3, 3 >	rot_z (double t)
	Construct rotation matrix from given yaw. More...

Eigen::Matrix< double, 3, 3 >	skew_x (const Eigen::Matrix< double, 3, 1 > &w)
	Skew-symmetric matrix from a given 3x1 vector. More...

Eigen::Matrix< double, 3, 1 >	vee (const Eigen::Matrix< double, 3, 3 > &w_x)
	Returns vector portion of skew-symmetric. More...

Detailed Description

Core algorithms for OpenVINS.

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:

3d feature initialization (see ov_core::FeatureInitializer)
SE(3) b-spline (see ov_core::BsplineSE3)
KLT, descriptor, aruco, and simulation feature trackers
Groundtruth dataset reader (see ov_core::DatasetReader)
Quaternion and other manifold math operations
Generic type system and their implementations (see ov_type namespace)
Closed-form preintegration [Eckenhoff2019IJRR]

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.

Function Documentation

◆ exp_se3()

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

inline

SE(3) matrix exponential function.

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*}$

Parameters

vec	6x1 in the R(6) space [omega, u]

Returns: 4x4 SE(3) matrix

Definition at line 332 of file quat_ops.h.

◆ exp_so3()

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

inline

SO(3) matrix exponential.

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*}$

Parameters

[in] w 3x1 vector in R(3) we will take the exponential of

Returns: SO(3) rotation matrix

Definition at line 231 of file quat_ops.h.

◆ hat_se3()

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

inline

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

$\begin{align*} \boldsymbol\Omega^{\wedge} = \begin{bmatrix} \lfloor \boldsymbol\omega \times\rfloor & \mathbf u \\ \mathbf 0 & 0 \end{bmatrix} \end{align*}$

Parameters

vec	6x1 in the R(6) space [omega, u]

Returns: Lie algebra se(3) 4x4 matrix

Definition at line 419 of file quat_ops.h.

◆ Inv()

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

inline

JPL Quaternion inverse.

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*}$

Parameters

[in] q quaternion we want to change

Returns: inversed quaternion

Definition at line 457 of file quat_ops.h.

◆ Inv_se3()

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

inline

SE(3) matrix analytical inverse.

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*}$

Parameters

[in] T SE(3) matrix

Returns: inversed SE(3) matrix

Definition at line 439 of file quat_ops.h.

◆ Jl_so3()

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

inline

Computes 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 [Barfoot2017]. 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*}$

Parameters

w	axis-angle

Returns: The left Jacobian of SO(3)

Definition at line 515 of file quat_ops.h.

◆ Jr_so3()

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

inline

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

Parameters

w	axis-angle

Returns: The right Jacobian of SO(3)

Definition at line 537 of file quat_ops.h.

◆ log_se3()

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

inline

SE(3) matrix logarithm.

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:

Parameters

mat	4x4 SE(3) matrix

Returns: 6x1 in the R(6) space [omega, u]

Definition at line 388 of file quat_ops.h.

◆ log_so3()

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

inline

SO(3) matrix logarithm.

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:

Parameters

[in] R 3x3 SO(3) rotation matrix

Returns: 3x1 in the R(3) space [omegax, omegay, omegaz]

Definition at line 273 of file quat_ops.h.

◆ Omega()

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

inline

Integrated quaternion from angular velocity.

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*}$

Parameters

w	Angular velocity

Returns: The matrix $\boldsymbol{\Omega}$

Definition at line 482 of file quat_ops.h.

◆ quat_2_Rot()

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

inline

Converts JPL quaterion to 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*}$

Parameters

[in] q JPL quaternion

Returns: 3x3 SO(3) rotation matrix

Definition at line 152 of file quat_ops.h.

◆ quat_multiply()

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

inline

Multiply two JPL quaternions.

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*}$

Parameters

[in]	q	First JPL quaternion
[in]	p	Second JPL quaternion

Returns: 4x1 resulting q*p quaternion

Definition at line 180 of file quat_ops.h.

◆ quatnorm()

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

inline

Normalizes a quaternion to make sure it is unit norm.

Parameters

q_t	Quaternion to normalized

Returns: Normalized quaterion

Definition at line 496 of file quat_ops.h.

◆ rot2rpy()

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

inline

Gets roll, pitch, yaw of argument rotation (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

Parameters

rot	SO(3) rotation matrix

Returns: roll, pitch, yaw values (in that order)

Definition at line 549 of file quat_ops.h.

◆ rot_2_quat()

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

inline

Returns a JPL quaternion from a rotation matrix.

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*}$

Parameters

[in] rot 3x3 rotation matrix

Returns: 4x1 quaternion

Definition at line 88 of file quat_ops.h.

◆ rot_x()

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

inline

Construct rotation matrix from given roll.

$\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*}$

Parameters

t	roll angle

Returns: SO(3) rotation matrix

Definition at line 577 of file quat_ops.h.

◆ rot_y()

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

inline

Construct rotation matrix from given pitch.

$\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*}$

Parameters

t	pitch angle

Returns: SO(3) rotation matrix

Definition at line 600 of file quat_ops.h.

◆ rot_z()

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

inline

Construct rotation matrix from given yaw.

$\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*}$

Parameters

t yaw angle

Returns: SO(3) rotation matrix

Definition at line 623 of file quat_ops.h.

◆ skew_x()

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

inline

Skew-symmetric matrix from a given 3x1 vector.

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*}$

Parameters

[in] w 3x1 vector to be made a skew-symmetric

Returns: 3x3 skew-symmetric matrix

Definition at line 135 of file quat_ops.h.

◆ vee()

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

inline

Returns vector portion of skew-symmetric.

See skew_x() for details.

Parameters

[in] w_x skew-symmetric matrix

Returns: 3x1 vector portion of skew

Definition at line 205 of file quat_ops.h.

Classes

Functions

Detailed Description

Function Documentation

◆ exp_se3()

◆ exp_so3()

◆ hat_se3()

◆ Inv()

◆ Inv_se3()

◆ Jl_so3()

◆ Jr_so3()

◆ log_se3()

◆ log_so3()

◆ Omega()

◆ quat_2_Rot()

◆ quat_multiply()

◆ quatnorm()

◆ rot2rpy()

◆ rot_2_quat()

◆ rot_x()

◆ rot_y()

◆ rot_z()

◆ skew_x()

◆ vee()