quat_ops.h
Go to the documentation of this file.
1 /*
2  * OpenVINS: An Open Platform for Visual-Inertial Research
3  * Copyright (C) 2018-2023 Patrick Geneva
4  * Copyright (C) 2018-2023 Guoquan Huang
5  * Copyright (C) 2018-2023 OpenVINS Contributors
6  * Copyright (C) 2018-2019 Kevin Eckenhoff
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program. If not, see <https://www.gnu.org/licenses/>.
20  */
21 
22 #ifndef OV_CORE_QUAT_OPS_H
23 #define OV_CORE_QUAT_OPS_H
24 
25 /*
26  * @section Summary
27  * This file contains the common utility functions for operating on JPL quaternions.
28  * We need to take special care to handle edge cases when converting to and from other rotation formats.
29  * All equations are based on the following tech report @cite Trawny2005TR :
30  *
31  * > Trawny, Nikolas, and Stergios I. Roumeliotis. "Indirect Kalman filter for 3D attitude estimation."
32  * > University of Minnesota, Dept. of Comp. Sci. & Eng., Tech. Rep 2 (2005): 2005.
33  * > http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf
34  *
35  * @section JPL Quaternion Definition
36  *
37  * We define the quaternion as the following linear combination:
38  * @f[
39  * \bar{q} = q_4+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}
40  * @f]
41  * Where i,j,k are defined as the following:
42  * @f[
43  * \mathbf{i}^2=-1~,~\mathbf{j}^2=-1~,~\mathbf{k}^2=-1
44  * @f]
45  * @f[
46  * -\mathbf{i}\mathbf{j}=\mathbf{j}\mathbf{i}=\mathbf{k}
47  * ~,~
48  * -\mathbf{j}\mathbf{k}=\mathbf{k}\mathbf{j}=\mathbf{i}
49  * ~,~
50  * -\mathbf{k}\mathbf{i}=\mathbf{i}\mathbf{k}=\mathbf{j}
51  * @f]
52  * As noted in @cite Trawny2005TR this does not correspond to the Hamilton notation, and follows the "JPL Proposed Standard Conventions".
53  * The q_4 quantity is the "scalar" portion of the quaternion, while q_1,q_2,q_3 are part of the "vector" portion.
54  * We split the 4x1 vector into the following convention:
55  * @f[
56  * \bar{q} = \begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix} = \begin{bmatrix}\mathbf{q}\\q_4\end{bmatrix}
57  * @f]
58  * It is also important to note that the quaternion is constrained to the unit circle:
59  * @f[
60  * |\bar{q}| = \sqrt{\bar{q}^\top\bar{q}} = \sqrt{|\mathbf{q}|^2+q_4^2} = 1
61  * @f]
62  *
63  */
64 
65 #include <Eigen/Eigen>
66 
67 namespace ov_core {
68 
88 inline Eigen::Matrix<double, 4, 1> rot_2_quat(const Eigen::Matrix<double, 3, 3> &rot) {
89  Eigen::Matrix<double, 4, 1> q;
90  double T = rot.trace();
91  if ((rot(0, 0) >= T) && (rot(0, 0) >= rot(1, 1)) && (rot(0, 0) >= rot(2, 2))) {
92  q(0) = sqrt((1 + (2 * rot(0, 0)) - T) / 4);
93  q(1) = (1 / (4 * q(0))) * (rot(0, 1) + rot(1, 0));
94  q(2) = (1 / (4 * q(0))) * (rot(0, 2) + rot(2, 0));
95  q(3) = (1 / (4 * q(0))) * (rot(1, 2) - rot(2, 1));
96 
97  } else if ((rot(1, 1) >= T) && (rot(1, 1) >= rot(0, 0)) && (rot(1, 1) >= rot(2, 2))) {
98  q(1) = sqrt((1 + (2 * rot(1, 1)) - T) / 4);
99  q(0) = (1 / (4 * q(1))) * (rot(0, 1) + rot(1, 0));
100  q(2) = (1 / (4 * q(1))) * (rot(1, 2) + rot(2, 1));
101  q(3) = (1 / (4 * q(1))) * (rot(2, 0) - rot(0, 2));
102  } else if ((rot(2, 2) >= T) && (rot(2, 2) >= rot(0, 0)) && (rot(2, 2) >= rot(1, 1))) {
103  q(2) = sqrt((1 + (2 * rot(2, 2)) - T) / 4);
104  q(0) = (1 / (4 * q(2))) * (rot(0, 2) + rot(2, 0));
105  q(1) = (1 / (4 * q(2))) * (rot(1, 2) + rot(2, 1));
106  q(3) = (1 / (4 * q(2))) * (rot(0, 1) - rot(1, 0));
107  } else {
108  q(3) = sqrt((1 + T) / 4);
109  q(0) = (1 / (4 * q(3))) * (rot(1, 2) - rot(2, 1));
110  q(1) = (1 / (4 * q(3))) * (rot(2, 0) - rot(0, 2));
111  q(2) = (1 / (4 * q(3))) * (rot(0, 1) - rot(1, 0));
112  }
113  if (q(3) < 0) {
114  q = -q;
115  }
116  // normalize and return
117  q = q / (q.norm());
118  return q;
119 }
120 
135 inline Eigen::Matrix<double, 3, 3> skew_x(const Eigen::Matrix<double, 3, 1> &w) {
136  Eigen::Matrix<double, 3, 3> w_x;
137  w_x << 0, -w(2), w(1), w(2), 0, -w(0), -w(1), w(0), 0;
138  return w_x;
139 }
140 
152 inline Eigen::Matrix<double, 3, 3> quat_2_Rot(const Eigen::Matrix<double, 4, 1> &q) {
153  Eigen::Matrix<double, 3, 3> q_x = skew_x(q.block(0, 0, 3, 1));
154  Eigen::MatrixXd Rot = (2 * std::pow(q(3, 0), 2) - 1) * Eigen::MatrixXd::Identity(3, 3) - 2 * q(3, 0) * q_x +
155  2 * q.block(0, 0, 3, 1) * (q.block(0, 0, 3, 1).transpose());
156  return Rot;
157 }
158 
180 inline Eigen::Matrix<double, 4, 1> quat_multiply(const Eigen::Matrix<double, 4, 1> &q, const Eigen::Matrix<double, 4, 1> &p) {
181  Eigen::Matrix<double, 4, 1> q_t;
182  Eigen::Matrix<double, 4, 4> Qm;
183  // create big L matrix
184  Qm.block(0, 0, 3, 3) = q(3, 0) * Eigen::MatrixXd::Identity(3, 3) - skew_x(q.block(0, 0, 3, 1));
185  Qm.block(0, 3, 3, 1) = q.block(0, 0, 3, 1);
186  Qm.block(3, 0, 1, 3) = -q.block(0, 0, 3, 1).transpose();
187  Qm(3, 3) = q(3, 0);
188  q_t = Qm * p;
189  // ensure unique by forcing q_4 to be >0
190  if (q_t(3, 0) < 0) {
191  q_t *= -1;
192  }
193  // normalize and return
194  return q_t / q_t.norm();
195 }
196 
205 inline Eigen::Matrix<double, 3, 1> vee(const Eigen::Matrix<double, 3, 3> &w_x) {
206  Eigen::Matrix<double, 3, 1> w;
207  w << w_x(2, 1), w_x(0, 2), w_x(1, 0);
208  return w;
209 }
210 
231 inline Eigen::Matrix<double, 3, 3> exp_so3(const Eigen::Matrix<double, 3, 1> &w) {
232  // get theta
233  Eigen::Matrix<double, 3, 3> w_x = skew_x(w);
234  double theta = w.norm();
235  // Handle small angle values
236  double A, B;
237  if (theta < 1e-7) {
238  A = 1;
239  B = 0.5;
240  } else {
241  A = sin(theta) / theta;
242  B = (1 - cos(theta)) / (theta * theta);
243  }
244  // compute so(3) rotation
245  Eigen::Matrix<double, 3, 3> R;
246  if (theta == 0) {
247  R = Eigen::MatrixXd::Identity(3, 3);
248  } else {
249  R = Eigen::MatrixXd::Identity(3, 3) + A * w_x + B * w_x * w_x;
250  }
251  return R;
252 }
253 
273 inline Eigen::Matrix<double, 3, 1> log_so3(const Eigen::Matrix<double, 3, 3> &R) {
274 
275  // note switch to base 1
276  double R11 = R(0, 0), R12 = R(0, 1), R13 = R(0, 2);
277  double R21 = R(1, 0), R22 = R(1, 1), R23 = R(1, 2);
278  double R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2);
279 
280  // Get trace(R)
281  const double tr = R.trace();
282  Eigen::Vector3d omega;
283 
284  // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
285  // we do something special
286  if (tr + 1.0 < 1e-10) {
287  if (std::abs(R33 + 1.0) > 1e-5)
288  omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Eigen::Vector3d(R13, R23, 1.0 + R33);
289  else if (std::abs(R22 + 1.0) > 1e-5)
290  omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Eigen::Vector3d(R12, 1.0 + R22, R32);
291  else
292  // if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit
293  omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Eigen::Vector3d(1.0 + R11, R21, R31);
294  } else {
295  double magnitude;
296  const double tr_3 = tr - 3.0; // always negative
297  if (tr_3 < -1e-7) {
298  double theta = acos((tr - 1.0) / 2.0);
299  magnitude = theta / (2.0 * sin(theta));
300  } else {
301  // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
302  // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
303  // see https://github.com/borglab/gtsam/issues/746 for details
304  magnitude = 0.5 - tr_3 / 12.0;
305  }
306  omega = magnitude * Eigen::Vector3d(R32 - R23, R13 - R31, R21 - R12);
307  }
308 
309  return omega;
310 }
311 
332 inline Eigen::Matrix4d exp_se3(Eigen::Matrix<double, 6, 1> vec) {
333 
334  // Precompute our values
335  Eigen::Vector3d w = vec.head(3);
336  Eigen::Vector3d u = vec.tail(3);
337  double theta = sqrt(w.dot(w));
338  Eigen::Matrix3d wskew;
339  wskew << 0, -w(2), w(1), w(2), 0, -w(0), -w(1), w(0), 0;
340 
341  // Handle small angle values
342  double A, B, C;
343  if (theta < 1e-7) {
344  A = 1;
345  B = 0.5;
346  C = 1.0 / 6.0;
347  } else {
348  A = sin(theta) / theta;
349  B = (1 - cos(theta)) / (theta * theta);
350  C = (1 - A) / (theta * theta);
351  }
352 
353  // Matrices we need V and Identity
354  Eigen::Matrix3d I_33 = Eigen::Matrix3d::Identity();
355  Eigen::Matrix3d V = I_33 + B * wskew + C * wskew * wskew;
356 
357  // Get the final matrix to return
358  Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
359  mat.block(0, 0, 3, 3) = I_33 + A * wskew + B * wskew * wskew;
360  mat.block(0, 3, 3, 1) = V * u;
361  mat(3, 3) = 1;
362  return mat;
363 }
364 
388 inline Eigen::Matrix<double, 6, 1> log_se3(Eigen::Matrix4d mat) {
389  Eigen::Vector3d w = log_so3(mat.block<3, 3>(0, 0));
390  Eigen::Vector3d T = mat.block<3, 1>(0, 3);
391  const double t = w.norm();
392  if (t < 1e-10) {
393  Eigen::Matrix<double, 6, 1> log;
394  log << w, T;
395  return log;
396  } else {
397  Eigen::Matrix3d W = skew_x(w / t);
398  // Formula from Agrawal06iros, equation (14)
399  // simplified with Mathematica, and multiplying in T to avoid matrix math
400  double Tan = tan(0.5 * t);
401  Eigen::Vector3d WT = W * T;
402  Eigen::Vector3d u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT);
403  Eigen::Matrix<double, 6, 1> log;
404  log << w, u;
405  return log;
406  }
407 }
408 
419 inline Eigen::Matrix4d hat_se3(const Eigen::Matrix<double, 6, 1> &vec) {
420  Eigen::Matrix4d mat = Eigen::Matrix4d::Zero();
421  mat.block(0, 0, 3, 3) = skew_x(vec.head(3));
422  mat.block(0, 3, 3, 1) = vec.tail(3);
423  return mat;
424 }
425 
439 inline Eigen::Matrix4d Inv_se3(const Eigen::Matrix4d &T) {
440  Eigen::Matrix4d Tinv = Eigen::Matrix4d::Identity();
441  Tinv.block(0, 0, 3, 3) = T.block(0, 0, 3, 3).transpose();
442  Tinv.block(0, 3, 3, 1) = -Tinv.block(0, 0, 3, 3) * T.block(0, 3, 3, 1);
443  return Tinv;
444 }
445 
457 inline Eigen::Matrix<double, 4, 1> Inv(Eigen::Matrix<double, 4, 1> q) {
458  Eigen::Matrix<double, 4, 1> qinv;
459  qinv.block(0, 0, 3, 1) = -q.block(0, 0, 3, 1);
460  qinv(3, 0) = q(3, 0);
461  return qinv;
462 }
463 
482 inline Eigen::Matrix<double, 4, 4> Omega(Eigen::Matrix<double, 3, 1> w) {
483  Eigen::Matrix<double, 4, 4> mat;
484  mat.block(0, 0, 3, 3) = -skew_x(w);
485  mat.block(3, 0, 1, 3) = -w.transpose();
486  mat.block(0, 3, 3, 1) = w;
487  mat(3, 3) = 0;
488  return mat;
489 }
490 
496 inline Eigen::Matrix<double, 4, 1> quatnorm(Eigen::Matrix<double, 4, 1> q_t) {
497  if (q_t(3, 0) < 0) {
498  q_t *= -1;
499  }
500  return q_t / q_t.norm();
501 }
502 
515 inline Eigen::Matrix<double, 3, 3> Jl_so3(const Eigen::Matrix<double, 3, 1> &w) {
516  double theta = w.norm();
517  if (theta < 1e-6) {
518  return Eigen::MatrixXd::Identity(3, 3);
519  } else {
520  Eigen::Matrix<double, 3, 1> a = w / theta;
521  Eigen::Matrix<double, 3, 3> J = sin(theta) / theta * Eigen::MatrixXd::Identity(3, 3) + (1 - sin(theta) / theta) * a * a.transpose() +
522  ((1 - cos(theta)) / theta) * skew_x(a);
523  return J;
524  }
525 }
526 
537 inline Eigen::Matrix<double, 3, 3> Jr_so3(const Eigen::Matrix<double, 3, 1> &w) { return Jl_so3(-w); }
538 
549 inline Eigen::Matrix<double, 3, 1> rot2rpy(const Eigen::Matrix<double, 3, 3> &rot) {
550  Eigen::Matrix<double, 3, 1> rpy;
551  rpy(1, 0) = atan2(-rot(2, 0), sqrt(rot(0, 0) * rot(0, 0) + rot(1, 0) * rot(1, 0)));
552  if (std::abs(cos(rpy(1, 0))) > 1.0e-12) {
553  rpy(2, 0) = atan2(rot(1, 0) / cos(rpy(1, 0)), rot(0, 0) / cos(rpy(1, 0)));
554  rpy(0, 0) = atan2(rot(2, 1) / cos(rpy(1, 0)), rot(2, 2) / cos(rpy(1, 0)));
555  } else {
556  rpy(2, 0) = 0;
557  rpy(0, 0) = atan2(rot(0, 1), rot(1, 1));
558  }
559  return rpy;
560 }
561 
577 inline Eigen::Matrix<double, 3, 3> rot_x(double t) {
578  Eigen::Matrix<double, 3, 3> r;
579  double ct = cos(t);
580  double st = sin(t);
581  r << 1.0, 0.0, 0.0, 0.0, ct, -st, 0.0, st, ct;
582  return r;
583 }
584 
600 inline Eigen::Matrix<double, 3, 3> rot_y(double t) {
601  Eigen::Matrix<double, 3, 3> r;
602  double ct = cos(t);
603  double st = sin(t);
604  r << ct, 0.0, st, 0.0, 1.0, 0.0, -st, 0.0, ct;
605  return r;
606 }
607 
623 inline Eigen::Matrix<double, 3, 3> rot_z(double t) {
624  Eigen::Matrix<double, 3, 3> r;
625  double ct = cos(t);
626  double st = sin(t);
627  r << ct, -st, 0.0, st, ct, 0.0, 0.0, 0.0, 1.0;
628  return r;
629 }
630 
631 } // namespace ov_core
632 
633 #endif /* OV_CORE_QUAT_OPS_H */
ov_core::Jr_so3
Eigen::Matrix< double, 3, 3 > Jr_so3(const Eigen::Matrix< double, 3, 1 > &w)
Computes right Jacobian of SO(3)
Definition: quat_ops.h:537
ov_core::skew_x
Eigen::Matrix< double, 3, 3 > skew_x(const Eigen::Matrix< double, 3, 1 > &w)
Skew-symmetric matrix from a given 3x1 vector.
Definition: quat_ops.h:135
ov_core::rot_z
Eigen::Matrix< double, 3, 3 > rot_z(double t)
Construct rotation matrix from given yaw.
Definition: quat_ops.h:623
ov_core::quat_multiply
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.
Definition: quat_ops.h:180
ov_core::log_se3
Eigen::Matrix< double, 6, 1 > log_se3(Eigen::Matrix4d mat)
SE(3) matrix logarithm.
Definition: quat_ops.h:388
ov_core::exp_se3
Eigen::Matrix4d exp_se3(Eigen::Matrix< double, 6, 1 > vec)
SE(3) matrix exponential function.
Definition: quat_ops.h:332
ov_core::rot_x
Eigen::Matrix< double, 3, 3 > rot_x(double t)
Construct rotation matrix from given roll.
Definition: quat_ops.h:577
ov_core::vee
Eigen::Matrix< double, 3, 1 > vee(const Eigen::Matrix< double, 3, 3 > &w_x)
Returns vector portion of skew-symmetric.
Definition: quat_ops.h:205
ov_core::exp_so3
Eigen::Matrix< double, 3, 3 > exp_so3(const Eigen::Matrix< double, 3, 1 > &w)
SO(3) matrix exponential.
Definition: quat_ops.h:231
ov_core::hat_se3
Eigen::Matrix4d hat_se3(const Eigen::Matrix< double, 6, 1 > &vec)
Hat operator for R^6 -> Lie Algebra se(3)
Definition: quat_ops.h:419
ov_core::Omega
Eigen::Matrix< double, 4, 4 > Omega(Eigen::Matrix< double, 3, 1 > w)
Integrated quaternion from angular velocity.
Definition: quat_ops.h:482
ov_core::rot_2_quat
Eigen::Matrix< double, 4, 1 > rot_2_quat(const Eigen::Matrix< double, 3, 3 > &rot)
Returns a JPL quaternion from a rotation matrix.
Definition: quat_ops.h:88
ov_core::quat_2_Rot
Eigen::Matrix< double, 3, 3 > quat_2_Rot(const Eigen::Matrix< double, 4, 1 > &q)
Converts JPL quaterion to SO(3) rotation matrix.
Definition: quat_ops.h:152
ov_core::rot_y
Eigen::Matrix< double, 3, 3 > rot_y(double t)
Construct rotation matrix from given pitch.
Definition: quat_ops.h:600
ov_core::quatnorm
Eigen::Matrix< double, 4, 1 > quatnorm(Eigen::Matrix< double, 4, 1 > q_t)
Normalizes a quaternion to make sure it is unit norm.
Definition: quat_ops.h:496
ov_core::Jl_so3
Eigen::Matrix< double, 3, 3 > Jl_so3(const Eigen::Matrix< double, 3, 1 > &w)
Computes left Jacobian of SO(3)
Definition: quat_ops.h:515
ov_core::Inv
Eigen::Matrix< double, 4, 1 > Inv(Eigen::Matrix< double, 4, 1 > q)
JPL Quaternion inverse.
Definition: quat_ops.h:457
ov_core::log_so3
Eigen::Matrix< double, 3, 1 > log_so3(const Eigen::Matrix< double, 3, 3 > &R)
SO(3) matrix logarithm.
Definition: quat_ops.h:273
ov_core::Inv_se3
Eigen::Matrix4d Inv_se3(const Eigen::Matrix4d &T)
SE(3) matrix analytical inverse.
Definition: quat_ops.h:439
ov_core
Core algorithms for OpenVINS.
Definition: CamBase.h:30
ov_core::rot2rpy
Eigen::Matrix< double, 3, 1 > rot2rpy(const Eigen::Matrix< double, 3, 3 > &rot)
Gets roll, pitch, yaw of argument rotation (in that order).
Definition: quat_ops.h:549

ov_core
Author(s): Patrick Geneva , Kevin Eckenhoff , Guoquan Huang
autogenerated on Mon Dec 16 2024 03:06:46