so2.hpp

Go to the documentation of this file.

 
#ifndef SOPHUS_SO2_HPP
#define SOPHUS_SO2_HPP
 
#include <complex>
#include <type_traits>
 
// Include only the selective set of Eigen headers that we need.
// This helps when using Sophus with unusual compilers, like nvcc.
#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 Sophus
 
namespace Eigen {
namespace internal {
 
template <class Scalar_, int Options>
struct traits<Sophus::SO2<Scalar_, 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>> {
  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> {
  using Scalar = Scalar_;
  using ComplexType = Map<Sophus::Vector2<Scalar> const, Options>;
};
}  // namespace internal
}  // namespace Eigen
 
namespace Sophus {
 
template <class Derived>
class SO2Base {
 public:
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  using ComplexT = typename Eigen::internal::traits<Derived>::ComplexType;
 
  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 OtherDerived::Scalar>::ReturnType;
 
  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() { return unit_complex_nonconst().data(); }
 
  SOPHUS_FUNC Scalar const* data() const { return unit_complex().data(); }
 
  SOPHUS_FUNC SO2<Scalar> inverse() const {
    return SO2<Scalar>(unit_complex().x(), -unit_complex().y());
  }
 
  SOPHUS_FUNC Scalar log() const {
    using std::atan2;
    return atan2(unit_complex().y(), unit_complex().x());
  }
 
  SOPHUS_FUNC void normalize() {
    using std::sqrt;
    Scalar length = sqrt(unit_complex().x() * unit_complex().x() +
                         unit_complex().y() * 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 = unit_complex().x();
    Scalar const& imag = unit_complex().y();
    Transformation R;
    // clang-format off
    R <<
      real, -imag,
      imag,  real;
    // clang-format on
    return R;
  }
 
  SOPHUS_FUNC SO2Base& operator=(SO2Base const& other) = default;
 
  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 lhs_real = unit_complex().x();
    Scalar const lhs_imag = unit_complex().y();
    typename OtherDerived::Scalar const& rhs_real = other.unit_complex().x();
    typename OtherDerived::Scalar const& rhs_imag = other.unit_complex().y();
    // complex multiplication
    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;
    // We can assume that the squared-norm is close to 1 since we deal with a
    // unit complex number. Due to numerical precision issues, there might
    // be a small drift after pose concatenation. Hence, we need to renormalizes
    // the complex number here.
    // Since squared-norm is close to 1, we do not need to calculate the costly
    // square-root, but can use an approximation around 1 (see
    // http://stackoverflow.com/a/12934750 for details).
    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 = unit_complex().x();
    Scalar const& imag = unit_complex().y();
    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 = unit_complex().x();
    Scalar const& imag = unit_complex().y();
    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 SO2Base<Derived> operator*=(SO2Base<OtherDerived> const& other) {
    *static_cast<Derived*>(this) = *this * other;
    return *this;
  }
 
  SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
      const {
    return Matrix<Scalar, num_parameters, DoF>(-unit_complex()[1],
                                               unit_complex()[0]);
  }
 
  SOPHUS_FUNC Sophus::Vector<Scalar, num_parameters> params() const {
    return unit_complex();
  }
 
  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>>;
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  SOPHUS_FUNC SO2() : unit_complex_(Scalar(1), Scalar(0)) {}
 
  SOPHUS_FUNC SO2(SO2 const& other) = default;
 
  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) + R(1, 1)),
                      Scalar(0.5) * (R(1, 0) - R(0, 1))) {
    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 const& imag)
      : 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() = SO2<Scalar>::exp(theta).unit_complex();
  }
 
  SOPHUS_FUNC ComplexMember const& unit_complex() const {
    return unit_complex_;
  }
 
  SOPHUS_FUNC static SO2<Scalar> exp(Tangent const& theta) {
    using std::cos;
    using std::sin;
    return SO2<Scalar>(cos(theta), sin(theta));
  }
 
  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
      Tangent const& theta) {
    using std::cos;
    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::Matrix<Scalar, num_parameters, DoF>(Scalar(0), Scalar(1));
  }
 
  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;
    // clang-format off
    Omega <<
        Scalar(0),   -theta,
            theta, Scalar(0);
    // clang-format on
    return Omega;
  }
 
  template <class S = Scalar>
  static SOPHUS_FUNC enable_if_t<std::is_floating_point<S>::value, SO2>
  fitToSO2(Transformation const& R) {
    return SO2(makeRotationMatrix(R));
  }
 
  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 Sophus
 
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>>;
 
  // LCOV_EXCL_START
  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map);
  // LCOV_EXCL_STOP
 
  using Base::operator*=;
  using Base::operator*;
 
  SOPHUS_FUNC
  Map(Scalar* coeffs) : unit_complex_(coeffs) {}
 
  SOPHUS_FUNC
  Map<Sophus::Vector2<Scalar>, Options> const& unit_complex() const {
    return unit_complex_;
  }
 
 protected:
  SOPHUS_FUNC
  Map<Sophus::Vector2<Scalar>, Options>& unit_complex_nonconst() {
    return unit_complex_;
  }
 
  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 Map<Sophus::Vector2<Scalar> const, Options> const& unit_complex()
      const {
    return unit_complex_;
  }
 
 protected:
  Map<Matrix<Scalar, 2, 1> const, Options> const unit_complex_;
};
}  // namespace Eigen
 
#endif  // SOPHUS_SO2_HPP