13 template <
class Scalar_,
int Options = 0>
22 template <
class Scalar_,
int Options>
23 struct traits<
Sophus::SO2<Scalar_, Options>> {
28 template <
class Scalar_,
int Options>
29 struct traits<Map<
Sophus::SO2<Scalar_>, Options>>
30 : traits<Sophus::SO2<Scalar_, Options>> {
35 template <
class Scalar_,
int Options>
36 struct traits<Map<
Sophus::SO2<Scalar_> const, Options>>
37 : traits<Sophus::SO2<Scalar_, Options> const> {
39 using ComplexType = Map<Sophus::Vector2<Scalar>
const, Options>;
71 template <
class Derived>
74 using Scalar =
typename Eigen::internal::traits<Derived>::Scalar;
75 using Complex =
typename Eigen::internal::traits<Derived>::ComplexType;
79 static int constexpr DoF = 1;
81 static int constexpr num_parameters = 2;
83 static int constexpr N = 2;
101 template <
class NewScalarType>
120 return SO2<Scalar>(unit_complex().x(), -unit_complex().y());
135 Scalar length = std::sqrt(unit_complex().
x() * unit_complex().
x() +
136 unit_complex().y() * unit_complex().y());
138 "Complex number should not be close to zero!");
139 unit_complex_nonconst().x() /= length;
140 unit_complex_nonconst().y() /= length;
149 Scalar const& real = unit_complex().x();
150 Scalar const& imag = unit_complex().y();
162 template <
class OtherDerived>
182 Scalar const& real = unit_complex().x();
183 Scalar const& imag = unit_complex().y();
184 return Point(real * p[0] - imag * p[1], imag * p[0] + real * p[1]);
190 Scalar lhs_real = unit_complex().x();
191 Scalar lhs_imag = unit_complex().y();
195 unit_complex_nonconst().x() = lhs_real * rhs_real - lhs_imag * rhs_imag;
196 unit_complex_nonconst().y() = lhs_real * rhs_imag + lhs_imag * rhs_real;
198 Scalar squared_norm = unit_complex_nonconst().squaredNorm();
206 if (squared_norm !=
Scalar(1.0)) {
207 unit_complex_nonconst() *=
Scalar(2.0) / (
Scalar(1.0) + squared_norm);
217 unit_complex_nonconst() = complex;
225 return static_cast<Derived const*
>(
this)->unit_complex();
242 return SO2<Scalar>(std::cos(theta), std::sin(theta));
324 return static_cast<Derived*
>(
this)->unit_complex_nonconst();
329 template <
class Scalar_,
int Options>
330 class SO2 :
public SO2Base<SO2<Scalar_, Options>> {
344 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
352 template <
class OtherDerived>
354 : unit_complex_(other.unit_complex()) {}
361 : unit_complex_(
Scalar(0.5) * (R(0, 0) + R(1, 1)),
362 Scalar(0.5) * (R(1, 0) - R(0, 1))) {
365 "det(R) should be (close to) 1.");
373 : unit_complex_(real, imag) {
383 : unit_complex_(complex) {
385 "must be same Scalar type");
398 return unit_complex_;
417 template <
class Scalar_,
int Options>
433 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
434 using Base::operator*=;
435 using Base::operator*;
438 Map(Scalar* coeffs) : unit_complex_(coeffs) {}
444 return unit_complex_;
452 return unit_complex_;
462 template <
class Scalar_,
int Options>
463 class Map<
Sophus::SO2<Scalar_> const, Options>
474 EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
475 using Base::operator*=;
476 using Base::operator*;
484 return unit_complex_;
494 #endif // SOPHUS_SO2_HPP SOPHUS_FUNC Complex & unit_complex_nonconst()
SOPHUS_FUNC Map< Sophus::Vector2< Scalar >, Options > & unit_complex_nonconst()
#define SOPHUS_ENSURE(expr, description,...)
Sophus::Vector2< Scalar, Options > ComplexType
SOPHUS_FUNC Point operator*(Point const &p) const
typename Base::Tangent Tangent
Map< Matrix< Scalar, 2, 1 > const, Options > const unit_complex_
static SOPHUS_FUNC SO2< Scalar > exp(Tangent const &theta)
SOPHUS_FUNC SO2< NewScalarType > cast() const
SOPHUS_FUNC SO2(Transformation const &R)
Matrix< Scalar, N, N > Transformation
SOPHUS_FUNC SO2Base< Derived > & operator=(SO2Base< OtherDerived > const &other)
T value(details::expression_node< T > *n)
SOPHUS_FUNC SO2(SO2Base< OtherDerived > const &other)
typename Base::Adjoint Adjoint
typename Base::Tangent Tangent
SOPHUS_FUNC SO2< Scalar > operator*(SO2< Scalar > const &other) const
SOPHUS_FUNC Adjoint Adj() const
Vector< Scalar, 2, Options > Vector2
typename Base::Adjoint Adjoint
static SOPHUS_FUNC Transformation hat(Tangent const &theta)
SOPHUS_FUNC Scalar * data()
Eigen::Matrix< Scalar, M, N > Matrix
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SO2()
Map< Matrix< Scalar, 2, 1 >, Options > unit_complex_
SOPHUS_FUNC void normalize()
typename Base::Transformation Transformation
SOPHUS_FUNC ComplexMember & unit_complex_nonconst()
typename Eigen::internal::traits< SO2< Scalar, Options > >::Scalar Scalar
ComplexMember unit_complex_
SOPHUS_FUNC Scalar const * data() const
typename Base::Point Point
SOPHUS_FUNC SO2Base< Derived > operator*=(SO2< Scalar > const &other)
SOPHUS_FUNC Scalar log() const
SOPHUS_FUNC Complex const & unit_complex() const
SOPHUS_FUNC SO2(Scalar theta)
static SOPHUS_FUNC Transformation generator()
void normalize(Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
static SOPHUS_FUNC Tangent lieBracket(Tangent const &, Tangent const &)
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 Transformation matrix() const
typename Base::Point Point
static SOPHUS_FUNC Tangent vee(Transformation const &Omega)
Vector2< Scalar, Options > ComplexMember
SOPHUS_FUNC SO2(Scalar const &real, Scalar const &imag)
SOPHUS_FUNC ComplexMember const & unit_complex() const
SOPHUS_FUNC SO2(Eigen::MatrixBase< D > const &complex)
SOPHUS_FUNC void setComplex(Point const &complex)
Map< Sophus::Vector2< Scalar > const, Options > ComplexType
SOPHUS_FUNC Map< Sophus::Vector2< Scalar > const, Options > const & unit_complex() const
SOPHUS_FUNC SO2< Scalar > inverse() const
static SOPHUS_FUNC Tangent log(SO2< Scalar > const &other)
SOPHUS_FUNC Map< Sophus::Vector2< Scalar >, Options > const & unit_complex() const
typename Eigen::internal::traits< SO2< Scalar, Options > >::ComplexType Complex
T atan2(const T v0, const T v1)
SOPHUS_FUNC Map(Scalar const *coeffs)
typename Base::Transformation Transformation