12 #include <mrpt/config.h>
34 q.rotationMatrixNoResize(
R);
52 tang2mat_jacob J = tang2mat_jacob::Zero();
53 J(1, 2) = J(5, 0) = J(6, 1) = +1.0;
54 J(2, 1) = J(3, 2) = J(7, 0) = -1.0;
74 const auto squared_n = q.
x() * q.
x() + q.
y() * q.
y() + q.
z() * q.
z();
75 const auto n = sqrt(squared_n);
78 double two_atan_nbyw_by_n;
91 ASSERTMSG_(std::abs(w) >= 1e-7,
"Quaternion should be normalized!");
92 two_atan_nbyw_by_n = 2.0 / w - 2.0 * (squared_n) / (w * w * w);
96 if (std::abs(w) < 1e-7)
99 two_atan_nbyw_by_n =
M_PI / n;
101 two_atan_nbyw_by_n = -
M_PI / n;
105 two_atan_nbyw_by_n = 2.0 * atan(n / w) / n;
110 ret[0] = two_atan_nbyw_by_n * q.
x();
111 ret[1] = two_atan_nbyw_by_n * q.
y();
112 ret[2] = two_atan_nbyw_by_n * q.
z();
118 const double yaw,
const double pitch,
const double roll)
122 ::sincos(yaw, &sy, &cy);
124 ::sincos(
pitch, &sp, &cp);
126 ::sincos(
roll, &sr, &cr);
128 const double cy = cos(yaw);
129 const double sy = sin(yaw);
130 const double cp = cos(
pitch);
131 const double sp = sin(
pitch);
132 const double cr = cos(
roll);
133 const double sr = sin(
roll);
137 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
const double rot_vals[] = {
138 cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr,
139 sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr,
140 -sp, cp * sr, cp * cr
144 R.loadFromArray(rot_vals);
148 template <
typename VEC3,
typename MAT3x3,
typename MAT3x9>
149 inline void M3x9(
const VEC3& a,
const MAT3x3& B, MAT3x9& RES)
152 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
const double vals[] = {
153 a[0], -B(0, 2), B(0, 1), B(0, 2), a[0], -B(0, 0), -B(0, 1), B(0, 0), a[0],
154 a[1], -B(1, 2), B(1, 1), B(1, 2), a[1], -B(1, 0), -B(1, 1), B(1, 0), a[1],
155 a[2], -B(2, 2), B(2, 1), B(2, 2), a[2], -B(2, 0), -B(2, 1), B(2, 0), a[2]
158 RES.loadFromArray(vals);
165 const double d = 0.5 * (
R(0, 0) +
R(1, 1) +
R(2, 2) - 1);
170 a[0] = a[1] = a[2] = 0;
175 const double theta = acos(d);
176 const double d2 =
square(d);
177 const double sq = std::sqrt(1 - d2);
179 a *= (d * theta - sq) / (4 * (sq * sq * sq));
190 v[0] =
R(2, 1) -
R(1, 2);
191 v[1] =
R(0, 2) -
R(2, 0);
192 v[2] =
R(1, 0) -
R(0, 1);