Program Listing for File so2.hpp

↰ Return to documentation for file (sophus/so2.hpp)

#pragma once

#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_>> {
  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 internal
}  // namespace Eigen

namespace Sophus {

template <class Derived>
class SO2Base {
 public:
  static constexpr int Options = Eigen::internal::traits<Derived>::Options;
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  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;
  static int constexpr Dim = 2;
  using Transformation = Matrix<Scalar, N, N>;
  using Point = Vector2<Scalar>;
  using HomogeneousPoint = Vector3<Scalar>;
  using Line = ParametrizedLine2<Scalar>;
  using Hyperplane = Hyperplane2<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::hypot;
    // Avoid under/overflows for higher precision
    Scalar length = hypot(unit_complex().x(), unit_complex().y());
    SOPHUS_ENSURE(length >= Constants<Scalar>::epsilon(), "%s",
                  "Complex number should not be close to zero!");
    unit_complex_nonconst() /= 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;
  }

  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());
  }

  SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const {
    return Hyperplane((*this) * p.normal(), p.offset());
  }

  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 Matrix<Scalar, DoF, num_parameters> Dx_log_this_inv_by_x_at_this()
      const {
    return Matrix<Scalar, DoF, num_parameters>(-unit_complex()[1],
                                               unit_complex()[0]);
  }

  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=;

  SOPHUS_FUNC SO2& operator=(SO2 const& other) = default;

  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 {}",
                  SOPHUS_FMT_ARG(R));
    SOPHUS_ENSURE(R.determinant() > Scalar(0), "det(R) is not positive: {}",
                  SOPHUS_FMT_ARG(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 Sophus::Matrix<Scalar, 2, DoF> Dx_exp_x_times_point_at_0(
      Point const& point) {
    return Point(-point.y(), point.x());
  }

  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>>;

  using Base::operator=;
  using Base::operator*=;
  using Base::operator*;

  SOPHUS_FUNC
  explicit 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 explicit 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