sim_details.hpp
Go to the documentation of this file.
1 #ifndef SOPHUS_SIM_DETAILS_HPP
2 #define SOPHUS_SIM_DETAILS_HPP
3
4 #include "types.hpp"
5
6 namespace Sophus {
7 namespace details {
8
9 template <class Scalar, int N>
11  Scalar const theta, Scalar const sigma) {
12  using std::abs;
13  using std::cos;
14  using std::exp;
15  using std::sin;
17  static Scalar const one(1);
18  static Scalar const half(0.5);
19  Matrix<Scalar, N, N> const Omega2 = Omega * Omega;
20  Scalar const scale = exp(sigma);
21  Scalar A, B, C;
22  if (abs(sigma) < Constants<Scalar>::epsilon()) {
23  C = one;
24  if (abs(theta) < Constants<Scalar>::epsilon()) {
25  A = half;
26  B = Scalar(1. / 6.);
27  } else {
28  Scalar theta_sq = theta * theta;
29  A = (one - cos(theta)) / theta_sq;
30  B = (theta - sin(theta)) / (theta_sq * theta);
31  }
32  } else {
33  C = (scale - one) / sigma;
34  if (abs(theta) < Constants<Scalar>::epsilon()) {
35  Scalar sigma_sq = sigma * sigma;
36  A = ((sigma - one) * scale + one) / sigma_sq;
37  B = (scale * half * sigma_sq + scale - one - sigma * scale) /
38  (sigma_sq * sigma);
39  } else {
40  Scalar theta_sq = theta * theta;
41  Scalar a = scale * sin(theta);
42  Scalar b = scale * cos(theta);
43  Scalar c = theta_sq + sigma * sigma;
44  A = (a * sigma + (one - b) * theta) / (theta * c);
45  B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq);
46  }
47  }
48  return A * Omega + B * Omega2 + C * I;
49 }
50
51 template <class Scalar, int N>
53  Scalar const theta, Scalar const sigma,
54  Scalar const scale) {
55  using std::abs;
56  using std::cos;
57  using std::sin;
59  static Scalar const half(0.5);
60  static Scalar const one(1);
61  static Scalar const two(2);
62  Matrix<Scalar, N, N> const Omega2 = Omega * Omega;
63  Scalar const scale_sq = scale * scale;
64  Scalar const theta_sq = theta * theta;
65  Scalar const sin_theta = sin(theta);
66  Scalar const cos_theta = cos(theta);
67
68  Scalar a, b, c;
69  if (abs(sigma * sigma) < Constants<Scalar>::epsilon()) {
70  c = one - half * sigma;
71  a = -half;
72  if (abs(theta_sq) < Constants<Scalar>::epsilon()) {
73  b = Scalar(1. / 12.);
74  } else {
75  b = (theta * sin_theta + two * cos_theta - two) /
76  (two * theta_sq * (cos_theta - one));
77  }
78  } else {
79  Scalar const scale_cu = scale_sq * scale;
80  c = sigma / (scale - one);
81  if (abs(theta_sq) < Constants<Scalar>::epsilon()) {
82  a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one));
83  b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) /
84  (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two);
85  } else {
86  Scalar const s_sin_theta = scale * sin_theta;
87  Scalar const s_cos_theta = scale * cos_theta;
88  a = (theta * s_cos_theta - theta - sigma * s_sin_theta) /
89  (theta * (scale_sq - two * s_cos_theta + one));
90  b = -scale *
91  (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta -
92  scale * sigma + sigma * cos_theta - sigma) /
93  (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq +
94  two * s_cos_theta + scale - one));
95  }
96  }
97  return a * Omega + b * Omega2 + c * I;
98 }
99
100 } // namespace details
101 } // namespace Sophus
102
103 #endif
Sophus::details::calcW
Matrix< Scalar, N, N > calcW(Matrix< Scalar, N, N > const &Omega, Scalar const theta, Scalar const sigma)
Definition: sim_details.hpp:10
types.hpp
Sophus::details::calcWInv
Matrix< Scalar, N, N > calcWInv(Matrix< Scalar, N, N > const &Omega, Scalar const theta, Scalar const sigma, Scalar const scale)
Definition: sim_details.hpp:52
Sophus::Constants::epsilon
static SOPHUS_FUNC Scalar epsilon()
Definition: common.hpp:147
Sophus::Matrix
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:41
Sophus
Definition: average.hpp:17
Sophus::Constants
Definition: common.hpp:146

sophus
Author(s): Hauke Strasdat
autogenerated on Wed Mar 2 2022 01:01:48