ov_eval::Math class

JPL quaternion math utilities.

Contents

This file contains the common utility functions for operating on JPL quaternions. We need to take special care to handle edge cases when converting to and from other rotation formats. All equations are based on the following tech report:

Trawny, Nikolas, and Stergios I. Roumeliotis. "Indirect Kalman filter for 3D attitude estimation." University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep 2 (2005): 2005. http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf

Public static functions

static auto rot_2_quat(const Eigen::Matrix<double, 3, 3>& rot) -> Eigen::Matrix<double, 4, 1>
Returns a JPL quaternion from a rotation matrix.
static auto skew_x(const Eigen::Matrix<double, 3, 1>& w) -> Eigen::Matrix<double, 3, 3>
Skew-symmetric matrix from a given 3x1 vector.
static auto quat_2_Rot(const Eigen::Matrix<double, 4, 1>& q) -> Eigen::Matrix<double, 3, 3>
Converts JPL quaterion to SO(3) rotation matrix.
static 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.
static auto vee(const Eigen::Matrix<double, 3, 3>& w_x) -> Eigen::Matrix<double, 3, 1>
Returns vector portion of skew-symmetric.
static auto exp_so3(const Eigen::Matrix<double, 3, 1>& w) -> Eigen::Matrix<double, 3, 3>
SO(3) matrix exponential.
static auto log_so3(const Eigen::Matrix<double, 3, 3>& R) -> Eigen::Matrix<double, 3, 1>
SO(3) matrix logarithm.
static auto exp_se3(Eigen::Matrix<double, 6, 1> vec) -> Eigen::Matrix4d
SE(3) matrix exponential function.
static auto log_se3(Eigen::Matrix4d mat) -> Eigen::Matrix<double, 6, 1>
SE(3) matrix logarithm.
static auto hat_se3(const Eigen::Matrix<double, 6, 1>& vec) -> Eigen::Matrix4d
Hat operator for R^6 -> Lie Algebra se(3)
static auto Inv(Eigen::Matrix<double, 4, 1> q) -> Eigen::Matrix<double, 4, 1>
JPL Quaternion inverse.
static auto Omega(Eigen::Matrix<double, 3, 1> w) -> Eigen::Matrix<double, 4, 4>
Integrated quaternion from angular velocity.
static auto quatnorm(Eigen::Matrix<double, 4, 1> q_t) -> Eigen::Matrix<double, 4, 1>
Normalizes a quaternion to make sure it is unit norm.
static auto rot2rpy(const Eigen::Matrix<double, 3, 3>& rot) -> Eigen::Matrix<double, 3, 1>
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)
static auto rot_x(double t) -> Eigen::Matrix<double, 3, 3>
Construct rotation matrix from given roll.
static auto rot_y(double t) -> Eigen::Matrix<double, 3, 3>
Construct rotation matrix from given pitch.
static auto rot_z(double t) -> Eigen::Matrix<double, 3, 3>
Construct rotation matrix from given yaw.

Function documentation

static Eigen::Matrix<double, 4, 1> ov_eval::Math::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*}

static Eigen::Matrix<double, 3, 3> ov_eval::Math::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*}

static Eigen::Matrix<double, 3, 3> ov_eval::Math::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*}

static Eigen::Matrix<double, 4, 1> ov_eval::Math::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*}

static Eigen::Matrix<double, 3, 1> ov_eval::Math::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.

static Eigen::Matrix<double, 3, 3> ov_eval::Math::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*}

static Eigen::Matrix<double, 3, 1> ov_eval::Math::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*}

static Eigen::Matrix4d ov_eval::Math::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*}

static Eigen::Matrix<double, 6, 1> ov_eval::Math::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*}

static Eigen::Matrix4d ov_eval::Math::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*}

static Eigen::Matrix<double, 4, 1> ov_eval::Math::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*}

static Eigen::Matrix<double, 4, 4> ov_eval::Math::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.

static Eigen::Matrix<double, 4, 1> ov_eval::Math::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

static Eigen::Matrix<double, 3, 1> ov_eval::Math::rot2rpy(const Eigen::Matrix<double, 3, 3>& rot)

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)

Parameters
rot Rotation matrix

static Eigen::Matrix<double, 3, 3> ov_eval::Math::rot_x(double t)

Construct rotation matrix from given roll.

Parameters
t roll angle

static Eigen::Matrix<double, 3, 3> ov_eval::Math::rot_y(double t)

Construct rotation matrix from given pitch.

Parameters
t pitch angle

static Eigen::Matrix<double, 3, 3> ov_eval::Math::rot_z(double t)

Construct rotation matrix from given yaw.

Parameters
t yaw angle