farm-ng-core
rotation3.h
Go to the documentation of this file.
1 // Copyright (c) 2011, Hauke Strasdat
2 // Copyright (c) 2012, Steven Lovegrove
3 // Copyright (c) 2021, farm-ng, inc.
4 //
5 // Use of this source code is governed by an MIT-style
6 // license that can be found in the LICENSE file or at
7 // https://opensource.org/licenses/MIT.
8 
9 #pragma once
10 #include "sophus/concepts/lie_group.h"
11 #include "sophus/manifold/quaternion.h"
12 #include "sophus/manifold/unit_vector.h"
13 
14 namespace sophus {
15 namespace lie {
16 
17 template <class TScalar>
18 class Rotation3Impl {
19  public:
20  using Scalar = TScalar;
21  using Quaternion = QuaternionImpl<TScalar>;
22 
23  static bool constexpr kIsOriginPreserving = true;
24  static bool constexpr kIsAxisDirectionPreserving = false;
25  static bool constexpr kIsDirectionVectorPreserving = false;
26  static bool constexpr kIsShapePreserving = true;
27  static bool constexpr kIisSizePreserving = true;
28  static bool constexpr kIisParallelLinePreserving = true;
29 
30  static int const kDof = 3;
31  static int const kNumParams = 4;
32  static int const kPointDim = 3;
33  static int const kAmbientDim = 3;
34 
35  using Tangent = Eigen::Vector<Scalar, kDof>;
36  using Params = Eigen::Vector<Scalar, kNumParams>;
37  using Point = Eigen::Vector<Scalar, kPointDim>;
38 
39  template <class TCompatibleScalar>
40  using ScalarReturn = typename Eigen::
41  ScalarBinaryOpTraits<Scalar, TCompatibleScalar>::ReturnType;
42 
43  template <class TCompatibleScalar>
44  using ParamsReturn =
45  Eigen::Vector<ScalarReturn<TCompatibleScalar>, kNumParams>;
46 
47  template <class TCompatibleScalar>
48  using PointReturn = Eigen::Vector<ScalarReturn<TCompatibleScalar>, kPointDim>;
49 
50  template <class TCompatibleScalar>
51  using UnitVectorReturn =
52  UnitVector<ScalarReturn<TCompatibleScalar>, kPointDim>;
53 
54  // constructors and factories
55 
56  static auto identityParams() -> Params {
57  return Eigen::Vector<Scalar, 4>(
58  Scalar(0.0), Scalar(0.0), Scalar(0.0), Scalar(1.0));
59  }
60 
61  static auto areParamsValid(Params const& unit_quaternion)
62  -> sophus::Expected<Success> {
63  static const Scalar kThr = kEpsilonSqrt<Scalar>;
64  const Scalar squared_norm = unit_quaternion.squaredNorm();
65  using std::abs;
66  if (!(abs(squared_norm - 1.0) <= kThr)) {
67  return SOPHUS_UNEXPECTED(
68  "quaternion number (({}), {}) is not unit length.\n"
69  "Squared norm: {}, thr: {}",
70  unit_quaternion.template head<3>(),
71  unit_quaternion[3],
72  squared_norm,
73  kThr);
74  }
75  return sophus::Expected<Success>{};
76  }
77 
78  static auto hasShortestPathAmbiguity(Params const& foo_from_bar) -> bool {
79  using std::abs;
80  TScalar angle = Rotation3Impl::log(foo_from_bar).norm();
81  TScalar const k_pi = kPi<TScalar>; // NOLINT
82  return abs(angle - k_pi) / (angle + k_pi) < kEpsilon<TScalar>;
83  }
84 
85  // Manifold / Lie Group concepts
86 
87  static auto exp(Tangent const& omega) -> Params {
88  using std::abs;
89  using std::cos;
90  using std::sin;
91  using std::sqrt;
92  Scalar theta;
93  Scalar theta_sq = omega.squaredNorm();
94 
95  Scalar imag_factor;
96  Scalar real_factor;
97  if (theta_sq < kEpsilon<Scalar> * kEpsilon<Scalar>) {
98  theta = Scalar(0);
99  Scalar theta_po4 = theta_sq * theta_sq;
100  imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq +
101  Scalar(1.0 / 3840.0) * theta_po4;
102  real_factor = Scalar(1) - Scalar(1.0 / 8.0) * theta_sq +
103  Scalar(1.0 / 384.0) * theta_po4;
104  } else {
105  theta = sqrt(theta_sq);
106  Scalar half_theta = Scalar(0.5) * (theta);
107  Scalar sin_half_theta = sin(half_theta);
108  imag_factor = sin_half_theta / (theta);
109  real_factor = cos(half_theta);
110  }
111 
112  return Params(
113  imag_factor * omega.x(),
114  imag_factor * omega.y(),
115  imag_factor * omega.z(),
116  real_factor);
117  }
118 
119  static auto log(Params const& unit_quaternion) -> Tangent {
120  using std::abs;
121  using std::atan2;
122  using std::sqrt;
123  Eigen::Vector3<Scalar> ivec = unit_quaternion.template head<3>();
124 
125  Scalar squared_n = ivec.squaredNorm();
126  Scalar w = unit_quaternion.w();
127 
128  Scalar two_atan_nbyw_by_n;
129 
130  // Atan-based log thanks to
131  //
132  // C. Hertzberg et al.:
133  // "Integrating Generic Sensor Fusion Algorithms with Sound State
134  // Representation through Encapsulation of Manifolds"
135  // Information Fusion, 2011
136  if (squared_n < kEpsilon<Scalar> * kEpsilon<Scalar>) {
137  // If quaternion is normalized and n=0, then w should be 1;
138  // w=0 should never happen here!
139  SOPHUS_ASSERT(
140  abs(w) >= kEpsilon<Scalar>,
141  "Quaternion ({}) should be normalized!",
142  unit_quaternion);
143  Scalar squared_w = w * w;
144  two_atan_nbyw_by_n =
145  Scalar(2) / w - Scalar(2.0 / 3.0) * (squared_n) / (w * squared_w);
146  } else {
147  Scalar n = sqrt(squared_n);
148 
149  // w < 0 ==> cos(theta/2) < 0 ==> theta > pi
150  //
151  // By convention, the condition |theta| < pi is imposed by wrapping theta
152  // to pi; The wrap operation can be folded inside evaluation of atan2
153  //
154  // theta - pi = atan(sin(theta - pi), cos(theta - pi))
155  // = atan(-sin(theta), -cos(theta))
156  //
157  Scalar atan_nbyw =
158  (w < Scalar(0)) ? Scalar(atan2(-n, -w)) : Scalar(atan2(n, w));
159  two_atan_nbyw_by_n = Scalar(2) * atan_nbyw / n;
160  }
161  return two_atan_nbyw_by_n * ivec;
162  }
163 
164  static auto hat(Tangent const& omega)
165  -> Eigen::Matrix<Scalar, kAmbientDim, kAmbientDim> {
166  Eigen::Matrix<Scalar, kAmbientDim, kAmbientDim> mat_omega;
167  // clang-format off
168  mat_omega <<
169  Scalar(0), -omega(2), omega(1),
170  omega(2), Scalar(0), -omega(0),
171  -omega(1), omega(0), Scalar(0);
172  // clang-format on
173  return mat_omega;
174  }
175 
176  static auto vee(
177  Eigen::Matrix<Scalar, kAmbientDim, kAmbientDim> const& mat_omega)
178  -> Eigen::Matrix<Scalar, kDof, 1> {
179  return Eigen::Matrix<Scalar, kDof, 1>(
180  mat_omega(2, 1), mat_omega(0, 2), mat_omega(1, 0));
181  }
182 
183  // group operations
184 
185  static auto inverse(Params const& unit_quat) -> Params {
186  return Quaternion::conjugate(unit_quat);
187  }
188 
189  template <class TCompatibleScalar>
190  static auto multiplication(
191  Params const& lhs_params,
192  Eigen::Vector<TCompatibleScalar, 4> const& rhs_params)
193  -> ParamsReturn<TCompatibleScalar> {
194  ParamsReturn<TCompatibleScalar> result =
195  Quaternion::multiplication(lhs_params, rhs_params);
196  using SReturn = ScalarReturn<TCompatibleScalar>;
197  SReturn const squared_norm = result.squaredNorm();
198 
199  // We can assume that the squared-norm is close to 1 since we deal with a
200  // unit complex number. Due to numerical precision issues, there might
201  // be a small drift after pose concatenation. Hence, we need to
202  // the complex number here.
203  // Since squared-norm is close to 1, we do not need to calculate the costly
204  // square-root, but can use an approximation around 1 (see
205  // http://stackoverflow.com/a/12934750 for details).
206  if (squared_norm != SReturn(1.0)) {
207  SReturn const scale = SReturn(2.0) / (SReturn(1.0) + squared_norm);
208  return scale * result;
209  }
210  return result;
211  }
212 
213  static auto adj(Params const& params) -> Eigen::Matrix<Scalar, kDof, kDof> {
214  return matrix(params);
215  }
216 
217  // Point actions
218 
219  template <class TCompatibleScalar>
220  static auto action(
221  Params const& unit_quat,
222  Eigen::Vector<TCompatibleScalar, kPointDim> const& point)
223  -> PointReturn<TCompatibleScalar> {
224  Eigen::Vector3<Scalar> ivec = unit_quat.template head<3>();
225 
226  PointReturn<TCompatibleScalar> uv = ivec.cross(point);
227  uv += uv;
228  return point + unit_quat.w() * uv + ivec.cross(uv);
229  }
230 
231  template <class TCompatibleScalar>
232  static auto action(
233  Params const& unit_quat,
234  UnitVector<TCompatibleScalar, 3> const& direction_vector)
235  -> UnitVectorReturn<TCompatibleScalar> {
236  // TODO: Implement normalization using expansion around 1 as done for
237  // ::multiplication to avoid possibly costly call to std::sqrt.
238  return UnitVectorReturn<TCompatibleScalar>::fromVectorAndNormalize(
239  action(unit_quat, direction_vector.params()));
240  }
241 
242  static auto toAmbient(Point const& point)
243  -> Eigen::Vector<Scalar, kAmbientDim> {
244  return point;
245  }
246 
247  // matrices
248 
249  static auto compactMatrix(Params const& unit_quat)
250  -> Eigen::Matrix<Scalar, kPointDim, kPointDim> {
251  Eigen::Vector3<Scalar> ivec = unit_quat.template head<3>();
252  Scalar real = unit_quat.w();
253  return Eigen::Matrix<Scalar, 3, 3>{
254  {Scalar(1.0 - 2.0 * (ivec[1] * ivec[1]) - 2.0 * (ivec[2] * ivec[2])),
255  Scalar(2.0 * ivec[0] * ivec[1] - 2.0 * ivec[2] * real),
256  Scalar(2.0 * ivec[0] * ivec[2] + 2.0 * ivec[1] * real)},
257  {
258  Scalar(2.0 * ivec[0] * ivec[1] + 2.0 * ivec[2] * real),
259  Scalar(1.0 - 2.0 * (ivec[0] * ivec[0]) - 2.0 * (ivec[2] * ivec[2])),
260  Scalar(2.0 * ivec[1] * ivec[2] - 2.0 * ivec[0] * real),
261  },
262  {Scalar(2.0 * ivec[0] * ivec[2] - 2.0 * ivec[1] * real),
263  Scalar(2.0 * ivec[1] * ivec[2] + 2.0 * ivec[0] * real),
264  Scalar(1.0 - 2.0 * (ivec[0] * ivec[0]) - 2.0 * (ivec[1] * ivec[1]))}};
265  }
266 
267  static auto matrix(Params const& unit_quat)
268  -> Eigen::Matrix<Scalar, kPointDim, kPointDim> {
269  return compactMatrix(unit_quat);
270  }
271 
272  // Sub-group concepts
273  static auto matV(Params const& params, Tangent const& omega)
274  -> Eigen::Matrix<Scalar, kPointDim, kPointDim> {
275  using std::cos;
276  using std::sin;
277  using std::sqrt;
278 
279  Scalar const theta_sq = omega.squaredNorm();
280  Eigen::Matrix3<Scalar> const mat_omega = hat(omega);
281  Eigen::Matrix3<Scalar> const mat_omega_sq = mat_omega * mat_omega;
282  Eigen::Matrix3<Scalar> v;
283 
284  if (theta_sq < kEpsilon<Scalar> * kEpsilon<Scalar>) {
285  v = Eigen::Matrix3<Scalar>::Identity() + Scalar(0.5) * mat_omega;
286  } else {
287  Scalar theta = sqrt(theta_sq);
288  v = Eigen::Matrix3<Scalar>::Identity() +
289  (Scalar(1) - cos(theta)) / theta_sq * mat_omega +
290  (theta - sin(theta)) / (theta_sq * theta) * mat_omega_sq;
291  }
292  return v;
293  }
294 
295  static auto matVInverse(Params const& /*unused*/, Tangent const& omega)
296  -> Eigen::Matrix<Scalar, kPointDim, kPointDim> {
297  using std::cos;
298  using std::sin;
299  using std::sqrt;
300  Scalar const theta_sq = omega.squaredNorm();
301  Eigen::Matrix3<Scalar> const mat_omega = hat(omega);
302 
303  Eigen::Matrix3<Scalar> v_inv;
304  if (theta_sq < kEpsilon<Scalar> * kEpsilon<Scalar>) {
305  v_inv = Eigen::Matrix3<Scalar>::Identity() - Scalar(0.5) * mat_omega +
306  Scalar(1. / 12.) * (mat_omega * mat_omega);
307 
308  } else {
309  Scalar const theta = sqrt(theta_sq);
310  Scalar const half_theta = Scalar(0.5) * theta;
311 
312  v_inv = Eigen::Matrix3<Scalar>::Identity() - Scalar(0.5) * mat_omega +
313  (Scalar(1) -
314  Scalar(0.5) * theta * cos(half_theta) / sin(half_theta)) /
315  (theta * theta) * (mat_omega * mat_omega);
316  }
317  return v_inv;
318  }
319 
320  static auto adjOfTranslation(Params const& params, Point const& point)
321  -> Eigen::Matrix<Scalar, kPointDim, kDof> {
322  return hat(point) * matrix(params);
323  }
324 
325  static auto adOfTranslation(Point const& point)
326  -> Eigen::Matrix<Scalar, kPointDim, kDof> {
327  return hat(point);
328  }
329 
330  // derivatives
331  static auto ad(Tangent const& omega) -> Eigen::Matrix<Scalar, kDof, kDof> {
332  return hat(omega);
333  }
334 
335  static auto dxExpX(Tangent const& omega)
336  -> Eigen::Matrix<Scalar, kNumParams, kDof> {
337  using std::cos;
338  using std::exp;
339  using std::sin;
340  using std::sqrt;
341  Scalar const c0 = omega[0] * omega[0];
342  Scalar const c1 = omega[1] * omega[1];
343  Scalar const c2 = omega[2] * omega[2];
344  Scalar const c3 = c0 + c1 + c2;
345 
346  if (c3 < kEpsilon<Scalar>) {
347  return dxExpXAt0();
348  }
349 
350  Scalar const c4 = sqrt(c3);
351  Scalar const c5 = 1.0 / c4;
352  Scalar const c6 = 0.5 * c4;
353  Scalar const c7 = sin(c6);
354  Scalar const c8 = c5 * c7;
355  Scalar const c9 = pow(c3, -3.0L / 2.0L);
356  Scalar const c10 = c7 * c9;
357  Scalar const c11 = Scalar(1.0) / c3;
358  Scalar const c12 = cos(c6);
359  Scalar const c13 = Scalar(0.5) * c11 * c12;
360  Scalar const c14 = c7 * c9 * omega[0];
361  Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0];
362  Scalar const c16 = -c14 * omega[1] + c15 * omega[1];
363  Scalar const c17 = -c14 * omega[2] + c15 * omega[2];
364  Scalar const c18 = omega[1] * omega[2];
365  Scalar const c19 = -c10 * c18 + c13 * c18;
366  Scalar const c20 = Scalar(0.5) * c5 * c7;
367 
368  Eigen::Matrix<Scalar, 4, 3> j;
369 
370  j(0, 0) = -c0 * c10 + c0 * c13 + c8;
371  j(0, 1) = c16;
372  j(0, 2) = c17;
373  j(1, 0) = c16;
374  j(1, 1) = -c1 * c10 + c1 * c13 + c8;
375  j(1, 2) = c19;
376  j(2, 0) = c17;
377  j(2, 1) = c19;
378  j(2, 2) = -c10 * c2 + c13 * c2 + c8;
379  j(3, 0) = -c20 * omega[0];
380  j(3, 1) = -c20 * omega[1];
381  j(3, 2) = -c20 * omega[2];
382  return j;
383  }
384 
385  static auto dxExpXAt0() -> Eigen::Matrix<Scalar, kNumParams, kDof> {
386  Eigen::Matrix<Scalar, 4, 3> j;
387  // clang-format off
388  j << Scalar(0.5), Scalar(0), Scalar(0),
389  Scalar(0), Scalar(0.5), Scalar(0),
390  Scalar(0), Scalar(0), Scalar(0.5),
391  Scalar(0), Scalar(0), Scalar(0);
392  // clang-format on
393  return j;
394  }
395 
396  static auto dxExpXTimesPointAt0(Point const& point)
397  -> Eigen::Matrix<Scalar, kPointDim, kDof> {
398  return hat(-point);
399  }
400 
401  static auto dxThisMulExpXAt0(Params const& unit_quat)
402  -> Eigen::Matrix<Scalar, kNumParams, kDof> {
403  Eigen::Matrix<Scalar, 4, 3> j;
404  Scalar const c0 = Scalar(0.5) * unit_quat.w();
405  Scalar const c1 = Scalar(0.5) * unit_quat.z();
406  Scalar const c2 = -c1;
407  Scalar const c3 = Scalar(0.5) * unit_quat.y();
408  Scalar const c4 = Scalar(0.5) * unit_quat.x();
409  Scalar const c5 = -c4;
410  Scalar const c6 = -c3;
411  j(0, 0) = c0;
412  j(0, 1) = c2;
413  j(0, 2) = c3;
414  j(1, 0) = c1;
415  j(1, 1) = c0;
416  j(1, 2) = c5;
417  j(2, 0) = c6;
418  j(2, 1) = c4;
419  j(2, 2) = c0;
420  j(3, 0) = c5;
421  j(3, 1) = c6;
422  j(3, 2) = c2;
423  return j;
424  }
425 
426  static auto dxLogThisInvTimesXAtThis(Params const& q)
427  -> Eigen::Matrix<Scalar, kDof, kNumParams> {
428  Eigen::Matrix<Scalar, 3, 4> j;
429  // clang-format off
430  j << q.w(), q.z(), -q.y(), -q.x(),
431  -q.z(), q.w(), q.x(), -q.y(),
432  q.y(), -q.x(), q.w(), -q.z();
433  // clang-format on
434  return j * Scalar(2.);
435  }
436 
437  // for tests
438 
439  static auto tangentExamples() -> std::vector<Tangent> {
440  Scalar o(0);
441  Scalar i(1);
442  return std::vector<Tangent>({
443  Tangent(o, o, o),
444  Tangent(i, o, o),
445  Tangent(o, i, o),
446  Tangent{Scalar(0.5) * kPi<Scalar>, Scalar(0.5) * kPi<Scalar>, o},
447  Tangent{-i, i, o},
448  Tangent{Scalar(20), -i, o},
449  Tangent{Scalar(30), Scalar(5), -i},
450  Tangent{i, i, Scalar(4)},
451  Tangent{i, Scalar(-3), Scalar(0.5)},
452  Tangent{Scalar(-5), Scalar(-6), Scalar(7)},
453  });
454  }
455 
456  static auto paramsExamples() -> std::vector<Params> {
457  using Point = Point;
458  return std::vector<Params>(
459  {Params(Scalar(0.1e-11), Scalar(0.), Scalar(1.), Scalar(0.)),
460  Params(Scalar(-1), Scalar(0.00001), Scalar(0.0), Scalar(0.0)),
461  exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0))),
462  exp(Point(Scalar(0.2), Scalar(0.5), Scalar(-1.0))),
463  exp(Point(Scalar(0.), Scalar(0.), Scalar(0.))),
464  exp(Point(Scalar(0.), Scalar(0.), Scalar(0.00001))),
465  exp(Point(kPi<Scalar>, Scalar(0), Scalar(0))),
466  multiplication(
467  multiplication(
468  exp(Point(Scalar(0.2), Scalar(0.5), Scalar(0.0))),
469  exp(Point(kPi<Scalar>, Scalar(0), Scalar(0)))),
470  exp(Point(kPi<Scalar>, Scalar(0), Scalar(0)))),
471  multiplication(
472  multiplication(
473  exp(Point(Scalar(0.3), Scalar(0.5), Scalar(0.1))),
474  exp(Point(kPi<Scalar>, Scalar(0), Scalar(0)))),
475  exp(Point(Scalar(-0.3), Scalar(-0.5), Scalar(-0.1))))});
476  }
477 
478  static auto invalidParamsExamples() -> std::vector<Params> {
479  return std::vector<Params>({
480  Params::Zero(),
481  Params::Ones(),
482  2.0 * Params::UnitX(),
483  });
484  }
485 };
486 
487 } // namespace lie
488 } // namespace sophus
sophus::lie::Rotation3Impl::matrix
static auto matrix(Params const &unit_quat) -> Eigen::Matrix< Scalar, kPointDim, kPointDim >
Definition: rotation3.h:267
sophus::lie::Rotation3Impl::Scalar
TScalar Scalar
Definition: rotation3.h:20
lie_group.h
sophus::lie::Rotation3Impl
Definition: rotation3.h:18
SOPHUS_ASSERT
#define SOPHUS_ASSERT(...)
Definition: common.h:40
sophus::lie::Rotation3Impl::multiplication
static auto multiplication(Params const &lhs_params, Eigen::Vector< TCompatibleScalar, 4 > const &rhs_params) -> ParamsReturn< TCompatibleScalar >
Definition: rotation3.h:190
sophus::UnitVector::fromVectorAndNormalize
static auto fromVectorAndNormalize(Eigen::Matrix< TScalar, kN, 1 > const &v) -> UnitVector
Definition: unit_vector.h:180
sophus::lie::Rotation3Impl::kAmbientDim
static const int kAmbientDim
Definition: rotation3.h:33
sophus::lie::Rotation3Impl::hat
static auto hat(Tangent const &omega) -> Eigen::Matrix< Scalar, kAmbientDim, kAmbientDim >
Definition: rotation3.h:164
sophus::UnitVector
Definition: lie_group.h:14
sophus::lie::Rotation3Impl::kNumParams
static const int kNumParams
Definition: rotation3.h:31
sophus::lie::Rotation3Impl::inverse
static auto inverse(Params const &unit_quat) -> Params
Definition: rotation3.h:185
sophus::lie::Rotation3Impl::action
static auto action(Params const &unit_quat, UnitVector< TCompatibleScalar, 3 > const &direction_vector) -> UnitVectorReturn< TCompatibleScalar >
Definition: rotation3.h:232
sophus
Image MutImage, owning images types.
Definition: num_diff.h:20
sophus::lie::Rotation3Impl::Point
Eigen::Vector< Scalar, kPointDim > Point
Definition: rotation3.h:37
sophus::lie::Rotation3Impl::toAmbient
static auto toAmbient(Point const &point) -> Eigen::Vector< Scalar, kAmbientDim >
Definition: rotation3.h:242
sophus::lie::Rotation3Impl::Params
Eigen::Vector< Scalar, kNumParams > Params
Definition: rotation3.h:36
quaternion.h
sophus::lie::Rotation3Impl::adj
static auto adj(Params const &params) -> Eigen::Matrix< Scalar, kDof, kDof >
Definition: rotation3.h:213
sophus::lie::Rotation3Impl::Tangent
Eigen::Vector< Scalar, kDof > Tangent
Definition: rotation3.h:35
sophus::lie::Rotation3Impl::hasShortestPathAmbiguity
static auto hasShortestPathAmbiguity(Params const &foo_from_bar) -> bool
Definition: rotation3.h:78
sophus::lie::Rotation3Impl::areParamsValid
static auto areParamsValid(Params const &unit_quaternion) -> sophus::Expected< Success >
Definition: rotation3.h:61
sophus::QuaternionImpl::conjugate
static auto conjugate(Eigen::Vector< Scalar, 4 > const &a) -> Eigen::Vector< Scalar, 4 >
Definition: quaternion.h:77
sophus::lie::Rotation3Impl::kIisParallelLinePreserving
static constexpr bool kIisParallelLinePreserving
Definition: rotation3.h:28
sophus::lie::Rotation3Impl::dxExpXTimesPointAt0
static auto dxExpXTimesPointAt0(Point const &point) -> Eigen::Matrix< Scalar, kPointDim, kDof >
Definition: rotation3.h:396
sophus::lie::Rotation3Impl::adOfTranslation
static auto adOfTranslation(Point const &point) -> Eigen::Matrix< Scalar, kPointDim, kDof >
Definition: rotation3.h:325
sophus::lie::Rotation3Impl::paramsExamples
static auto paramsExamples() -> std::vector< Params >
Definition: rotation3.h:456
sophus::lie::Rotation3Impl::kIsOriginPreserving
static constexpr bool kIsOriginPreserving
Definition: rotation3.h:23
sophus::lie::Rotation3Impl::matV
static auto matV(Params const &params, Tangent const &omega) -> Eigen::Matrix< Scalar, kPointDim, kPointDim >
Definition: rotation3.h:273
SOPHUS_UNEXPECTED
#define SOPHUS_UNEXPECTED(...)
Definition: common.h:57
sophus::lie::Rotation3Impl::identityParams
static auto identityParams() -> Params
Definition: rotation3.h:56
sophus::lie::Rotation3Impl::kIsAxisDirectionPreserving
static constexpr bool kIsAxisDirectionPreserving
Definition: rotation3.h:24
sophus::lie::Rotation3Impl::tangentExamples
static auto tangentExamples() -> std::vector< Tangent >
Definition: rotation3.h:439
unit_vector.h
sophus::lie::Rotation3Impl::ParamsReturn
Eigen::Vector< ScalarReturn< TCompatibleScalar >, kNumParams > ParamsReturn
Definition: rotation3.h:45
sophus::lie::Rotation3Impl::ad
static auto ad(Tangent const &omega) -> Eigen::Matrix< Scalar, kDof, kDof >
Definition: rotation3.h:331
sophus::lie::Rotation3Impl::ScalarReturn
typename Eigen::ScalarBinaryOpTraits< Scalar, TCompatibleScalar >::ReturnType ScalarReturn
Definition: rotation3.h:41
sophus::lie::Rotation3Impl::kPointDim
static const int kPointDim
Definition: rotation3.h:32
sophus::lie::Rotation3Impl::vee
static auto vee(Eigen::Matrix< Scalar, kAmbientDim, kAmbientDim > const &mat_omega) -> Eigen::Matrix< Scalar, kDof, 1 >
Definition: rotation3.h:176
sophus::lie::Rotation3Impl::log
static auto log(Params const &unit_quaternion) -> Tangent
Definition: rotation3.h:119
sophus::lie::Rotation3Impl::invalidParamsExamples
static auto invalidParamsExamples() -> std::vector< Params >
Definition: rotation3.h:478
sophus::lie::Rotation3Impl::kIisSizePreserving
static constexpr bool kIisSizePreserving
Definition: rotation3.h:27
sophus::lie::Rotation3Impl::exp
static auto exp(Tangent const &omega) -> Params
Definition: rotation3.h:87
sophus::QuaternionImpl
Definition: quaternion.h:17
sophus::lie::Rotation3Impl::kDof
static const int kDof
Definition: rotation3.h:30
sophus::lie::Rotation3Impl::PointReturn
Eigen::Vector< ScalarReturn< TCompatibleScalar >, kPointDim > PointReturn
Definition: rotation3.h:48
sophus::lie::Rotation3Impl::dxLogThisInvTimesXAtThis
static auto dxLogThisInvTimesXAtThis(Params const &q) -> Eigen::Matrix< Scalar, kDof, kNumParams >
Definition: rotation3.h:426
sophus::lie::Rotation3Impl::kIsDirectionVectorPreserving
static constexpr bool kIsDirectionVectorPreserving
Definition: rotation3.h:25
sophus::lie::Rotation3Impl::dxExpX
static auto dxExpX(Tangent const &omega) -> Eigen::Matrix< Scalar, kNumParams, kDof >
Definition: rotation3.h:335
sophus::QuaternionImpl::multiplication
static auto multiplication(Eigen::Vector< Scalar, 4 > const &lhs, Eigen::Vector< TCompatibleScalar, 4 > const &rhs) -> ParamsReturn< TCompatibleScalar >
Definition: quaternion.h:55
sophus::lie::Rotation3Impl::action
static auto action(Params const &unit_quat, Eigen::Vector< TCompatibleScalar, kPointDim > const &point) -> PointReturn< TCompatibleScalar >
Definition: rotation3.h:220
sophus::lie::Rotation3Impl::compactMatrix
static auto compactMatrix(Params const &unit_quat) -> Eigen::Matrix< Scalar, kPointDim, kPointDim >
Definition: rotation3.h:249
sophus::lie::Rotation3Impl::adjOfTranslation
static auto adjOfTranslation(Params const &params, Point const &point) -> Eigen::Matrix< Scalar, kPointDim, kDof >
Definition: rotation3.h:320
sophus::lie::Rotation3Impl::kIsShapePreserving
static constexpr bool kIsShapePreserving
Definition: rotation3.h:26
sophus::lie::Rotation3Impl::dxThisMulExpXAt0
static auto dxThisMulExpXAt0(Params const &unit_quat) -> Eigen::Matrix< Scalar, kNumParams, kDof >
Definition: rotation3.h:401
sophus::lie::Rotation3Impl::dxExpXAt0
static auto dxExpXAt0() -> Eigen::Matrix< Scalar, kNumParams, kDof >
Definition: rotation3.h:385
sophus::lie::Rotation3Impl::matVInverse
static auto matVInverse(Params const &, Tangent const &omega) -> Eigen::Matrix< Scalar, kPointDim, kPointDim >
Definition: rotation3.h:295