8 #include <Eigen/src/Geometry/OrthoMethods.h> 9 #include <Eigen/src/Geometry/Quaternion.h> 10 #include <Eigen/src/Geometry/RotationBase.h> 13 template <
class Scalar_,
int Options = 0>
22 template <
class Scalar_,
int Options>
23 struct traits<
Sophus::SO3<Scalar_, Options>> {
28 template <
class Scalar_,
int Options>
29 struct traits<Map<
Sophus::SO3<Scalar_>, Options>>
30 : traits<Sophus::SO3<Scalar_, Options>> {
35 template <
class Scalar_,
int Options>
36 struct traits<Map<
Sophus::SO3<Scalar_> const, Options>>
37 : traits<Sophus::SO3<Scalar_, Options> const> {
68 template <
class Derived>
71 using Scalar =
typename Eigen::internal::traits<Derived>::Scalar;
73 typename Eigen::internal::traits<Derived>::QuaternionType;
76 static int constexpr DoF = 3;
78 static int constexpr num_parameters = 4;
80 static int constexpr N = 3;
98 template <
class NewScalarType>
112 return unit_quaternion_nonconst().coeffs().data();
118 return unit_quaternion().coeffs().data();
126 Eigen::Quaternion<Scalar> internal_gen_q;
127 internalGenerator(i, &internal_gen_q);
128 res.template head<4>() = (unit_quaternion() * internal_gen_q).coeffs();
136 for (
int i = 0; i < DoF; ++i) {
137 J.col(i) = internalMultiplyByGenerator(i);
160 Scalar length = unit_quaternion_nonconst().norm();
162 "Quaternion (%) should not be close to zero!",
163 unit_quaternion_nonconst().coeffs().
transpose());
164 unit_quaternion_nonconst().coeffs() /= length;
173 return unit_quaternion().toRotationMatrix();
178 template <
class OtherDerived>
207 return unit_quaternion()._transformVector(p);
215 Scalar squared_norm = unit_quaternion().squaredNorm();
224 if (squared_norm !=
Scalar(1.0)) {
225 unit_quaternion_nonconst().coeffs() *=
236 unit_quaternion_nonconst() = quaternion;
243 return static_cast<Derived const*
>(
this)->unit_quaternion();
272 return expAndTheta(omega, &theta);
285 Scalar theta_sq = omega.squaredNorm();
286 *theta = sqrt(theta_sq);
292 Scalar theta_po4 = theta_sq * theta_sq;
293 imag_factor =
Scalar(0.5) -
Scalar(1.0 / 48.0) * theta_sq +
294 Scalar(1.0 / 3840.0) * theta_po4;
298 Scalar sin_half_theta = sin(half_theta);
299 imag_factor = sin_half_theta / (*theta);
300 real_factor = cos(half_theta);
304 q.unit_quaternion_nonconst() =
306 imag_factor * omega.y(), imag_factor * omega.z());
309 "SO3::exp failed! omega: %, real: %, img: %",
310 omega.transpose(), real_factor, imag_factor);
333 SOPHUS_ENSURE(i >= 0 && i <= 2,
"i should be in range [0,2].");
345 int i, Eigen::Quaternion<Scalar>* internal_gen_q) {
346 SOPHUS_ENSURE(i >= 0 && i <= 2,
"i should be in range [0,2]");
348 "internal_gen_q must not be the null pointer");
350 internal_gen_q->coeffs().setZero();
351 internal_gen_q->coeffs()[i] =
Scalar(0.5);
372 Scalar(0), -omega(2), omega(1),
373 omega(2),
Scalar(0), -omega(0),
374 -omega(1), omega(0),
Scalar(0);
394 return omega1.cross(omega2);
409 return logAndTheta(other, &theta);
420 Scalar n = sqrt(squared_n);
423 Scalar two_atan_nbyw_by_n;
436 "Quaternion (%) should be normalized!",
440 Scalar(2) / w -
Scalar(2) * (squared_n) / (w * squared_w);
449 two_atan_nbyw_by_n =
Scalar(2) * atan(n / w) / n;
453 *theta = two_atan_nbyw_by_n * n;
473 return Tangent(Omega(2, 1), Omega(0, 2), Omega(1, 0));
481 return static_cast<Derived*
>(
this)->unit_quaternion_nonconst();
486 template <
class Scalar_,
int Options>
487 class SO3 :
public SO3Base<SO3<Scalar_, Options>> {
502 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
511 template <
class OtherDerived>
513 : unit_quaternion_(other.unit_quaternion()) {}
527 : unit_quaternion_(quat) {
529 std::is_same<
typename Eigen::QuaternionBase<D>::Scalar,
Scalar>::
value,
530 "Input must be of same scalar type");
555 return unit_quaternion_;
562 return unit_quaternion_;
576 template <
class Scalar_,
int Options>
592 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
593 using Base::operator*=;
594 using Base::operator*;
602 return unit_quaternion_;
608 SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>, Options>&
610 return unit_quaternion_;
620 template <
class Scalar_,
int Options>
621 class Map<
Sophus::SO3<Scalar_> const, Options>
632 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
633 using Base::operator*=;
634 using Base::operator*;
640 SOPHUS_FUNC Map<Eigen::Quaternion<Scalar>
const, Options>
const&
642 return unit_quaternion_;
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > const & unit_quaternion() const
typename Base::Tangent Tangent
#define SOPHUS_ENSURE(expr, description,...)
static SOPHUS_FUNC Tangent log(SO3< Scalar > const &other)
typename Base::Tangent Tangent
Matrix< Scalar, DoF, DoF > Adjoint
Vector< Scalar, DoF > Tangent
static SOPHUS_FUNC Transformation hat(Tangent const &omega)
static SOPHUS_FUNC Adjoint d_lieBracketab_by_d_a(Tangent const &b)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SO3()
Matrix< Scalar, N, N > Transformation
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
SOPHUS_FUNC QuaternionMember const & unit_quaternion() const
QuaternionMember unit_quaternion_
static SOPHUS_FUNC SO3 rotX(Scalar const &x)
Map< Eigen::Quaternion< Scalar >, Options > unit_quaternion_
SOPHUS_FUNC SO3< Scalar > inverse() const
SOPHUS_FUNC Scalar const * data() const
SOPHUS_FUNC QuaternionType & unit_quaternion_nonconst()
T value(details::expression_node< T > *n)
typename Base::Adjoint Adjoint
SOPHUS_FUNC SO3< NewScalarType > cast() const
static SOPHUS_FUNC Scalar pi()
static SOPHUS_FUNC SO3< Scalar > exp(Tangent const &omega)
static SOPHUS_FUNC Transformation generator(int i)
SOPHUS_FUNC void setQuaternion(Eigen::Quaternion< Scalar > const &quaternion)
Map< Eigen::Quaternion< Scalar > const, Options > QuaternionType
SOPHUS_FUNC SO3(Transformation const &R)
auto transpose(T const &p) -> decltype(details::Transpose< T >::impl(T()))
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar > const, Options > const & unit_quaternion() const
Eigen::Matrix< Scalar, M, N > Matrix
SOPHUS_FUNC Vector< Scalar, num_parameters > internalMultiplyByGenerator(int i) const
typename Base::Adjoint Adjoint
static SOPHUS_FUNC Scalar epsilon()
static SOPHUS_FUNC void internalGenerator(int i, Eigen::Quaternion< Scalar > *internal_gen_q)
SOPHUS_FUNC Tangent log() const
typename Base::Point Point
Vector< Scalar, 3, Options > Vector3
SOPHUS_FUNC SO3Base< Derived > & operator*=(SO3< Scalar > const &other)
SOPHUS_FUNC Matrix< Scalar, num_parameters, DoF > internalJacobian() const
SOPHUS_FUNC Adjoint Adj() const
Eigen::Quaternion< Scalar, Options > QuaternionType
static SOPHUS_FUNC SO3 rotZ(Scalar const &z)
void normalize(Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
typename Base::Point Point
static SOPHUS_FUNC Tangent logAndTheta(SO3< Scalar > const &other, Scalar *theta)
typename Base::Transformation Transformation
Eigen::Quaternion< Scalar, Options > QuaternionMember
SOPHUS_FUNC SO3Base< Derived > & operator=(SO3Base< OtherDerived > const &other)
typename Eigen::internal::traits< SO3< Scalar_, Options > >::QuaternionType QuaternionType
SOPHUS_FUNC SO3< Scalar > operator*(SO3< Scalar > const &other) const
typename Base::Transformation Transformation
SOPHUS_FUNC Transformation matrix() const
Eigen::Matrix< Scalar, M, 1, Options > Vector
SOPHUS_FUNC QuaternionMember & unit_quaternion_nonconst()
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
SOPHUS_FUNC SO3(SO3Base< OtherDerived > const &other)
SOPHUS_FUNC SO3(Eigen::QuaternionBase< D > const &quat)
Map< Eigen::Quaternion< Scalar > const, Options > const unit_quaternion_
static SOPHUS_FUNC Tangent lieBracket(Tangent const &omega1, Tangent const &omega2)
SOPHUS_FUNC Point operator*(Point const &p) const
static SOPHUS_FUNC SO3< Scalar > expAndTheta(Tangent const &omega, Scalar *theta)
static SOPHUS_FUNC SO3 rotY(Scalar const &y)
SOPHUS_FUNC QuaternionType const & unit_quaternion() const
SOPHUS_FUNC Scalar * data()
SOPHUS_FUNC Map< Eigen::Quaternion< Scalar >, Options > & unit_quaternion_nonconst()
typename Eigen::internal::traits< SO3< Scalar_, Options > >::Scalar Scalar
SOPHUS_FUNC void normalize()
SOPHUS_FUNC Map(Scalar const *coeffs)