123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621 |
- #ifndef SOPHUS_SO2_HPP
- #define SOPHUS_SO2_HPP
- #include <complex>
- #include <type_traits>
- #include <Eigen/LU>
- #include "rotation_matrix.hpp"
- #include "types.hpp"
- namespace Sophus {
- template <class Scalar_, int Options = 0>
- class SO2;
- using SO2d = SO2<double>;
- using SO2f = SO2<float>;
- }
- namespace Eigen {
- namespace internal {
- template <class Scalar_, int Options_>
- struct traits<Sophus::SO2<Scalar_, Options_>> {
- static constexpr int Options = Options_;
- using Scalar = Scalar_;
- using ComplexType = Sophus::Vector2<Scalar, Options>;
- };
- template <class Scalar_, int Options_>
- struct traits<Map<Sophus::SO2<Scalar_>, Options_>>
- : traits<Sophus::SO2<Scalar_, Options_>> {
- static constexpr int Options = Options_;
- using Scalar = Scalar_;
- using ComplexType = Map<Sophus::Vector2<Scalar>, Options>;
- };
- template <class Scalar_, int Options_>
- struct traits<Map<Sophus::SO2<Scalar_> const, Options_>>
- : traits<Sophus::SO2<Scalar_, Options_> const> {
- static constexpr int Options = Options_;
- using Scalar = Scalar_;
- using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;
- };
- }
- }
- namespace Sophus {
- template <class Derived>
- class SO2Base {
- public:
- static constexpr int Options = Eigen::internal::traits<Derived>::Options;
- using Scalar = typename ;
- using ComplexT = typename Eigen::internal::traits<Derived>::ComplexType;
- using ComplexTemporaryType = Sophus::Vector2<Scalar, Options>;
-
-
- static int constexpr DoF = 1;
-
- static int constexpr num_parameters = 2;
-
- static int constexpr N = 2;
- using Transformation = Matrix<Scalar, N, N>;
- using Point = Vector2<Scalar>;
- using HomogeneousPoint = Vector3<Scalar>;
- using Line = ParametrizedLine2<Scalar>;
- using Tangent = Scalar;
- using Adjoint = Scalar;
-
-
-
-
- template <typename OtherDerived>
- using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
- Scalar, typename ;
- template <typename OtherDerived>
- using SO2Product = SO2<ReturnScalar<OtherDerived>>;
- template <typename PointDerived>
- using PointProduct = Vector2<ReturnScalar<PointDerived>>;
- template <typename HPointDerived>
- using HomogeneousPointProduct = Vector3<ReturnScalar<HPointDerived>>;
-
-
-
-
-
-
-
-
- SOPHUS_FUNC Adjoint Adj() const { return Scalar(1); }
-
-
- template <class NewScalarType>
- SOPHUS_FUNC SO2<NewScalarType> cast() const {
- return SO2<NewScalarType>(unit_complex().template cast<NewScalarType>());
- }
-
-
-
-
-
- SOPHUS_FUNC Scalar* data() ; }
-
-
- SOPHUS_FUNC Scalar const* data() ; }
-
-
- SOPHUS_FUNC SO2<Scalar> inverse()
- return ;
- }
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC Scalar log()
- using ;
- return atan2(unit_complex().y(), unit_complex().x());
- }
-
-
-
-
-
- SOPHUS_FUNC void normalize() {
- using std::sqrt;
- Scalar length
- unit_complex().y() ;
- SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(),
- "Complex number should not be close to zero!");
- unit_complex_nonconst().x() /= length;
- unit_complex_nonconst().y() /= length;
- }
-
-
-
-
-
- SOPHUS_FUNC Transformation matrix() const {
- Scalar const& real ;
- Scalar const& imag ;
- Transformation R;
-
- R <<
- real, -imag,
- imag, real;
-
- return R;
- }
-
-
- template <class OtherDerived>
- SOPHUS_FUNC SO2Base<Derived>& operator=(SO2Base<OtherDerived> const& other) {
- unit_complex_nonconst() = other.unit_complex();
- return *this;
- }
-
-
- template <typename OtherDerived>
- SOPHUS_FUNC SO2Product<OtherDerived> operator*(
- SO2Base<OtherDerived> const& other) const {
- using ResultT = ReturnScalar<OtherDerived>;
- Scalar const ;
- Scalar const ;
- typename OtherDerived::Scalar const& rhs_real ;
- typename OtherDerived::Scalar const& rhs_imag ;
-
- ResultT const result_real = lhs_real * rhs_real - lhs_imag * rhs_imag;
- ResultT const result_imag = lhs_real * rhs_imag + lhs_imag * rhs_real;
- ResultT const squared_norm =
- result_real * result_real + result_imag * result_imag;
-
-
-
-
-
-
-
- if (squared_norm != ResultT(1.0)) {
- ResultT const scale = ResultT(2.0) / (ResultT(1.0) + squared_norm);
- return SO2Product<OtherDerived>(result_real * scale, result_imag * scale);
- }
- return SO2Product<OtherDerived>(result_real, result_imag);
- }
-
-
-
-
-
- template <typename PointDerived,
- typename = typename std::enable_if<
- IsFixedSizeVector<PointDerived, 2>::value>::type>
- SOPHUS_FUNC PointProduct<PointDerived> operator*(
- Eigen::MatrixBase<PointDerived> const& p) const {
- Scalar const& real ;
- Scalar const& imag ;
- return PointProduct<PointDerived>(real * p[0] - imag * p[1],
- imag * p[0] + real * p[1]);
- }
-
-
-
-
-
- template <typename HPointDerived,
- typename = typename std::enable_if<
- IsFixedSizeVector<HPointDerived, 3>::value>::type>
- SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
- Eigen::MatrixBase<HPointDerived> const& p) const {
- Scalar const& real ;
- Scalar const& imag ;
- return HomogeneousPointProduct<HPointDerived>(
- real * p[0] - imag * p[1], imag * p[0] + real * p[1], p[2]);
- }
-
-
-
-
-
-
-
- SOPHUS_FUNC Line operator*(Line const& l) const {
- return Line((*this) * l.origin(), (*this) * l.direction());
- }
-
-
-
- template <typename OtherDerived,
- typename = typename std::enable_if<
- std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
- SOPHUS_FUNC
- *static_cast<Derived*>(this) = *this ;
- return *this;
- }
-
-
- SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
- const
- return
- unit_complex()[0]);
- }
-
-
-
-
- SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params()
- return ;
- }
-
-
-
-
- SOPHUS_FUNC void setComplex(Point const& complex) {
- unit_complex_nonconst() = complex;
- normalize();
- }
-
-
- SOPHUS_FUNC
- ComplexT const& unit_complex() const {
- return static_cast<Derived const*>(this)->unit_complex();
- }
- private:
-
-
-
- SOPHUS_FUNC
- ComplexT& unit_complex_nonconst() {
- return static_cast<Derived*>(this)->unit_complex_nonconst();
- }
- };
- template <class Scalar_, int Options>
- class SO2 : public SO2Base<SO2<Scalar_, Options>> {
- public:
- using Base = SO2Base<SO2<Scalar_, Options>>;
- static int constexpr DoF = Base::DoF;
- static int constexpr num_parameters = Base::num_parameters;
- using Scalar = Scalar_;
- using Transformation = typename Base::Transformation;
- using Point = typename Base::Point;
- using HomogeneousPoint = typename Base::HomogeneousPoint;
- using Tangent = typename Base::Tangent;
- using Adjoint = typename Base::Adjoint;
- using ComplexMember = Vector2<Scalar, Options>;
-
- friend class SO2Base<SO2<Scalar, Options>>;
- using Base::operator=;
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
-
- SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
-
-
- SOPHUS_FUNC ;
-
-
- template <class OtherDerived>
- SOPHUS_FUNC SO2(SO2Base<OtherDerived> const& other)
- : unit_complex_(other.unit_complex()) {}
-
-
-
-
- SOPHUS_FUNC explicit SO2(Transformation const& R)
- : unit_complex_(Scalar(0.5) * (R(0, 0)
- Scalar(0.5) * (R(1, 0)
- SOPHUS_ENSURE(isOrthogonal(R), "R is not orthogonal:\n %", R);
- SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: %",
- R.determinant());
- }
-
-
-
-
- SOPHUS_FUNC SO2(Scalar const& real, Scalar
- : unit_complex_(real, imag)
- Base::normalize();
- }
-
-
-
-
- template <class D>
- SOPHUS_FUNC explicit SO2(Eigen::MatrixBase<D> const& complex)
- : unit_complex_(complex) {
- static_assert(std::is_same<typename D::Scalar, Scalar>::value,
- "must be same Scalar type");
- Base::normalize();
- }
-
-
- SOPHUS_FUNC explicit SO2(Scalar theta) {
- unit_complex_nonconst() ;
- }
-
-
- SOPHUS_FUNC ComplexMember const& unit_complex() const {
- return unit_complex_;
- }
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static SO2<Scalar> exp(Tangent
- using ;
- using std::sin;
- return SO2<Scalar>(cos(theta), sin(theta));
- }
-
-
- SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
- Tangent
- using ;
- using std::sin;
- return Sophus::Matrix<Scalar, num_parameters, DoF>(-sin(theta), cos(theta));
- }
-
-
- SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
- Dx_exp_x_at_0()
- return ;
- }
-
-
- SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int) {
- return generator();
- }
-
-
-
-
-
-
-
- SOPHUS_FUNC static Transformation generator() { return hat(Scalar(1)); }
-
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Transformation hat(Tangent const& theta) {
- Transformation Omega;
-
- Omega <<
- Scalar(0), -theta,
- theta, Scalar(0);
-
- return Omega;
- }
-
-
- template <class S = Scalar>
- static
- fitToSO2(Transformation
- return ;
- }
-
-
-
-
-
- SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
- return Scalar(0);
- }
-
-
- template <class UniformRandomBitGenerator>
- static SO2 sampleUniform(UniformRandomBitGenerator& generator) {
- static_assert(IsUniformRandomBitGenerator<UniformRandomBitGenerator>::value,
- "generator must meet the UniformRandomBitGenerator concept");
- std::uniform_real_distribution<Scalar> uniform(-Constants<Scalar>::pi(),
- Constants<Scalar>::pi());
- return SO2(uniform(generator));
- }
-
-
-
-
-
-
-
-
-
-
-
-
- SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
- using std::abs;
- return Omega(1, 0);
- }
- protected:
-
-
- SOPHUS_FUNC ComplexMember& unit_complex_nonconst() { return unit_complex_; }
- ComplexMember unit_complex_;
- };
- }
- namespace Eigen {
- template <class Scalar_, int Options>
- class Map<Sophus::SO2<Scalar_>, Options>
- : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>> {
- public:
- using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
- using Scalar = Scalar_;
- using Transformation = typename Base::Transformation;
- using Point = typename Base::Point;
- using HomogeneousPoint = typename Base::HomogeneousPoint;
- using Tangent = typename Base::Tangent;
- using Adjoint = typename Base::Adjoint;
-
- friend class Sophus::SO2Base<Map<Sophus::SO2<Scalar_>, Options>>;
- using Base::operator=;
- using Base::operator*=;
- using Base::operator*;
- SOPHUS_FUNC
- Map(Scalar* coeffs) : unit_complex_(coeffs)
-
-
- SOPHUS_FUNC
- Map<Sophus::Vector2<Scalar>, Options> const& unit_complex()
- return ;
- }
- protected:
-
-
- SOPHUS_FUNC
- Map<Sophus::Vector2<Scalar>, Options>& unit_complex_nonconst()
- return ;
- }
- Map<Matrix<Scalar, 2, 1>, Options> unit_complex_;
- };
- template <class Scalar_, int Options>
- class Map<Sophus::SO2<Scalar_> const, Options>
- : public Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>> {
- public:
- using Base = Sophus::SO2Base<Map<Sophus::SO2<Scalar_> const, Options>>;
- using Scalar = Scalar_;
- using Transformation = typename Base::Transformation;
- using Point = typename Base::Point;
- using HomogeneousPoint = typename Base::HomogeneousPoint;
- using Tangent = typename Base::Tangent;
- using Adjoint = typename Base::Adjoint;
- using Base::operator*=;
- using Base::operator*;
- SOPHUS_FUNC Map(Scalar const* coeffs) : unit_complex_(coeffs)
-
-
- SOPHUS_FUNC
- const
- return ;
- }
- protected:
-
-
- Map<Matrix<Scalar, 2, 1> const, Options> const ;
- };
- }
- #endif
|