se3.hpp
Go to the documentation of this file.
1 
4 #ifndef SOPHUS_SE3_HPP
5 #define SOPHUS_SE3_HPP
6 
7 #include "so3.hpp"
8 
9 namespace Sophus {
10 template <class Scalar_, int Options = 0>
11 class SE3;
12 using SE3d = SE3<double>;
13 using SE3f = SE3<float>;
14 } // namespace Sophus
15 
16 namespace Eigen {
17 namespace internal {
18 
19 template <class Scalar_, int Options>
20 struct traits<Sophus::SE3<Scalar_, Options>> {
21  using Scalar = Scalar_;
24 };
25 
26 template <class Scalar_, int Options>
27 struct traits<Map<Sophus::SE3<Scalar_>, Options>>
28  : traits<Sophus::SE3<Scalar_, Options>> {
29  using Scalar = Scalar_;
30  using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
31  using SO3Type = Map<Sophus::SO3<Scalar>, Options>;
32 };
33 
34 template <class Scalar_, int Options>
35 struct traits<Map<Sophus::SE3<Scalar_> const, Options>>
36  : traits<Sophus::SE3<Scalar_, Options> const> {
37  using Scalar = Scalar_;
38  using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
39  using SO3Type = Map<Sophus::SO3<Scalar> const, Options>;
40 };
41 } // namespace internal
42 } // namespace Eigen
43 
44 namespace Sophus {
45 
57 template <class Derived>
58 class SE3Base {
59  public:
60  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
61  using TranslationType =
62  typename Eigen::internal::traits<Derived>::TranslationType;
63  using SO3Type = typename Eigen::internal::traits<Derived>::SO3Type;
64  using QuaternionType = typename SO3Type::QuaternionType;
67  static int constexpr DoF = 6;
70  static int constexpr num_parameters = 7;
72  static int constexpr N = 4;
79 
84  template <typename OtherDerived>
85  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
86  Scalar, typename OtherDerived::Scalar>::ReturnType;
87 
88  template <typename OtherDerived>
90 
91  template <typename PointDerived>
93 
94  template <typename HPointDerived>
96 
104  Sophus::Matrix3<Scalar> const R = so3().matrix();
105  Adjoint res;
106  res.block(0, 0, 3, 3) = R;
107  res.block(3, 3, 3, 3) = R;
108  res.block(0, 3, 3, 3) = SO3<Scalar>::hat(translation()) * R;
109  res.block(3, 0, 3, 3) = Matrix3<Scalar>::Zero(3, 3);
110  return res;
111  }
112 
115  Scalar angleX() const { return so3().angleX(); }
116 
119  Scalar angleY() const { return so3().angleY(); }
120 
123  Scalar angleZ() const { return so3().angleZ(); }
124 
127  template <class NewScalarType>
129  return SE3<NewScalarType>(so3().template cast<NewScalarType>(),
130  translation().template cast<NewScalarType>());
131  }
132 
136  const {
138  Eigen::Quaternion<Scalar> const q = unit_quaternion();
139  Scalar const c0 = Scalar(0.5) * q.w();
140  Scalar const c1 = Scalar(0.5) * q.z();
141  Scalar const c2 = -c1;
142  Scalar const c3 = Scalar(0.5) * q.y();
143  Scalar const c4 = Scalar(0.5) * q.x();
144  Scalar const c5 = -c4;
145  Scalar const c6 = -c3;
146  Scalar const c7 = q.w() * q.w();
147  Scalar const c8 = q.x() * q.x();
148  Scalar const c9 = q.y() * q.y();
149  Scalar const c10 = -c9;
150  Scalar const c11 = q.z() * q.z();
151  Scalar const c12 = -c11;
152  Scalar const c13 = Scalar(2) * q.w();
153  Scalar const c14 = c13 * q.z();
154  Scalar const c15 = Scalar(2) * q.x();
155  Scalar const c16 = c15 * q.y();
156  Scalar const c17 = c13 * q.y();
157  Scalar const c18 = c15 * q.z();
158  Scalar const c19 = c7 - c8;
159  Scalar const c20 = c13 * q.x();
160  Scalar const c21 = Scalar(2) * q.y() * q.z();
161  J(0, 0) = 0;
162  J(0, 1) = 0;
163  J(0, 2) = 0;
164  J(0, 3) = c0;
165  J(0, 4) = c2;
166  J(0, 5) = c3;
167  J(1, 0) = 0;
168  J(1, 1) = 0;
169  J(1, 2) = 0;
170  J(1, 3) = c1;
171  J(1, 4) = c0;
172  J(1, 5) = c5;
173  J(2, 0) = 0;
174  J(2, 1) = 0;
175  J(2, 2) = 0;
176  J(2, 3) = c6;
177  J(2, 4) = c4;
178  J(2, 5) = c0;
179  J(3, 0) = 0;
180  J(3, 1) = 0;
181  J(3, 2) = 0;
182  J(3, 3) = c5;
183  J(3, 4) = c6;
184  J(3, 5) = c2;
185  J(4, 0) = c10 + c12 + c7 + c8;
186  J(4, 1) = -c14 + c16;
187  J(4, 2) = c17 + c18;
188  J(4, 3) = 0;
189  J(4, 4) = 0;
190  J(4, 5) = 0;
191  J(5, 0) = c14 + c16;
192  J(5, 1) = c12 + c19 + c9;
193  J(5, 2) = -c20 + c21;
194  J(5, 3) = 0;
195  J(5, 4) = 0;
196  J(5, 5) = 0;
197  J(6, 0) = -c17 + c18;
198  J(6, 1) = c20 + c21;
199  J(6, 2) = c10 + c11 + c19;
200  J(6, 3) = 0;
201  J(6, 4) = 0;
202  J(6, 5) = 0;
203  return J;
204  }
205 
209  SO3<Scalar> invR = so3().inverse();
210  return SE3<Scalar>(invR, invR * (translation() * Scalar(-1)));
211  }
212 
224  // For the derivation of the logarithm of SE(3), see
225  // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices
226  // and logarithms of orthogonal matrices", IJRA 2002.
227  // https:///pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
228  // (Sec. 6., pp. 8)
229  using std::abs;
230  using std::cos;
231  using std::sin;
232  Tangent upsilon_omega;
233  auto omega_and_theta = so3().logAndTheta();
234  Scalar theta = omega_and_theta.theta;
235  upsilon_omega.template tail<3>() = omega_and_theta.tangent;
236  Matrix3<Scalar> const Omega =
237  SO3<Scalar>::hat(upsilon_omega.template tail<3>());
238 
239  if (abs(theta) < Constants<Scalar>::epsilon()) {
241  Scalar(0.5) * Omega +
242  Scalar(1. / 12.) * (Omega * Omega);
243 
244  upsilon_omega.template head<3>() = V_inv * translation();
245  } else {
246  Scalar const half_theta = Scalar(0.5) * theta;
247 
248  Matrix3<Scalar> const V_inv =
249  (Matrix3<Scalar>::Identity() - Scalar(0.5) * Omega +
250  (Scalar(1) -
251  theta * cos(half_theta) / (Scalar(2) * sin(half_theta))) /
252  (theta * theta) * (Omega * Omega));
253  upsilon_omega.template head<3>() = V_inv * translation();
254  }
255  return upsilon_omega;
256  }
257 
263  SOPHUS_FUNC void normalize() { so3().normalize(); }
264 
276  Transformation homogenious_matrix;
277  homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
278  homogenious_matrix.row(3) =
280  return homogenious_matrix;
281  }
282 
287  matrix.template topLeftCorner<3, 3>() = rotationMatrix();
288  matrix.col(3) = translation();
289  return matrix;
290  }
291 
294  SOPHUS_FUNC SE3Base& operator=(SE3Base const& other) = default;
295 
298  template <class OtherDerived>
300  so3() = other.so3();
301  translation() = other.translation();
302  return *this;
303  }
304 
307  template <typename OtherDerived>
309  SE3Base<OtherDerived> const& other) const {
311  so3() * other.so3(), translation() + so3() * other.translation());
312  }
313 
322  template <typename PointDerived,
323  typename = typename std::enable_if<
326  Eigen::MatrixBase<PointDerived> const& p) const {
327  return so3() * p + translation();
328  }
329 
332  template <typename HPointDerived,
333  typename = typename std::enable_if<
336  Eigen::MatrixBase<HPointDerived> const& p) const {
337  const PointProduct<HPointDerived> tp =
338  so3() * p.template head<3>() + p(3) * translation();
339  return HomogeneousPointProduct<HPointDerived>(tp(0), tp(1), tp(2), p(3));
340  }
341 
350  SOPHUS_FUNC Line operator*(Line const& l) const {
351  return Line((*this) * l.origin(), so3() * l.direction());
352  }
353 
357  template <typename OtherDerived,
358  typename = typename std::enable_if<
359  std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
361  *static_cast<Derived*>(this) = *this * other;
362  return *this;
363  }
364 
367  SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const { return so3().matrix(); }
368 
371  SOPHUS_FUNC SO3Type& so3() { return static_cast<Derived*>(this)->so3(); }
372 
375  SOPHUS_FUNC SO3Type const& so3() const {
376  return static_cast<const Derived*>(this)->so3();
377  }
378 
383  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
384  so3().setQuaternion(quat);
385  }
386 
392  SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R);
393  SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %",
394  R.determinant());
395  so3().setQuaternion(Eigen::Quaternion<Scalar>(R));
396  }
397 
405  p << so3().params(), translation();
406  return p;
407  }
408 
412  return static_cast<Derived*>(this)->translation();
413  }
414 
418  return static_cast<Derived const*>(this)->translation();
419  }
420 
424  return this->so3().unit_quaternion();
425  }
426 };
427 
429 template <class Scalar_, int Options>
430 class SE3 : public SE3Base<SE3<Scalar_, Options>> {
432 
433  public:
434  static int constexpr DoF = Base::DoF;
435  static int constexpr num_parameters = Base::num_parameters;
436 
437  using Scalar = Scalar_;
439  using Point = typename Base::Point;
441  using Tangent = typename Base::Tangent;
442  using Adjoint = typename Base::Adjoint;
445 
446  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
447 
450  SOPHUS_FUNC SE3();
451 
454  SOPHUS_FUNC SE3(SE3 const& other) = default;
455 
458  template <class OtherDerived>
460  : so3_(other.so3()), translation_(other.translation()) {
461  static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
462  "must be same Scalar type");
463  }
464 
467  template <class OtherDerived, class D>
469  Eigen::MatrixBase<D> const& translation)
471  static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
472  "must be same Scalar type");
473  static_assert(std::is_same<typename D::Scalar, Scalar>::value,
474  "must be same Scalar type");
475  }
476 
483  SE3(Matrix3<Scalar> const& rotation_matrix, Point const& translation)
484  : so3_(rotation_matrix), translation_(translation) {}
485 
490  SOPHUS_FUNC SE3(Eigen::Quaternion<Scalar> const& quaternion,
491  Point const& translation)
492  : so3_(quaternion), translation_(translation) {}
493 
499  SOPHUS_FUNC explicit SE3(Matrix4<Scalar> const& T)
500  : so3_(T.template topLeftCorner<3, 3>()),
501  translation_(T.template block<3, 1>(0, 3)) {
502  SOPHUS_ENSURE((T.row(3) - Matrix<Scalar, 1, 4>(Scalar(0), Scalar(0),
503  Scalar(0), Scalar(1)))
505  "Last row is not (0,0,0,1), but (%).", T.row(3));
506  }
507 
514  // so3_ and translation_ are laid out sequentially with no padding
515  return so3_.data();
516  }
517 
520  SOPHUS_FUNC Scalar const* data() const {
521  // so3_ and translation_ are laid out sequentially with no padding
522  return so3_.data();
523  }
524 
527  SOPHUS_FUNC SO3Member& so3() { return so3_; }
528 
531  SOPHUS_FUNC SO3Member const& so3() const { return so3_; }
532 
536 
540  return translation_;
541  }
542 
546  Tangent const& upsilon_omega) {
547  using std::cos;
548  using std::pow;
549  using std::sin;
550  using std::sqrt;
552  Sophus::Vector<Scalar, 3> upsilon = upsilon_omega.template head<3>();
553  Sophus::Vector<Scalar, 3> omega = upsilon_omega.template tail<3>();
554 
555  Scalar const c0 = omega[0] * omega[0];
556  Scalar const c1 = omega[1] * omega[1];
557  Scalar const c2 = omega[2] * omega[2];
558  Scalar const c3 = c0 + c1 + c2;
559  Scalar const o(0);
560  Scalar const h(0.5);
561  Scalar const i(1);
562 
563  if (c3 < Constants<Scalar>::epsilon()) {
564  Scalar const ux = Scalar(0.5) * upsilon[0];
565  Scalar const uy = Scalar(0.5) * upsilon[1];
566  Scalar const uz = Scalar(0.5) * upsilon[2];
567 
569  J << o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o, o, h, o, o, o, o, o,
570  o, i, o, o, o, uz, -uy, o, i, o, -uz, o, ux, o, o, i, uy, -ux, o;
572  return J;
573  }
574 
575  Scalar const c4 = sqrt(c3);
576  Scalar const c5 = Scalar(1.0) / c4;
577  Scalar const c6 = Scalar(0.5) * c4;
578  Scalar const c7 = sin(c6);
579  Scalar const c8 = c5 * c7;
580  Scalar const c9 = pow(c3, -3.0L / 2.0L);
581  Scalar const c10 = c7 * c9;
582  Scalar const c11 = Scalar(1.0) / c3;
583  Scalar const c12 = cos(c6);
584  Scalar const c13 = Scalar(0.5) * c11 * c12;
585  Scalar const c14 = c7 * c9 * omega[0];
586  Scalar const c15 = Scalar(0.5) * c11 * c12 * omega[0];
587  Scalar const c16 = -c14 * omega[1] + c15 * omega[1];
588  Scalar const c17 = -c14 * omega[2] + c15 * omega[2];
589  Scalar const c18 = omega[1] * omega[2];
590  Scalar const c19 = -c10 * c18 + c13 * c18;
591  Scalar const c20 = c5 * omega[0];
592  Scalar const c21 = Scalar(0.5) * c7;
593  Scalar const c22 = c5 * omega[1];
594  Scalar const c23 = c5 * omega[2];
595  Scalar const c24 = -c1;
596  Scalar const c25 = -c2;
597  Scalar const c26 = c24 + c25;
598  Scalar const c27 = sin(c4);
599  Scalar const c28 = -c27 + c4;
600  Scalar const c29 = c28 * c9;
601  Scalar const c30 = cos(c4);
602  Scalar const c31 = -c30 + Scalar(1);
603  Scalar const c32 = c11 * c31;
604  Scalar const c33 = c32 * omega[2];
605  Scalar const c34 = c29 * omega[0];
606  Scalar const c35 = c34 * omega[1];
607  Scalar const c36 = c32 * omega[1];
608  Scalar const c37 = c34 * omega[2];
609  Scalar const c38 = pow(c3, -5.0L / 2.0L);
610  Scalar const c39 = Scalar(3) * c28 * c38 * omega[0];
611  Scalar const c40 = c26 * c9;
612  Scalar const c41 = -c20 * c30 + c20;
613  Scalar const c42 = c27 * c9 * omega[0];
614  Scalar const c43 = c42 * omega[1];
615  Scalar const c44 = pow(c3, -2);
616  Scalar const c45 = Scalar(2) * c31 * c44 * omega[0];
617  Scalar const c46 = c45 * omega[1];
618  Scalar const c47 = c29 * omega[2];
619  Scalar const c48 = c43 - c46 + c47;
620  Scalar const c49 = Scalar(3) * c0 * c28 * c38;
621  Scalar const c50 = c9 * omega[0] * omega[2];
622  Scalar const c51 = c41 * c50 - c49 * omega[2];
623  Scalar const c52 = c9 * omega[0] * omega[1];
624  Scalar const c53 = c41 * c52 - c49 * omega[1];
625  Scalar const c54 = c42 * omega[2];
626  Scalar const c55 = c45 * omega[2];
627  Scalar const c56 = c29 * omega[1];
628  Scalar const c57 = -c54 + c55 + c56;
629  Scalar const c58 = Scalar(-2) * c56;
630  Scalar const c59 = Scalar(3) * c28 * c38 * omega[1];
631  Scalar const c60 = -c22 * c30 + c22;
632  Scalar const c61 = -c18 * c39;
633  Scalar const c62 = c32 + c61;
634  Scalar const c63 = c27 * c9;
635  Scalar const c64 = c1 * c63;
636  Scalar const c65 = Scalar(2) * c31 * c44;
637  Scalar const c66 = c1 * c65;
638  Scalar const c67 = c50 * c60;
639  Scalar const c68 = -c1 * c39 + c52 * c60;
640  Scalar const c69 = c18 * c63;
641  Scalar const c70 = c18 * c65;
642  Scalar const c71 = c34 - c69 + c70;
643  Scalar const c72 = Scalar(-2) * c47;
644  Scalar const c73 = Scalar(3) * c28 * c38 * omega[2];
645  Scalar const c74 = -c23 * c30 + c23;
646  Scalar const c75 = -c32 + c61;
647  Scalar const c76 = c2 * c63;
648  Scalar const c77 = c2 * c65;
649  Scalar const c78 = c52 * c74;
650  Scalar const c79 = c34 + c69 - c70;
651  Scalar const c80 = -c2 * c39 + c50 * c74;
652  Scalar const c81 = -c0;
653  Scalar const c82 = c25 + c81;
654  Scalar const c83 = c32 * omega[0];
655  Scalar const c84 = c18 * c29;
656  Scalar const c85 = Scalar(-2) * c34;
657  Scalar const c86 = c82 * c9;
658  Scalar const c87 = c0 * c63;
659  Scalar const c88 = c0 * c65;
660  Scalar const c89 = c9 * omega[1] * omega[2];
661  Scalar const c90 = c41 * c89;
662  Scalar const c91 = c54 - c55 + c56;
663  Scalar const c92 = -c1 * c73 + c60 * c89;
664  Scalar const c93 = -c43 + c46 + c47;
665  Scalar const c94 = -c2 * c59 + c74 * c89;
666  Scalar const c95 = c24 + c81;
667  Scalar const c96 = c9 * c95;
668  J(0, 0) = o;
669  J(0, 1) = o;
670  J(0, 2) = o;
671  J(0, 3) = -c0 * c10 + c0 * c13 + c8;
672  J(0, 4) = c16;
673  J(0, 5) = c17;
674  J(1, 0) = o;
675  J(1, 1) = o;
676  J(1, 2) = o;
677  J(1, 3) = c16;
678  J(1, 4) = -c1 * c10 + c1 * c13 + c8;
679  J(1, 5) = c19;
680  J(2, 0) = o;
681  J(2, 1) = o;
682  J(2, 2) = o;
683  J(2, 3) = c17;
684  J(2, 4) = c19;
685  J(2, 5) = -c10 * c2 + c13 * c2 + c8;
686  J(3, 0) = o;
687  J(3, 1) = o;
688  J(3, 2) = o;
689  J(3, 3) = -c20 * c21;
690  J(3, 4) = -c21 * c22;
691  J(3, 5) = -c21 * c23;
692  J(4, 0) = c26 * c29 + Scalar(1);
693  J(4, 1) = -c33 + c35;
694  J(4, 2) = c36 + c37;
695  J(4, 3) = upsilon[0] * (-c26 * c39 + c40 * c41) + upsilon[1] * (c53 + c57) +
696  upsilon[2] * (c48 + c51);
697  J(4, 4) = upsilon[0] * (-c26 * c59 + c40 * c60 + c58) +
698  upsilon[1] * (c68 + c71) + upsilon[2] * (c62 + c64 - c66 + c67);
699  J(4, 5) = upsilon[0] * (-c26 * c73 + c40 * c74 + c72) +
700  upsilon[1] * (c75 - c76 + c77 + c78) + upsilon[2] * (c79 + c80);
701  J(5, 0) = c33 + c35;
702  J(5, 1) = c29 * c82 + Scalar(1);
703  J(5, 2) = -c83 + c84;
704  J(5, 3) = upsilon[0] * (c53 + c91) +
705  upsilon[1] * (-c39 * c82 + c41 * c86 + c85) +
706  upsilon[2] * (c75 - c87 + c88 + c90);
707  J(5, 4) = upsilon[0] * (c68 + c79) + upsilon[1] * (-c59 * c82 + c60 * c86) +
708  upsilon[2] * (c92 + c93);
709  J(5, 5) = upsilon[0] * (c62 + c76 - c77 + c78) +
710  upsilon[1] * (c72 - c73 * c82 + c74 * c86) +
711  upsilon[2] * (c57 + c94);
712  J(6, 0) = -c36 + c37;
713  J(6, 1) = c83 + c84;
714  J(6, 2) = c29 * c95 + Scalar(1);
715  J(6, 3) = upsilon[0] * (c51 + c93) + upsilon[1] * (c62 + c87 - c88 + c90) +
716  upsilon[2] * (-c39 * c95 + c41 * c96 + c85);
717  J(6, 4) = upsilon[0] * (-c64 + c66 + c67 + c75) + upsilon[1] * (c48 + c92) +
718  upsilon[2] * (c58 - c59 * c95 + c60 * c96);
719  J(6, 5) = upsilon[0] * (c71 + c80) + upsilon[1] * (c91 + c94) +
720  upsilon[2] * (-c73 * c95 + c74 * c96);
721 
722  return J;
723  }
724 
730  Scalar const o(0);
731  Scalar const h(0.5);
732  Scalar const i(1);
733 
734  // clang-format off
735  J << o, o, o, h, o, o, o,
736  o, o, o, h, o, o, o,
737  o, o, o, h, o, o, o,
738  o, o, o, i, o, o, o,
739  o, o, o, i, o, o, o,
740  o, o, o, i, o, o, o;
741  // clang-format on
742  return J;
743  }
744 
748  return generator(i);
749  }
750 
763  SOPHUS_FUNC static SE3<Scalar> exp(Tangent const& a) {
764  using std::cos;
765  using std::sin;
766  Vector3<Scalar> const omega = a.template tail<3>();
767 
768  Scalar theta;
769  SO3<Scalar> const so3 = SO3<Scalar>::expAndTheta(omega, &theta);
770  Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
771  Matrix3<Scalar> const Omega_sq = Omega * Omega;
772  Matrix3<Scalar> V;
773 
774  if (theta < Constants<Scalar>::epsilon()) {
775  V = so3.matrix();
777  } else {
778  Scalar theta_sq = theta * theta;
780  (Scalar(1) - cos(theta)) / (theta_sq)*Omega +
781  (theta - sin(theta)) / (theta_sq * theta) * Omega_sq);
782  }
783  return SE3<Scalar>(so3, V * a.template head<3>());
784  }
785 
788  template <class S = Scalar>
791  return SE3(SO3<Scalar>::fitToSO3(T.template block<3, 3>(0, 0)),
792  T.template block<3, 1>(0, 3));
793  }
794 
834  SOPHUS_ENSURE(i >= 0 && i <= 5, "i should be in range [0,5].");
835  Tangent e;
836  e.setZero();
837  e[i] = Scalar(1);
838  return hat(e);
839  }
840 
855  Transformation Omega;
856  Omega.setZero();
857  Omega.template topLeftCorner<3, 3>() =
858  SO3<Scalar>::hat(a.template tail<3>());
859  Omega.col(3).template head<3>() = a.template head<3>();
860  return Omega;
861  }
862 
872  SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
873  Vector3<Scalar> const upsilon1 = a.template head<3>();
874  Vector3<Scalar> const upsilon2 = b.template head<3>();
875  Vector3<Scalar> const omega1 = a.template tail<3>();
876  Vector3<Scalar> const omega2 = b.template tail<3>();
877 
878  Tangent res;
879  res.template head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2);
880  res.template tail<3>() = omega1.cross(omega2);
881 
882  return res;
883  }
884 
887  static SOPHUS_FUNC SE3 rotX(Scalar const& x) {
889  }
890 
893  static SOPHUS_FUNC SE3 rotY(Scalar const& y) {
895  }
896 
899  static SOPHUS_FUNC SE3 rotZ(Scalar const& z) {
901  }
902 
907  template <class UniformRandomBitGenerator>
908  static SE3 sampleUniform(UniformRandomBitGenerator& generator) {
909  std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
911  Vector3<Scalar>(uniform(generator), uniform(generator),
912  uniform(generator)));
913  }
914 
917  template <class T0, class T1, class T2>
918  static SOPHUS_FUNC SE3 trans(T0 const& x, T1 const& y, T2 const& z) {
919  return SE3(SO3<Scalar>(), Vector3<Scalar>(x, y, z));
920  }
921 
922  static SOPHUS_FUNC SE3 trans(Vector3<Scalar> const& xyz) {
923  return SE3(SO3<Scalar>(), xyz);
924  }
925 
928  static SOPHUS_FUNC SE3 transX(Scalar const& x) {
929  return SE3::trans(x, Scalar(0), Scalar(0));
930  }
931 
934  static SOPHUS_FUNC SE3 transY(Scalar const& y) {
935  return SE3::trans(Scalar(0), y, Scalar(0));
936  }
937 
940  static SOPHUS_FUNC SE3 transZ(Scalar const& z) {
941  return SE3::trans(Scalar(0), Scalar(0), z);
942  }
943 
958  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
959  Tangent upsilon_omega;
960  upsilon_omega.template head<3>() = Omega.col(3).template head<3>();
961  upsilon_omega.template tail<3>() =
962  SO3<Scalar>::vee(Omega.template topLeftCorner<3, 3>());
963  return upsilon_omega;
964  }
965 
966  protected:
969 };
970 
971 template <class Scalar, int Options>
972 SE3<Scalar, Options>::SE3() : translation_(TranslationMember::Zero()) {
973  static_assert(std::is_standard_layout<SE3>::value,
974  "Assume standard layout for the use of offsetof check below.");
975  static_assert(
976  offsetof(SE3, so3_) + sizeof(Scalar) * SO3<Scalar>::num_parameters ==
977  offsetof(SE3, translation_),
978  "This class assumes packed storage and hence will only work "
979  "correctly depending on the compiler (options) - in "
980  "particular when using [this->data(), this-data() + "
981  "num_parameters] to access the raw data in a contiguous fashion.");
982 }
983 } // namespace Sophus
984 
985 namespace Eigen {
986 
990 template <class Scalar_, int Options>
991 class Map<Sophus::SE3<Scalar_>, Options>
992  : public Sophus::SE3Base<Map<Sophus::SE3<Scalar_>, Options>> {
993  public:
995  using Scalar = Scalar_;
997  using Point = typename Base::Point;
999  using Tangent = typename Base::Tangent;
1000  using Adjoint = typename Base::Adjoint;
1001 
1002  // LCOV_EXCL_START
1003  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map);
1004  // LCOV_EXCL_STOP
1005 
1006  using Base::operator*=;
1007  using Base::operator*;
1008 
1010  : so3_(coeffs),
1011  translation_(coeffs + Sophus::SO3<Scalar>::num_parameters) {}
1012 
1015  SOPHUS_FUNC Map<Sophus::SO3<Scalar>, Options>& so3() { return so3_; }
1016 
1019  SOPHUS_FUNC Map<Sophus::SO3<Scalar>, Options> const& so3() const {
1020  return so3_;
1021  }
1022 
1025  SOPHUS_FUNC Map<Sophus::Vector3<Scalar, Options>>& translation() {
1026  return translation_;
1027  }
1028 
1031  SOPHUS_FUNC Map<Sophus::Vector3<Scalar, Options>> const& translation() const {
1032  return translation_;
1033  }
1034 
1035  protected:
1036  Map<Sophus::SO3<Scalar>, Options> so3_;
1037  Map<Sophus::Vector3<Scalar>, Options> translation_;
1038 };
1039 
1043 template <class Scalar_, int Options>
1044 class Map<Sophus::SE3<Scalar_> const, Options>
1045  : public Sophus::SE3Base<Map<Sophus::SE3<Scalar_> const, Options>> {
1046  public:
1048  using Scalar = Scalar_;
1050  using Point = typename Base::Point;
1052  using Tangent = typename Base::Tangent;
1053  using Adjoint = typename Base::Adjoint;
1054 
1055  using Base::operator*=;
1056  using Base::operator*;
1057 
1058  SOPHUS_FUNC Map(Scalar const* coeffs)
1059  : so3_(coeffs),
1060  translation_(coeffs + Sophus::SO3<Scalar>::num_parameters) {}
1061 
1064  SOPHUS_FUNC Map<Sophus::SO3<Scalar> const, Options> const& so3() const {
1065  return so3_;
1066  }
1067 
1070  SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const& translation()
1071  const {
1072  return translation_;
1073  }
1074 
1075  protected:
1076  Map<Sophus::SO3<Scalar> const, Options> const so3_;
1077  Map<Sophus::Vector3<Scalar> const, Options> const translation_;
1078 };
1079 } // namespace Eigen
1080 
1081 #endif
c73
const Scalar c73
Definition: Se3_Dx_exp_x.cpp:74
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::so3_
const Map< Sophus::SO3< Scalar > const, Options > so3_
Definition: se3.hpp:1076
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::Scalar
Scalar_ Scalar
Definition: se3.hpp:1048
c49
const Scalar c49
Definition: Se3_Dx_exp_x.cpp:50
Sophus::SE3Base::setRotationMatrix
SOPHUS_FUNC void setRotationMatrix(Matrix3< Scalar > const &R)
Definition: se3.hpp:391
Sophus::SE3::rotY
static SOPHUS_FUNC SE3 rotY(Scalar const &y)
Definition: se3.hpp:893
c1
const Scalar c1
Definition: Se2_Dx_exp_x.cpp:2
Sophus::SE3::so3
SOPHUS_FUNC SO3Member & so3()
Definition: se3.hpp:527
c59
const Scalar c59
Definition: Se3_Dx_exp_x.cpp:60
Sophus::SE3::so3_
SO3Member so3_
Definition: se3.hpp:967
c46
const Scalar c46
Definition: Se3_Dx_exp_x.cpp:47
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::so3
SOPHUS_FUNC Map< Sophus::SO3< Scalar >, Options > & so3()
Definition: se3.hpp:1015
Eigen
Definition: rxso2.hpp:16
c71
const Scalar c71
Definition: Se3_Dx_exp_x.cpp:72
c90
const Scalar c90
Definition: Se3_Dx_exp_x.cpp:91
c58
const Scalar c58
Definition: Se3_Dx_exp_x.cpp:59
Sophus::SE3Base::operator*=
SOPHUS_FUNC SE3Base< Derived > & operator*=(SE3Base< OtherDerived > const &other)
Definition: se3.hpp:360
c56
const Scalar c56
Definition: Se3_Dx_exp_x.cpp:57
c81
const Scalar c81
Definition: Se3_Dx_exp_x.cpp:82
Sophus::SE3Base::angleY
Scalar angleY() const
Definition: se3.hpp:119
c50
const Scalar c50
Definition: Se3_Dx_exp_x.cpp:51
Sophus::SE3Base::operator=
SOPHUS_FUNC SE3Base & operator=(SE3Base const &other)=default
Eigen::internal::traits< Sophus::SE3< Scalar_, Options > >::TranslationType
Sophus::Vector3< Scalar, Options > TranslationType
Definition: se3.hpp:22
Sophus::SE3::Transformation
typename Base::Transformation Transformation
Definition: se3.hpp:438
Eigen::internal::traits< Sophus::SE3< Scalar_, Options > >::Scalar
Scalar_ Scalar
Definition: se3.hpp:21
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::Map
SOPHUS_FUNC Map(Scalar const *coeffs)
Definition: se3.hpp:1058
Sophus::SE3::Scalar
Scalar_ Scalar
Definition: se3.hpp:437
Sophus::SE3::Adjoint
typename Base::Adjoint Adjoint
Definition: se3.hpp:442
Sophus::SO3< Scalar, Options >
Sophus::SE3Base::N
static constexpr int N
Group transformations are 4x4 matrices.
Definition: se3.hpp:72
Sophus::SE3Base::inverse
SOPHUS_FUNC SE3< Scalar > inverse() const
Definition: se3.hpp:208
Sophus::SE3::data
SOPHUS_FUNC Scalar * data()
Definition: se3.hpp:513
c87
const Scalar c87
Definition: Se3_Dx_exp_x.cpp:88
Sophus::SO3::vee
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Definition: so3.hpp:749
c84
const Scalar c84
Definition: Se3_Dx_exp_x.cpp:85
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::Map
SOPHUS_FUNC Map(Scalar *coeffs)
Definition: se3.hpp:1009
c47
const Scalar c47
Definition: Se3_Dx_exp_x.cpp:48
c19
const Scalar c19
Definition: Se3_Dx_exp_x.cpp:20
Eigen::internal::traits< Map< Sophus::SE3< Scalar_ > const, Options > >::SO3Type
Map< Sophus::SO3< Scalar > const, Options > SO3Type
Definition: se3.hpp:39
c79
const Scalar c79
Definition: Se3_Dx_exp_x.cpp:80
Sophus::SE3::DoF
static constexpr int DoF
Definition: se3.hpp:434
c17
const Scalar c17
Definition: Se3_Dx_exp_x.cpp:18
Sophus::Vector3
Vector< Scalar, 3, Options > Vector3
Definition: types.hpp:21
c25
const Scalar c25
Definition: Se3_Dx_exp_x.cpp:26
c88
const Scalar c88
Definition: Se3_Dx_exp_x.cpp:89
c39
const Scalar c39
Definition: Se3_Dx_exp_x.cpp:40
Sophus::SE3::SE3
SOPHUS_FUNC SE3(SO3Base< OtherDerived > const &so3, Eigen::MatrixBase< D > const &translation)
Definition: se3.hpp:468
c55
const Scalar c55
Definition: Se3_Dx_exp_x.cpp:56
Sophus::SE3::SE3
SOPHUS_FUNC SE3(Eigen::Quaternion< Scalar > const &quaternion, Point const &translation)
Definition: se3.hpp:490
c83
const Scalar c83
Definition: Se3_Dx_exp_x.cpp:84
c66
const Scalar c66
Definition: Se3_Dx_exp_x.cpp:67
c40
const Scalar c40
Definition: Se3_Dx_exp_x.cpp:41
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::Tangent
typename Base::Tangent Tangent
Definition: se3.hpp:999
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_ENSURE
#define SOPHUS_ENSURE(expr,...)
Definition: common.hpp:137
Sophus::SE3::Tangent
typename Base::Tangent Tangent
Definition: se3.hpp:441
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::Transformation
Matrix< Scalar, N, N > Transformation
Definition: se3.hpp:73
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::PointProduct
Vector3< ReturnScalar< PointDerived > > PointProduct
Definition: se3.hpp:92
Sophus::SE3::sampleUniform
static SE3 sampleUniform(UniformRandomBitGenerator &generator)
Definition: se3.hpp:908
Eigen::internal::traits< Map< Sophus::SE3< Scalar_ > const, Options > >::TranslationType
Map< Sophus::Vector3< Scalar > const, Options > TranslationType
Definition: se3.hpp:38
Sophus::SE3::exp
static SOPHUS_FUNC SE3< Scalar > exp(Tangent const &a)
Definition: se3.hpp:763
c54
const Scalar c54
Definition: Se3_Dx_exp_x.cpp:55
c18
const Scalar c18
Definition: Se3_Dx_exp_x.cpp:19
Sophus::SE3::SE3
SOPHUS_FUNC SE3(Matrix4< Scalar > const &T)
Definition: se3.hpp:499
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::Adjoint
typename Base::Adjoint Adjoint
Definition: se3.hpp:1000
c7
const Scalar c7
Definition: Se2_Dx_exp_x.cpp:8
Sophus::SE3::lieBracket
static SOPHUS_FUNC Tangent lieBracket(Tangent const &a, Tangent const &b)
Definition: se3.hpp:872
c14
const Scalar c14
Definition: Se3_Dx_exp_x.cpp:15
Sophus::SE3Base::operator*
SOPHUS_FUNC Line operator*(Line const &l) const
Definition: se3.hpp:350
c61
const Scalar c61
Definition: Se3_Dx_exp_x.cpp:62
Sophus::SE3Base::setQuaternion
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion< Scalar > const &quat)
Definition: se3.hpp:383
Sophus::ParametrizedLine3
ParametrizedLine< Scalar, 3, Options > ParametrizedLine3
Definition: types.hpp:72
c70
const Scalar c70
Definition: Se3_Dx_exp_x.cpp:71
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::Tangent
typename Base::Tangent Tangent
Definition: se3.hpp:1052
c69
const Scalar c69
Definition: Se3_Dx_exp_x.cpp:70
Sophus::SE3::Dx_exp_x
static SOPHUS_FUNC Sophus::Matrix< Scalar, num_parameters, DoF > Dx_exp_x(Tangent const &upsilon_omega)
Definition: se3.hpp:545
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::so3
SOPHUS_FUNC Map< Sophus::SO3< Scalar > const, Options > const & so3() const
Definition: se3.hpp:1064
c31
const Scalar c31
Definition: Se3_Dx_exp_x.cpp:32
Sophus::SE3Base::so3
SOPHUS_FUNC SO3Type const & so3() const
Definition: se3.hpp:375
so3.hpp
Sophus::SE3Base::unit_quaternion
SOPHUS_FUNC QuaternionType const & unit_quaternion() const
Definition: se3.hpp:423
Sophus::SE3Base::operator=
SOPHUS_FUNC SE3Base< Derived > & operator=(SE3Base< OtherDerived > const &other)
Definition: se3.hpp:299
Sophus::SE3::Dxi_exp_x_matrix_at_0
static SOPHUS_FUNC Transformation Dxi_exp_x_matrix_at_0(int i)
Definition: se3.hpp:747
c23
const Scalar c23
Definition: Se3_Dx_exp_x.cpp:24
Sophus::SE3::rotZ
static SOPHUS_FUNC SE3 rotZ(Scalar const &z)
Definition: se3.hpp:899
Sophus::SE3::HomogeneousPoint
typename Base::HomogeneousPoint HomogeneousPoint
Definition: se3.hpp:440
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::HomogeneousPoint
Vector4< Scalar > HomogeneousPoint
Definition: se3.hpp:75
c28
const Scalar c28
Definition: Se3_Dx_exp_x.cpp:29
c52
const Scalar c52
Definition: Se3_Dx_exp_x.cpp:53
c95
const Scalar c95
Definition: Se3_Dx_exp_x.cpp:96
c16
const Scalar c16
Definition: Se3_Dx_exp_x.cpp:17
c26
const Scalar c26
Definition: Se3_Dx_exp_x.cpp:27
Sophus::SE3Base::operator*
SOPHUS_FUNC HomogeneousPointProduct< HPointDerived > operator*(Eigen::MatrixBase< HPointDerived > const &p) const
Definition: se3.hpp:335
Sophus::SE3Base::normalize
SOPHUS_FUNC void normalize()
Definition: se3.hpp:263
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::TranslationType
typename Eigen::internal::traits< Map< Sophus::SE3< Scalar_ >, Options > >::TranslationType TranslationType
Definition: se3.hpp:62
Sophus::SE3Base::Adj
SOPHUS_FUNC Adjoint Adj() const
Definition: se3.hpp:103
Sophus::Matrix
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:41
Sophus
Definition: average.hpp:17
Sophus::SO3::hat
static SOPHUS_FUNC Transformation hat(Tangent const &omega)
Definition: so3.hpp:668
Sophus::SE3::SE3
SOPHUS_FUNC SE3(Matrix3< Scalar > const &rotation_matrix, Point const &translation)
Definition: se3.hpp:483
SOPHUS_FUNC
#define SOPHUS_FUNC
Definition: common.hpp:37
c2
const Scalar c2
Definition: Se2_Dx_exp_x.cpp:3
c64
const Scalar c64
Definition: Se3_Dx_exp_x.cpp:65
Sophus::SE3Base::log
SOPHUS_FUNC Tangent log() const
Definition: se3.hpp:223
c51
const Scalar c51
Definition: Se3_Dx_exp_x.cpp:52
Sophus::SE3::translation_
TranslationMember translation_
Definition: se3.hpp:968
Sophus::isOrthogonal
SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase< D > const &R)
Definition: rotation_matrix.hpp:17
c76
const Scalar c76
Definition: Se3_Dx_exp_x.cpp:77
Sophus::SE3Base::operator*
SOPHUS_FUNC SE3Product< OtherDerived > operator*(SE3Base< OtherDerived > const &other) const
Definition: se3.hpp:308
c67
const Scalar c67
Definition: Se3_Dx_exp_x.cpp:68
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::Point
typename Base::Point Point
Definition: se3.hpp:997
c80
const Scalar c80
Definition: Se3_Dx_exp_x.cpp:81
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::HomogeneousPointProduct
Vector4< ReturnScalar< HPointDerived > > HomogeneousPointProduct
Definition: se3.hpp:95
Sophus::SE3Base::params
SOPHUS_FUNC Sophus::Vector< Scalar, num_parameters > params() const
Definition: se3.hpp:403
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::Adjoint
typename Base::Adjoint Adjoint
Definition: se3.hpp:1053
Sophus::SE3::data
SOPHUS_FUNC Scalar const * data() const
Definition: se3.hpp:520
Sophus::SE3::Dx_exp_x_at_0
static SOPHUS_FUNC Sophus::Matrix< Scalar, num_parameters, DoF > Dx_exp_x_at_0()
Definition: se3.hpp:728
c77
const Scalar c77
Definition: Se3_Dx_exp_x.cpp:78
c43
const Scalar c43
Definition: Se3_Dx_exp_x.cpp:44
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::Transformation
typename Base::Transformation Transformation
Definition: se3.hpp:1049
Sophus::SE3::translation
SOPHUS_FUNC TranslationMember & translation()
Definition: se3.hpp:535
c9
const Scalar c9
Definition: Se2_Dx_exp_x.cpp:10
c35
const Scalar c35
Definition: Se3_Dx_exp_x.cpp:36
c41
const Scalar c41
Definition: Se3_Dx_exp_x.cpp:42
Sophus::SE3
SE3 using default storage; derived from SE3Base.
Definition: se3.hpp:11
c21
const Scalar c21
Definition: Se3_Dx_exp_x.cpp:22
Sophus::SE3Base
Definition: se3.hpp:58
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::Transformation
typename Base::Transformation Transformation
Definition: se3.hpp:996
c32
const Scalar c32
Definition: Se3_Dx_exp_x.cpp:33
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::translation
SOPHUS_FUNC Map< Sophus::Vector3< Scalar > const, Options > const & translation() const
Definition: se3.hpp:1070
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::translation_
Map< Sophus::Vector3< Scalar >, Options > translation_
Definition: se3.hpp:1037
c37
const Scalar c37
Definition: Se3_Dx_exp_x.cpp:38
c60
const Scalar c60
Definition: Se3_Dx_exp_x.cpp:61
c57
const Scalar c57
Definition: Se3_Dx_exp_x.cpp:58
Sophus::SE3Base::DoF
static constexpr int DoF
Definition: se3.hpp:67
c3
const Scalar c3
Definition: Se2_Dx_exp_x.cpp:4
c62
const Scalar c62
Definition: Se3_Dx_exp_x.cpp:63
Sophus::SE3Base::angleX
Scalar angleX() const
Definition: se3.hpp:115
Sophus::SE3::so3
SOPHUS_FUNC SO3Member const & so3() const
Definition: se3.hpp:531
Sophus::SE3::trans
static SOPHUS_FUNC SE3 trans(T0 const &x, T1 const &y, T2 const &z)
Definition: se3.hpp:918
c36
const Scalar c36
Definition: Se3_Dx_exp_x.cpp:37
Sophus::SE3::fitToSE3
static SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, SE3 > fitToSE3(Matrix4< Scalar > const &T)
Definition: se3.hpp:790
c45
const Scalar c45
Definition: Se3_Dx_exp_x.cpp:46
Sophus::SE3Base::matrix
SOPHUS_FUNC Transformation matrix() const
Definition: se3.hpp:275
Sophus::SE3::transZ
static SOPHUS_FUNC SE3 transZ(Scalar const &z)
Definition: se3.hpp:940
Sophus::SE3::transX
static SOPHUS_FUNC SE3 transX(Scalar const &x)
Definition: se3.hpp:928
Sophus::IsFixedSizeVector
Definition: types.hpp:225
c93
const Scalar c93
Definition: Se3_Dx_exp_x.cpp:94
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::Line
ParametrizedLine3< Scalar > Line
Definition: se3.hpp:76
Sophus::SE3Base::translation
SOPHUS_FUNC TranslationType const & translation() const
Definition: se3.hpp:417
c34
const Scalar c34
Definition: Se3_Dx_exp_x.cpp:35
Sophus::Constants
Definition: common.hpp:146
c53
const Scalar c53
Definition: Se3_Dx_exp_x.cpp:54
c78
const Scalar c78
Definition: Se3_Dx_exp_x.cpp:79
Sophus::SE3Base::num_parameters
static constexpr int num_parameters
Definition: se3.hpp:70
Sophus::SE3::SE3
SOPHUS_FUNC SE3(SE3Base< OtherDerived > const &other)
Definition: se3.hpp:459
Sophus::squaredNorm
auto squaredNorm(T const &p) -> decltype(details::SquaredNorm< T >::impl(p))
Definition: types.hpp:187
c24
const Scalar c24
Definition: Se3_Dx_exp_x.cpp:25
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::SE3< Scalar_ > const, Options >::HomogeneousPoint
typename Base::HomogeneousPoint HomogeneousPoint
Definition: se3.hpp:1051
Sophus::SE3::Point
typename Base::Point Point
Definition: se3.hpp:439
c44
const Scalar c44
Definition: Se3_Dx_exp_x.cpp:45
Sophus::SE3::trans
static SOPHUS_FUNC SE3 trans(Vector3< Scalar > const &xyz)
Definition: se3.hpp:922
Sophus::Vector4
Vector< Scalar, 4 > Vector4
Definition: types.hpp:26
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::Scalar
Scalar_ Scalar
Definition: se3.hpp:995
c91
const Scalar c91
Definition: Se3_Dx_exp_x.cpp:92
c6
const Scalar c6
Definition: Se2_Dx_exp_x.cpp:7
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::QuaternionType
typename SO3Type::QuaternionType QuaternionType
Definition: se3.hpp:64
c65
const Scalar c65
Definition: Se3_Dx_exp_x.cpp:66
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::HomogeneousPoint
typename Base::HomogeneousPoint HomogeneousPoint
Definition: se3.hpp:998
c85
const Scalar c85
Definition: Se3_Dx_exp_x.cpp:86
Sophus::Vector
Eigen::Matrix< Scalar, M, 1, Options > Vector
Definition: types.hpp:13
c92
const Scalar c92
Definition: Se3_Dx_exp_x.cpp:93
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::so3
SOPHUS_FUNC Map< Sophus::SO3< Scalar >, Options > const & so3() const
Definition: se3.hpp:1019
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::so3_
Map< Sophus::SO3< Scalar >, Options > so3_
Definition: se3.hpp:1036
c94
const Scalar c94
Definition: Se3_Dx_exp_x.cpp:95
c22
const Scalar c22
Definition: Se3_Dx_exp_x.cpp:23
c82
const Scalar c82
Definition: Se3_Dx_exp_x.cpp:83
Sophus::SE3Base::translation
SOPHUS_FUNC TranslationType & translation()
Definition: se3.hpp:411
Sophus::SE3::hat
static SOPHUS_FUNC Transformation hat(Tangent const &a)
Definition: se3.hpp:854
Sophus::SE3Base::angleZ
Scalar angleZ() const
Definition: se3.hpp:123
c86
const Scalar c86
Definition: Se3_Dx_exp_x.cpp:87
c68
const Scalar c68
Definition: Se3_Dx_exp_x.cpp:69
c42
const Scalar c42
Definition: Se3_Dx_exp_x.cpp:43
c63
const Scalar c63
Definition: Se3_Dx_exp_x.cpp:64
Sophus::SE3::vee
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Definition: se3.hpp:958
Sophus::SO3::expAndTheta
static SOPHUS_FUNC SO3< Scalar > expAndTheta(Tangent const &omega, Scalar *theta)
Definition: so3.hpp:580
Sophus::SE3::TranslationMember
Vector3< Scalar, Options > TranslationMember
Definition: se3.hpp:444
c11
const Scalar c11
Definition: Se3_Dx_exp_x.cpp:12
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::Adjoint
Matrix< Scalar, DoF, DoF > Adjoint
Definition: se3.hpp:78
c5
const Scalar c5
Definition: Se2_Dx_exp_x.cpp:6
Sophus::SE3::translation
SOPHUS_FUNC TranslationMember const & translation() const
Definition: se3.hpp:539
c29
const Scalar c29
Definition: Se3_Dx_exp_x.cpp:30
Sophus::SE3Base::matrix3x4
SOPHUS_FUNC Matrix< Scalar, 3, 4 > matrix3x4() const
Definition: se3.hpp:285
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::Point
Vector3< Scalar > Point
Definition: se3.hpp:74
c33
const Scalar c33
Definition: Se3_Dx_exp_x.cpp:34
Sophus::SE3::rotX
static SOPHUS_FUNC SE3 rotX(Scalar const &x)
Definition: se3.hpp:887
c48
const Scalar c48
Definition: Se3_Dx_exp_x.cpp:49
c89
const Scalar c89
Definition: Se3_Dx_exp_x.cpp:90
Sophus::SE3Base::rotationMatrix
SOPHUS_FUNC Matrix3< Scalar > rotationMatrix() const
Definition: se3.hpp:367
c4
const Scalar c4
Definition: Se2_Dx_exp_x.cpp:5
Sophus::SE3Base::operator*
SOPHUS_FUNC PointProduct< PointDerived > operator*(Eigen::MatrixBase< PointDerived > const &p) const
Definition: se3.hpp:325
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::translation
SOPHUS_FUNC Map< Sophus::Vector3< Scalar, Options > > & translation()
Definition: se3.hpp:1025
c13
const Scalar c13
Definition: Se3_Dx_exp_x.cpp:14
Sophus::SE3::num_parameters
static constexpr int num_parameters
Definition: se3.hpp:435
Sophus::SE3::transY
static SOPHUS_FUNC SE3 transY(Scalar const &y)
Definition: se3.hpp:934
Sophus::SE3Base::Dx_this_mul_exp_x_at_0
SOPHUS_FUNC Matrix< Scalar, num_parameters, DoF > Dx_this_mul_exp_x_at_0() const
Definition: se3.hpp:135
Sophus::SE3Base::cast
SOPHUS_FUNC SE3< NewScalarType > cast() const
Definition: se3.hpp:128
Sophus::Matrix4
Matrix< Scalar, 4, 4 > Matrix4
Definition: types.hpp:54
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::ReturnScalar
typename Eigen::ScalarBinaryOpTraits< Scalar, typename OtherDerived::Scalar >::ReturnType ReturnScalar
Definition: se3.hpp:86
Sophus::SE3::SE3
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SE3()
Definition: se3.hpp:972
c15
const Scalar c15
Definition: Se3_Dx_exp_x.cpp:16
Sophus::SE3::generator
static SOPHUS_FUNC Transformation generator(int i)
Definition: se3.hpp:833
c30
const Scalar c30
Definition: Se3_Dx_exp_x.cpp:31
Eigen::internal::traits< Map< Sophus::SE3< Scalar_ > const, Options > >::Scalar
Scalar_ Scalar
Definition: se3.hpp:37
c74
const Scalar c74
Definition: Se3_Dx_exp_x.cpp:75
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::translation_
const Map< Sophus::Vector3< Scalar > const, Options > translation_
Definition: se3.hpp:1077
c72
const Scalar c72
Definition: Se3_Dx_exp_x.cpp:73
Sophus::SE3Base::so3
SOPHUS_FUNC SO3Type & so3()
Definition: se3.hpp:371
c38
const Scalar c38
Definition: Se3_Dx_exp_x.cpp:39
Eigen::Map< Sophus::SE3< Scalar_ >, Options >::translation
SOPHUS_FUNC Map< Sophus::Vector3< Scalar, Options > > const & translation() const
Definition: se3.hpp:1031
c75
const Scalar c75
Definition: Se3_Dx_exp_x.cpp:76
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::SO3Type
typename Eigen::internal::traits< Map< Sophus::SE3< Scalar_ >, Options > >::SO3Type SO3Type
Definition: se3.hpp:63
c0
const Scalar c0
Definition: Se2_Dx_exp_x.cpp:1
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::Scalar
typename Eigen::internal::traits< Map< Sophus::SE3< Scalar_ >, Options > >::Scalar Scalar
Definition: se3.hpp:60
Eigen::Map< Sophus::SE3< Scalar_ > const, Options >::Point
typename Base::Point Point
Definition: se3.hpp:1050
c8
const Scalar c8
Definition: Se2_Dx_exp_x.cpp:9
c12
const Scalar c12
Definition: Se3_Dx_exp_x.cpp:13
c27
const Scalar c27
Definition: Se3_Dx_exp_x.cpp:28
Sophus::SE3Base< Map< Sophus::SE3< Scalar_ >, Options > >::Tangent
Vector< Scalar, DoF > Tangent
Definition: se3.hpp:77


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