so3.hpp
Go to the documentation of this file.
1 
4 #ifndef SOPHUS_SO3_HPP
5 #define SOPHUS_SO3_HPP
6 
7 #include "rotation_matrix.hpp"
8 #include "so2.hpp"
9 #include "types.hpp"
10 
11 // Include only the selective set of Eigen headers that we need.
12 // This helps when using Sophus with unusual compilers, like nvcc.
13 #include <Eigen/src/Geometry/OrthoMethods.h>
14 #include <Eigen/src/Geometry/Quaternion.h>
15 #include <Eigen/src/Geometry/RotationBase.h>
16 
17 namespace Sophus {
18 template <class Scalar_, int Options = 0>
19 class SO3;
20 using SO3d = SO3<double>;
21 using SO3f = SO3<float>;
22 } // namespace Sophus
23 
24 namespace Eigen {
25 namespace internal {
26 
27 template <class Scalar_, int Options>
28 struct traits<Sophus::SO3<Scalar_, Options>> {
29  using Scalar = Scalar_;
30  using QuaternionType = Eigen::Quaternion<Scalar, Options>;
31 };
32 
33 template <class Scalar_, int Options>
34 struct traits<Map<Sophus::SO3<Scalar_>, Options>>
35  : traits<Sophus::SO3<Scalar_, Options>> {
36  using Scalar = Scalar_;
37  using QuaternionType = Map<Eigen::Quaternion<Scalar>, Options>;
38 };
39 
40 template <class Scalar_, int Options>
41 struct traits<Map<Sophus::SO3<Scalar_> const, Options>>
42  : traits<Sophus::SO3<Scalar_, Options> const> {
43  using Scalar = Scalar_;
44  using QuaternionType = Map<Eigen::Quaternion<Scalar> const, Options>;
45 };
46 } // namespace internal
47 } // namespace Eigen
48 
49 namespace Sophus {
50 
73 template <class Derived>
74 class SO3Base {
75  public:
76  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
77  using QuaternionType =
78  typename Eigen::internal::traits<Derived>::QuaternionType;
79 
81  static int constexpr DoF = 3;
83  static int constexpr num_parameters = 4;
85  static int constexpr N = 3;
92 
93  struct TangentAndTheta {
94  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
95 
98  };
99 
104  template <typename OtherDerived>
105  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
106  Scalar, typename OtherDerived::Scalar>::ReturnType;
107 
108  template <typename OtherDerived>
110 
111  template <typename PointDerived>
113 
114  template <typename HPointDerived>
116 
118  //
122  //
125  SOPHUS_FUNC Adjoint Adj() const { return matrix(); }
126 
129  template <class S = Scalar>
132  Sophus::Matrix2<Scalar> Rx = R.template block<2, 2>(1, 1);
133  return SO2<Scalar>(makeRotationMatrix(Rx)).log();
134  }
135 
138  template <class S = Scalar>
142  // clang-format off
143  Ry <<
144  R(0, 0), R(2, 0),
145  R(0, 2), R(2, 2);
146  // clang-format on
147  return SO2<Scalar>(makeRotationMatrix(Ry)).log();
148  }
149 
152  template <class S = Scalar>
155  Sophus::Matrix2<Scalar> Rz = R.template block<2, 2>(0, 0);
156  return SO2<Scalar>(makeRotationMatrix(Rz)).log();
157  }
158 
161  template <class NewScalarType>
163  return SO3<NewScalarType>(unit_quaternion().template cast<NewScalarType>());
164  }
165 
175  return unit_quaternion_nonconst().coeffs().data();
176  }
177 
180  SOPHUS_FUNC Scalar const* data() const {
181  return unit_quaternion().coeffs().data();
182  }
183 
187  const {
189  Eigen::Quaternion<Scalar> const q = unit_quaternion();
190  Scalar const c0 = Scalar(0.5) * q.w();
191  Scalar const c1 = Scalar(0.5) * q.z();
192  Scalar const c2 = -c1;
193  Scalar const c3 = Scalar(0.5) * q.y();
194  Scalar const c4 = Scalar(0.5) * q.x();
195  Scalar const c5 = -c4;
196  Scalar const c6 = -c3;
197  J(0, 0) = c0;
198  J(0, 1) = c2;
199  J(0, 2) = c3;
200  J(1, 0) = c1;
201  J(1, 1) = c0;
202  J(1, 2) = c5;
203  J(2, 0) = c6;
204  J(2, 1) = c4;
205  J(2, 2) = c0;
206  J(3, 0) = c5;
207  J(3, 1) = c6;
208  J(3, 2) = c2;
209 
210  return J;
211  }
212 
219  return unit_quaternion().coeffs();
220  }
221 
225  return SO3<Scalar>(unit_quaternion().conjugate());
226  }
227 
238  SOPHUS_FUNC Tangent log() const { return logAndTheta().tangent; }
239 
242  SOPHUS_FUNC TangentAndTheta logAndTheta() const {
243  TangentAndTheta J;
244  using std::abs;
245  using std::atan;
246  using std::sqrt;
247  Scalar squared_n = unit_quaternion().vec().squaredNorm();
248  Scalar w = unit_quaternion().w();
249 
250  Scalar two_atan_nbyw_by_n;
251 
258 
260  // If quaternion is normalized and n=0, then w should be 1;
261  // w=0 should never happen here!
263  "Quaternion (%) should be normalized!",
264  unit_quaternion().coeffs().transpose());
265  Scalar squared_w = w * w;
266  two_atan_nbyw_by_n =
267  Scalar(2) / w - Scalar(2) * (squared_n) / (w * squared_w);
268  J.theta = Scalar(2) * squared_n / w;
269  } else {
270  Scalar n = sqrt(squared_n);
271  if (abs(w) < Constants<Scalar>::epsilon()) {
272  if (w > Scalar(0)) {
273  two_atan_nbyw_by_n = Constants<Scalar>::pi() / n;
274  } else {
275  two_atan_nbyw_by_n = -Constants<Scalar>::pi() / n;
276  }
277  } else {
278  two_atan_nbyw_by_n = Scalar(2) * atan(n / w) / n;
279  }
280  J.theta = two_atan_nbyw_by_n * n;
281  }
282 
283  J.tangent = two_atan_nbyw_by_n * unit_quaternion().vec();
284  return J;
285  }
286 
293  Scalar length = unit_quaternion_nonconst().norm();
295  "Quaternion (%) should not be close to zero!",
296  unit_quaternion_nonconst().coeffs().transpose());
297  unit_quaternion_nonconst().coeffs() /= length;
298  }
299 
306  return unit_quaternion().toRotationMatrix();
307  }
308 
311  SOPHUS_FUNC SO3Base& operator=(SO3Base const& other) = default;
312 
315  template <class OtherDerived>
318  return *this;
319  }
320 
323  template <typename OtherDerived>
325  SO3Base<OtherDerived> const& other) const {
326  using QuaternionProductType =
328  const QuaternionType& a = unit_quaternion();
329  const typename OtherDerived::QuaternionType& b = other.unit_quaternion();
333  return SO3Product<OtherDerived>(QuaternionProductType(
334  a.w() * b.w() - a.x() * b.x() - a.y() * b.y() - a.z() * b.z(),
335  a.w() * b.x() + a.x() * b.w() + a.y() * b.z() - a.z() * b.y(),
336  a.w() * b.y() + a.y() * b.w() + a.z() * b.x() - a.x() * b.z(),
337  a.w() * b.z() + a.z() * b.w() + a.x() * b.y() - a.y() * b.x()));
338  }
339 
354  template <typename PointDerived,
355  typename = typename std::enable_if<
358  Eigen::MatrixBase<PointDerived> const& p) const {
362  const QuaternionType& q = unit_quaternion();
363  PointProduct<PointDerived> uv = q.vec().cross(p);
364  uv += uv;
365  return p + q.w() * uv + q.vec().cross(uv);
366  }
367 
369  template <typename HPointDerived,
370  typename = typename std::enable_if<
373  Eigen::MatrixBase<HPointDerived> const& p) const {
374  const auto rp = *this * p.template head<3>();
375  return HomogeneousPointProduct<HPointDerived>(rp(0), rp(1), rp(2), p(3));
376  }
377 
385  SOPHUS_FUNC Line operator*(Line const& l) const {
386  return Line((*this) * l.origin(), (*this) * l.direction());
387  }
388 
392  template <typename OtherDerived,
393  typename = typename std::enable_if<
394  std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
396  *static_cast<Derived*>(this) = *this * other;
397  return *this;
398  }
399 
404  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quaternion) {
405  unit_quaternion_nonconst() = quaternion;
406  normalize();
407  }
408 
412  return static_cast<Derived const*>(this)->unit_quaternion();
413  }
414 
415  private:
420  return static_cast<Derived*>(this)->unit_quaternion_nonconst();
421  }
422 };
423 
425 template <class Scalar_, int Options>
426 class SO3 : public SO3Base<SO3<Scalar_, Options>> {
427  public:
429  static int constexpr DoF = Base::DoF;
430  static int constexpr num_parameters = Base::num_parameters;
431 
432  using Scalar = Scalar_;
434  using Point = typename Base::Point;
436  using Tangent = typename Base::Tangent;
437  using Adjoint = typename Base::Adjoint;
438  using QuaternionMember = Eigen::Quaternion<Scalar, Options>;
439 
442  friend class SO3Base<SO3<Scalar, Options>>;
443 
444  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
445 
449  : unit_quaternion_(Scalar(1), Scalar(0), Scalar(0), Scalar(0)) {}
450 
453  SOPHUS_FUNC SO3(SO3 const& other) = default;
454 
457  template <class OtherDerived>
459  : unit_quaternion_(other.unit_quaternion()) {}
460 
467  SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %",
468  R * R.transpose());
469  SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %",
470  R.determinant());
471  }
472 
477  template <class D>
478  SOPHUS_FUNC explicit SO3(Eigen::QuaternionBase<D> const& quat)
479  : unit_quaternion_(quat) {
480  static_assert(
481  std::is_same<typename Eigen::QuaternionBase<D>::Scalar, Scalar>::value,
482  "Input must be of same scalar type");
483  Base::normalize();
484  }
485 
489  return unit_quaternion_;
490  }
491 
495  Tangent const& omega) {
496  using std::cos;
497  using std::exp;
498  using std::sin;
499  using std::sqrt;
500  Scalar const c0 = omega[0] * omega[0];
501  Scalar const c1 = omega[1] * omega[1];
502  Scalar const c2 = omega[2] * omega[2];
503  Scalar const c3 = c0 + c1 + c2;
504 
505  if (c3 < Constants<Scalar>::epsilon()) {
506  return Dx_exp_x_at_0();
507  }
508 
509  Scalar const c4 = sqrt(c3);
510  Scalar const c5 = 1.0 / c4;
511  Scalar const c6 = 0.5 * c4;
512  Scalar const c7 = sin(c6);
513  Scalar const c8 = c5 * c7;
514  Scalar const c9 = pow(c3, -3.0L / 2.0L);
515  Scalar const c10 = c7 * c9;
516  Scalar const c11 = Scalar(1.0) / c3;
517  Scalar const c12 = cos(c6);
518  Scalar const c13 = Scalar(0.5) * c11 * c12;
519  Scalar const c14 = c7 * c9 * omega[0];
520  Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0];
521  Scalar const c16 = -c14 * omega[1] + c15 * omega[1];
522  Scalar const c17 = -c14 * omega[2] + c15 * omega[2];
523  Scalar const c18 = omega[1] * omega[2];
524  Scalar const c19 = -c10 * c18 + c13 * c18;
525  Scalar const c20 = Scalar(0.5) * c5 * c7;
527  J(0, 0) = -c0 * c10 + c0 * c13 + c8;
528  J(0, 1) = c16;
529  J(0, 2) = c17;
530  J(1, 0) = c16;
531  J(1, 1) = -c1 * c10 + c1 * c13 + c8;
532  J(1, 2) = c19;
533  J(2, 0) = c17;
534  J(2, 1) = c19;
535  J(2, 2) = -c10 * c2 + c13 * c2 + c8;
536  J(3, 0) = -c20 * omega[0];
537  J(3, 1) = -c20 * omega[1];
538  J(3, 2) = -c20 * omega[2];
539  return J;
540  }
541 
547  // clang-format off
548  J << Scalar(0.5), Scalar(0), Scalar(0),
549  Scalar(0), Scalar(0.5), Scalar(0),
550  Scalar(0), Scalar(0), Scalar(0.5),
551  Scalar(0), Scalar(0), Scalar(0);
552  // clang-format on
553  return J;
554  }
555 
559  return generator(i);
560  }
561 
571  SOPHUS_FUNC static SO3<Scalar> exp(Tangent const& omega) {
572  Scalar theta;
573  return expAndTheta(omega, &theta);
574  }
575 
581  Scalar* theta) {
582  SOPHUS_ENSURE(theta != nullptr, "must not be nullptr.");
583  using std::abs;
584  using std::cos;
585  using std::sin;
586  using std::sqrt;
587  Scalar theta_sq = omega.squaredNorm();
588 
589  Scalar imag_factor;
590  Scalar real_factor;
591  if (theta_sq <
593  *theta = Scalar(0);
594  Scalar theta_po4 = theta_sq * theta_sq;
595  imag_factor = Scalar(0.5) - Scalar(1.0 / 48.0) * theta_sq +
596  Scalar(1.0 / 3840.0) * theta_po4;
597  real_factor = Scalar(1) - Scalar(1.0 / 8.0) * theta_sq +
598  Scalar(1.0 / 384.0) * theta_po4;
599  } else {
600  *theta = sqrt(theta_sq);
601  Scalar half_theta = Scalar(0.5) * (*theta);
602  Scalar sin_half_theta = sin(half_theta);
603  imag_factor = sin_half_theta / (*theta);
604  real_factor = cos(half_theta);
605  }
606 
607  SO3 q;
608  q.unit_quaternion_nonconst() =
609  QuaternionMember(real_factor, imag_factor * omega.x(),
610  imag_factor * omega.y(), imag_factor * omega.z());
611  SOPHUS_ENSURE(abs(q.unit_quaternion().squaredNorm() - Scalar(1)) <
613  "SO3::exp failed! omega: %, real: %, img: %",
614  omega.transpose(), real_factor, imag_factor);
615  return q;
616  }
617 
620  template <class S = Scalar>
623  return SO3(::Sophus::makeRotationMatrix(R));
624  }
625 
647  SOPHUS_ENSURE(i >= 0 && i <= 2, "i should be in range [0,2].");
648  Tangent e;
649  e.setZero();
650  e[i] = Scalar(1);
651  return hat(e);
652  }
653 
668  SOPHUS_FUNC static Transformation hat(Tangent const& omega) {
669  Transformation Omega;
670  // clang-format off
671  Omega <<
672  Scalar(0), -omega(2), omega(1),
673  omega(2), Scalar(0), -omega(0),
674  -omega(1), omega(0), Scalar(0);
675  // clang-format on
676  return Omega;
677  }
678 
692  SOPHUS_FUNC static Tangent lieBracket(Tangent const& omega1,
693  Tangent const& omega2) {
694  return omega1.cross(omega2);
695  }
696 
699  static SOPHUS_FUNC SO3 rotX(Scalar const& x) {
700  return SO3::exp(Sophus::Vector3<Scalar>(x, Scalar(0), Scalar(0)));
701  }
702 
705  static SOPHUS_FUNC SO3 rotY(Scalar const& y) {
706  return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), y, Scalar(0)));
707  }
708 
711  static SOPHUS_FUNC SO3 rotZ(Scalar const& z) {
712  return SO3::exp(Sophus::Vector3<Scalar>(Scalar(0), Scalar(0), z));
713  }
714 
717  template <class UniformRandomBitGenerator>
718  static SO3 sampleUniform(UniformRandomBitGenerator& generator) {
720  "generator must meet the UniformRandomBitGenerator concept");
721  std::uniform_real_distribution<Scalar> uniform(-Constants<Scalar>::pi(),
723  std::normal_distribution<Scalar> normal(0, 1);
725  Scalar nrm;
726  do {
727  axis.x() = normal(generator);
728  axis.y() = normal(generator);
729  axis.z() = normal(generator);
730  nrm = axis.norm();
731  } while (nrm < Constants<Scalar>::epsilon());
732  axis /= nrm;
733  return SO3::exp(uniform(generator) * axis);
734  }
735 
749  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
750  return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0));
751  }
752 
753  protected:
757  return unit_quaternion_;
758  }
759 
761 };
762 
763 } // namespace Sophus
764 
765 namespace Eigen {
770 template <class Scalar_, int Options>
771 class Map<Sophus::SO3<Scalar_>, Options>
772  : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>> {
773  public:
775  using Scalar = Scalar_;
777  using Point = typename Base::Point;
779  using Tangent = typename Base::Tangent;
780  using Adjoint = typename Base::Adjoint;
781 
784  friend class Sophus::SO3Base<Map<Sophus::SO3<Scalar_>, Options>>;
785 
786  // LCOV_EXCL_START
787  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map);
788  // LCOV_EXCL_STOP
789 
790  using Base::operator*=;
791  using Base::operator*;
792 
793  SOPHUS_FUNC Map(Scalar* coeffs) : unit_quaternion_(coeffs) {}
794 
797  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options> const& unit_quaternion()
798  const {
799  return unit_quaternion_;
800  }
801 
802  protected:
805  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>&
807  return unit_quaternion_;
808  }
809 
810  Map<Eigen::Quaternion<Scalar>, Options> unit_quaternion_;
811 };
812 
817 template <class Scalar_, int Options>
818 class Map<Sophus::SO3<Scalar_> const, Options>
819  : public Sophus::SO3Base<Map<Sophus::SO3<Scalar_> const, Options>> {
820  public:
822  using Scalar = Scalar_;
824  using Point = typename Base::Point;
826  using Tangent = typename Base::Tangent;
827  using Adjoint = typename Base::Adjoint;
828 
829  using Base::operator*=;
830  using Base::operator*;
831 
832  SOPHUS_FUNC Map(Scalar const* coeffs) : unit_quaternion_(coeffs) {}
833 
836  SOPHUS_FUNC Map<Eigen::Quaternion<Scalar> const, Options> const&
837  unit_quaternion() const {
838  return unit_quaternion_;
839  }
840 
841  protected:
844  Map<Eigen::Quaternion<Scalar> const, Options> const unit_quaternion_;
845 };
846 } // namespace Eigen
847 
848 #endif
Sophus::SO3Base::TangentAndTheta::tangent
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Tangent tangent
Definition: so3.hpp:96
Sophus::SO3::DoF
static constexpr int DoF
Definition: so3.hpp:429
c1
const Scalar c1
Definition: Se2_Dx_exp_x.cpp:2
Sophus::SO2
SO2 using default storage; derived from SO2Base.
Definition: so2.hpp:19
Eigen
Definition: rxso2.hpp:16
Eigen::Map< Sophus::SO3< Scalar_ > const, Options >::Map
SOPHUS_FUNC Map(Scalar const *coeffs)
Definition: so3.hpp:832
Sophus::SO3::rotX
static SOPHUS_FUNC SO3 rotX(Scalar const &x)
Definition: so3.hpp:699
Sophus::SO3Base::inverse
SOPHUS_FUNC SO3< Scalar > inverse() const
Definition: so3.hpp:224
Eigen::Map< Sophus::SO3< Scalar_ > const, Options >::unit_quaternion
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar > const, Options > const & unit_quaternion() const
Definition: so3.hpp:837
Eigen::internal::traits< Sophus::SO3< Scalar_, Options > >::Scalar
Scalar_ Scalar
Definition: so3.hpp:29
Sophus::SO3::num_parameters
static constexpr int num_parameters
Definition: so3.hpp:430
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::Scalar
typename Eigen::internal::traits< Map< Sophus::SO3< Scalar_ > const, Options > >::Scalar Scalar
Definition: so3.hpp:76
Sophus::SO3
SO3 using default storage; derived from SO3Base.
Definition: so3.hpp:19
Sophus::SO3::unit_quaternion_nonconst
SOPHUS_FUNC QuaternionMember & unit_quaternion_nonconst()
Definition: so3.hpp:756
Eigen::Map< Sophus::SO3< Scalar_ >, Options >::unit_quaternion_nonconst
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > & unit_quaternion_nonconst()
Definition: so3.hpp:806
Eigen::Map< Sophus::SO3< Scalar_ >, Options >::unit_quaternion_
Map< Eigen::Quaternion< Scalar >, Options > unit_quaternion_
Definition: so3.hpp:810
Sophus::SO3::vee
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Definition: so3.hpp:749
Sophus::SO3Base::data
SOPHUS_FUNC Scalar const * data() const
Definition: so3.hpp:180
Eigen::Map< Sophus::SO3< Scalar_ > const, Options >::Transformation
typename Base::Transformation Transformation
Definition: so3.hpp:823
Sophus::SO3::exp
static SOPHUS_FUNC SO3< Scalar > exp(Tangent const &omega)
Definition: so3.hpp:571
types.hpp
c19
const Scalar c19
Definition: Se3_Dx_exp_x.cpp:20
Eigen::Map< Sophus::SO3< Scalar_ > const, Options >::Scalar
Scalar_ Scalar
Definition: so3.hpp:822
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::HomogeneousPointProduct
Vector4< ReturnScalar< HPointDerived > > HomogeneousPointProduct
Definition: so3.hpp:115
c17
const Scalar c17
Definition: Se3_Dx_exp_x.cpp:18
Sophus::Vector3
Vector< Scalar, 3, Options > Vector3
Definition: types.hpp:21
Sophus::SO3Base::N
static constexpr int N
Group transformations are 3x3 matrices.
Definition: so3.hpp:85
c20
const Scalar c20
Definition: Se3_Dx_exp_x.cpp:21
Sophus::enable_if_t
typename std::enable_if< B, T >::type enable_if_t
Definition: common.hpp:221
Sophus::Matrix2
Matrix< Scalar, 2, 2 > Matrix2
Definition: types.hpp:44
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::Tangent
Vector< Scalar, DoF > Tangent
Definition: so3.hpp:90
SOPHUS_ENSURE
#define SOPHUS_ENSURE(expr,...)
Definition: common.hpp:137
Sophus::SO3< Scalar, Options >::Tangent
typename Base::Tangent Tangent
Definition: so3.hpp:436
Sophus::SO3Base::TangentAndTheta::theta
Scalar theta
Definition: so3.hpp:97
Sophus::SO3Base::operator*
SOPHUS_FUNC HomogeneousPointProduct< HPointDerived > operator*(Eigen::MatrixBase< HPointDerived > const &p) const
Group action on homogeneous 3-points. See above for more details.
Definition: so3.hpp:372
Sophus::SO3Base::Dx_this_mul_exp_x_at_0
SOPHUS_FUNC Matrix< Scalar, num_parameters, DoF > Dx_this_mul_exp_x_at_0() const
Definition: so3.hpp:186
c18
const Scalar c18
Definition: Se3_Dx_exp_x.cpp:19
Eigen::Map< Sophus::SO3< Scalar_ > const, Options >::unit_quaternion_
const Map< Eigen::Quaternion< Scalar > const, Options > unit_quaternion_
Definition: so3.hpp:844
c7
const Scalar c7
Definition: Se2_Dx_exp_x.cpp:8
c14
const Scalar c14
Definition: Se3_Dx_exp_x.cpp:15
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::Transformation
Matrix< Scalar, N, N > Transformation
Definition: so3.hpp:86
Eigen::Map< Sophus::SO3< Scalar_ > const, Options >::Adjoint
typename Base::Adjoint Adjoint
Definition: so3.hpp:827
Sophus::ParametrizedLine3
ParametrizedLine< Scalar, 3, Options > ParametrizedLine3
Definition: types.hpp:72
Sophus::SO3::SO3
SOPHUS_FUNC SO3(Transformation const &R)
Definition: so3.hpp:466
Sophus::SO3Base::log
SOPHUS_FUNC Tangent log() const
Definition: so3.hpp:238
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::QuaternionType
typename Eigen::internal::traits< Map< Sophus::SO3< Scalar_ > const, Options > >::QuaternionType QuaternionType
Definition: so3.hpp:78
Sophus::SO3< Scalar, Options >::Transformation
typename Base::Transformation Transformation
Definition: so3.hpp:433
Sophus::SO3Base::logAndTheta
SOPHUS_FUNC TangentAndTheta logAndTheta() const
Definition: so3.hpp:242
c16
const Scalar c16
Definition: Se3_Dx_exp_x.cpp:17
Eigen::Map< Sophus::SO3< Scalar_ >, Options >::Scalar
Scalar_ Scalar
Definition: so3.hpp:775
Eigen::Map< Sophus::SO3< Scalar_ > const, Options >::Tangent
typename Base::Tangent Tangent
Definition: so3.hpp:826
Sophus::SO3< Scalar, Options >::Adjoint
typename Base::Adjoint Adjoint
Definition: so3.hpp:437
Sophus::SO3Base::operator*=
SOPHUS_FUNC SO3Base< Derived > & operator*=(SO3Base< OtherDerived > const &other)
Definition: so3.hpp:395
Sophus::Matrix
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:41
Sophus
Definition: average.hpp:17
Sophus::SO3Base::operator*
SOPHUS_FUNC SO3Product< OtherDerived > operator*(SO3Base< OtherDerived > const &other) const
Definition: so3.hpp:324
Sophus::SO3::hat
static SOPHUS_FUNC Transformation hat(Tangent const &omega)
Definition: so3.hpp:668
SOPHUS_FUNC
#define SOPHUS_FUNC
Definition: common.hpp:37
c2
const Scalar c2
Definition: Se2_Dx_exp_x.cpp:3
Sophus::SO3< Scalar, Options >::Scalar
Scalar Scalar
Definition: so3.hpp:432
rotation_matrix.hpp
Sophus::isOrthogonal
SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase< D > const &R)
Definition: rotation_matrix.hpp:17
Sophus::SO3Base::Adj
SOPHUS_FUNC Adjoint Adj() const
Adjoint transformation.
Definition: so3.hpp:125
Sophus::SO3::Dx_exp_x
static SOPHUS_FUNC Sophus::Matrix< Scalar, num_parameters, DoF > Dx_exp_x(Tangent const &omega)
Definition: so3.hpp:494
Sophus::SO3Base::num_parameters
static constexpr int num_parameters
Number of internal parameters used (quaternion is a 4-tuple).
Definition: so3.hpp:83
Sophus::transpose
auto transpose(T const &p) -> decltype(details::Transpose< T >::impl(T()))
Definition: types.hpp:195
Sophus::SO3Base::operator=
SOPHUS_FUNC SO3Base & operator=(SO3Base const &other)=default
c9
const Scalar c9
Definition: Se2_Dx_exp_x.cpp:10
Sophus::SO3Base::data
SOPHUS_FUNC Scalar * data()
Definition: so3.hpp:174
Sophus::SO3::rotY
static SOPHUS_FUNC SO3 rotY(Scalar const &y)
Definition: so3.hpp:705
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::HomogeneousPoint
Vector4< Scalar > HomogeneousPoint
Definition: so3.hpp:88
Sophus::SO3Base::TangentAndTheta
Definition: so3.hpp:93
Sophus::makeRotationMatrix
SOPHUS_FUNC enable_if_t< std::is_floating_point< typename D::Scalar >::value, Matrix< typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime > > makeRotationMatrix(Eigen::MatrixBase< D > const &R)
Definition: rotation_matrix.hpp:62
c3
const Scalar c3
Definition: Se2_Dx_exp_x.cpp:4
Sophus::SO3::sampleUniform
static SO3 sampleUniform(UniformRandomBitGenerator &generator)
Definition: so3.hpp:718
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::Line
ParametrizedLine3< Scalar > Line
Definition: so3.hpp:89
Sophus::SO3::fitToSO3
static SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, SO3 > fitToSO3(Transformation const &R)
Definition: so3.hpp:622
Sophus::Constants::pi
static SOPHUS_FUNC Scalar pi()
Definition: common.hpp:154
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::Point
Vector3< Scalar > Point
Definition: so3.hpp:87
Sophus::SO3Base::ReturnScalar
typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar >::ReturnType ReturnScalar
Definition: so3.hpp:106
Sophus::IsFixedSizeVector
Definition: types.hpp:225
Sophus::SO3Base::operator=
SOPHUS_FUNC SO3Base< Derived > & operator=(SO3Base< OtherDerived > const &other)
Definition: so3.hpp:316
Sophus::SO3::rotZ
static SOPHUS_FUNC SO3 rotZ(Scalar const &z)
Definition: so3.hpp:711
Sophus::SO3Base::normalize
SOPHUS_FUNC void normalize()
Definition: so3.hpp:292
Sophus::SO3::SO3
SOPHUS_FUNC SO3(SO3Base< OtherDerived > const &other)
Definition: so3.hpp:458
Sophus::Constants
Definition: common.hpp:146
Sophus::Matrix3
Matrix< Scalar, 3, 3 > Matrix3
Definition: types.hpp:49
c10
const Scalar c10
Definition: Se3_Dx_exp_x.cpp:11
Sophus::SO3Base
Definition: so3.hpp:74
Eigen::Map< Sophus::SO3< Scalar_ > const, Options >::Point
typename Base::Point Point
Definition: so3.hpp:824
Sophus::SO3< Scalar, Options >::HomogeneousPoint
typename Base::HomogeneousPoint HomogeneousPoint
Definition: so3.hpp:435
Sophus::Vector4
Vector< Scalar, 4 > Vector4
Definition: types.hpp:26
Sophus::SO3Base::unit_quaternion
SOPHUS_FUNC QuaternionType const & unit_quaternion() const
Definition: so3.hpp:411
Sophus::SO3::Dxi_exp_x_matrix_at_0
static SOPHUS_FUNC Transformation Dxi_exp_x_matrix_at_0(int i)
Definition: so3.hpp:558
Eigen::Map< Sophus::SO3< Scalar_ > const, Options >::HomogeneousPoint
typename Base::HomogeneousPoint HomogeneousPoint
Definition: so3.hpp:825
Eigen::internal::traits< Map< Sophus::SO3< Scalar_ > const, Options > >::QuaternionType
Map< Eigen::Quaternion< Scalar > const, Options > QuaternionType
Definition: so3.hpp:44
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::PointProduct
Vector3< ReturnScalar< PointDerived > > PointProduct
Definition: so3.hpp:112
c6
const Scalar c6
Definition: Se2_Dx_exp_x.cpp:7
Sophus::SO3::generator
static SOPHUS_FUNC Transformation generator(int i)
Definition: so3.hpp:646
Sophus::SO3< Scalar, Options >::Point
typename Base::Point Point
Definition: so3.hpp:434
Sophus::SO3Base::angleY
SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, S > angleY() const
Definition: so3.hpp:139
Sophus::SO3< Scalar, Options >::QuaternionMember
Eigen::Quaternion< Scalar, Options > QuaternionMember
Definition: so3.hpp:438
Sophus::Vector
Eigen::Matrix< Scalar, M, 1, Options > Vector
Definition: types.hpp:13
Eigen::Map< Sophus::SO3< Scalar_ >, Options >::unit_quaternion
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > const & unit_quaternion() const
Definition: so3.hpp:797
Sophus::SO3::lieBracket
static SOPHUS_FUNC Tangent lieBracket(Tangent const &omega1, Tangent const &omega2)
Definition: so3.hpp:692
Sophus::SO3Base::operator*
SOPHUS_FUNC PointProduct< PointDerived > operator*(Eigen::MatrixBase< PointDerived > const &p) const
Definition: so3.hpp:357
Sophus::SO3Base::setQuaternion
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion< Scalar > const &quaternion)
Definition: so3.hpp:404
Sophus::SO3Base::unit_quaternion_nonconst
SOPHUS_FUNC QuaternionType & unit_quaternion_nonconst()
Definition: so3.hpp:419
Sophus::SO3::SO3
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SO3()
Definition: so3.hpp:448
Sophus::SO3Base::matrix
SOPHUS_FUNC Transformation matrix() const
Definition: so3.hpp:305
Sophus::IsUniformRandomBitGenerator
Definition: common.hpp:224
Sophus::SO3Base::cast
SOPHUS_FUNC SO3< NewScalarType > cast() const
Definition: so3.hpp:162
Sophus::SO3::expAndTheta
static SOPHUS_FUNC SO3< Scalar > expAndTheta(Tangent const &omega, Scalar *theta)
Definition: so3.hpp:580
Eigen::internal::traits< Sophus::SO3< Scalar_, Options > >::QuaternionType
Eigen::Quaternion< Scalar, Options > QuaternionType
Definition: so3.hpp:30
c11
const Scalar c11
Definition: Se3_Dx_exp_x.cpp:12
c5
const Scalar c5
Definition: Se2_Dx_exp_x.cpp:6
Sophus::SO3::SO3
SOPHUS_FUNC SO3(Eigen::QuaternionBase< D > const &quat)
Definition: so3.hpp:478
Sophus::SO3Base::params
SOPHUS_FUNC Sophus::Vector< Scalar, num_parameters > params() const
Definition: so3.hpp:218
Sophus::SO3::unit_quaternion
SOPHUS_FUNC QuaternionMember const & unit_quaternion() const
Definition: so3.hpp:488
Sophus::SO3Base::angleX
SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, S > angleX() const
Definition: so3.hpp:130
Sophus::SO3Base::operator*
SOPHUS_FUNC Line operator*(Line const &l) const
Definition: so3.hpp:385
c4
const Scalar c4
Definition: Se2_Dx_exp_x.cpp:5
c13
const Scalar c13
Definition: Se3_Dx_exp_x.cpp:14
Sophus::SO3Base::angleZ
SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, S > angleZ() const
Definition: so3.hpp:153
so2.hpp
c15
const Scalar c15
Definition: Se3_Dx_exp_x.cpp:16
Sophus::SO3::Dx_exp_x_at_0
static SOPHUS_FUNC Sophus::Matrix< Scalar, num_parameters, DoF > Dx_exp_x_at_0()
Definition: so3.hpp:545
Eigen::internal::traits< Map< Sophus::SO3< Scalar_ > const, Options > >::Scalar
Scalar_ Scalar
Definition: so3.hpp:43
Sophus::SO3Base::DoF
static constexpr int DoF
Degrees of freedom of group, number of dimensions in tangent space.
Definition: so3.hpp:81
Sophus::SO3::unit_quaternion_
QuaternionMember unit_quaternion_
Definition: so3.hpp:760
c0
const Scalar c0
Definition: Se2_Dx_exp_x.cpp:1
c8
const Scalar c8
Definition: Se2_Dx_exp_x.cpp:9
c12
const Scalar c12
Definition: Se3_Dx_exp_x.cpp:13
Sophus::SO3Base< Map< Sophus::SO3< Scalar_ > const, Options > >::Adjoint
Matrix< Scalar, DoF, DoF > Adjoint
Definition: so3.hpp:91


sophus
Author(s): Hauke Strasdat
autogenerated on Thu Jun 11 2020 03:39:48