Program Listing for File cartesian.hpp

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

#pragma once
#include <sophus/types.hpp>

namespace Sophus {
template <class Scalar_, int M, int Options = 0>
class Cartesian;

template <class Scalar_>
using Cartesian2 = Cartesian<Scalar_, 2>;

template <class Scalar_>
using Cartesian3 = Cartesian<Scalar_, 3>;

using Cartesian2d = Cartesian2<double>;
using Cartesian3d = Cartesian3<double>;

}  // namespace Sophus

namespace Eigen {
namespace internal {

template <class Scalar_, int M, int Options>
struct traits<Sophus::Cartesian<Scalar_, M, Options>> {
  using Scalar = Scalar_;
  using ParamsType = Sophus::Vector<Scalar, M, Options>;
};

template <class Scalar_, int M, int Options>
struct traits<Map<Sophus::Cartesian<Scalar_, M>, Options>>
    : traits<Sophus::Cartesian<Scalar_, M, Options>> {
  using Scalar = Scalar_;
  using ParamsType = Map<Sophus::Vector<Scalar, M>, Options>;
};

template <class Scalar_, int M, int Options>
struct traits<Map<Sophus::Cartesian<Scalar_, M> const, Options>>
    : traits<Sophus::Cartesian<Scalar_, M, Options> const> {
  using Scalar = Scalar_;
  using ParamsType = Map<Sophus::Vector<Scalar, M> const, Options>;
};
}  // namespace internal
}  // namespace Eigen

namespace Sophus {

//  (M+1) x (M+1) homogeneous matrix:
template <class Derived, int M>
class CartesianBase {
 public:
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  using ParamsType = typename Eigen::internal::traits<Derived>::ParamsType;
  static int constexpr DoF = M;
  static int constexpr num_parameters = M;
  static int constexpr N = M + 1;
  static int constexpr Dim = M;

  using Transformation = Sophus::Matrix<Scalar, N, N>;
  using Point = Sophus::Vector<Scalar, M>;
  using HomogeneousPoint = Sophus::Vector<Scalar, N>;
  using Line = ParametrizedLine<Scalar, M>;
  using Hyperplane = Eigen::Hyperplane<Scalar, M>;
  using Tangent = Sophus::Vector<Scalar, DoF>;
  using Adjoint = Matrix<Scalar, DoF, DoF>;

  template <typename OtherDerived>
  using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<
      Scalar, typename OtherDerived::Scalar>::ReturnType;

  template <typename OtherDerived>
  using CartesianSum = Cartesian<ReturnScalar<OtherDerived>, M>;

  template <typename PointDerived>
  using PointProduct = Sophus::Vector<ReturnScalar<PointDerived>, M>;

  template <typename HPointDerived>
  using HomogeneousPointProduct =
      Sophus::Vector<ReturnScalar<HPointDerived>, N>;

  SOPHUS_FUNC Adjoint Adj() const { return Adjoint::Identity(); }

  template <class NewScalarType>
  SOPHUS_FUNC Cartesian<NewScalarType, M> cast() const {
    return Cartesian<NewScalarType, M>(params().template cast<NewScalarType>());
  }

  SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_this_mul_exp_x_at_0()
      const {
    Sophus::Matrix<Scalar, num_parameters, DoF> m;
    m.setIdentity();
    return m;
  }

  SOPHUS_FUNC Matrix<Scalar, num_parameters, DoF> Dx_log_this_inv_by_x_at_this()
      const {
    Matrix<Scalar, DoF, num_parameters> m;
    m.setIdentity();
    return m;
  }

  SOPHUS_FUNC Cartesian<Scalar, M> inverse() const {
    return Cartesian<Scalar, M>(-params());
  }

  SOPHUS_FUNC Tangent log() const { return params(); }

  SOPHUS_FUNC Transformation matrix() const {
    Sophus::Matrix<Scalar, N, N> matrix;
    matrix.setIdentity();
    matrix.col(M).template head<M>() = params();
    return matrix;
  }

  template <class OtherDerived>
  SOPHUS_FUNC CartesianBase<Derived, M>& operator=(
      CartesianBase<OtherDerived, M> const& other) {
    params() = other.params();
    return *this;
  }

  template <typename OtherDerived>
  SOPHUS_FUNC CartesianSum<OtherDerived> operator*(
      CartesianBase<OtherDerived, M> const& other) const {
    return CartesianSum<OtherDerived>(params() + other.params());
  }

  template <typename PointDerived,
            typename = typename std::enable_if<
                IsFixedSizeVector<PointDerived, M>::value>::type>
  SOPHUS_FUNC PointProduct<PointDerived> operator*(
      Eigen::MatrixBase<PointDerived> const& p) const {
    return PointProduct<PointDerived>(params() + p);
  }

  template <typename HPointDerived,
            typename = typename std::enable_if<
                IsFixedSizeVector<HPointDerived, N>::value>::type>
  SOPHUS_FUNC HomogeneousPointProduct<HPointDerived> operator*(
      Eigen::MatrixBase<HPointDerived> const& p) const {
    const auto rp = *this * p.template head<M>();
    HomogeneousPointProduct<HPointDerived> r;
    r << rp, p(M);
    return r;
  }

  SOPHUS_FUNC Line operator*(Line const& l) const {
    return Line((*this) * l.origin(), l.direction());
  }

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

  template <typename OtherDerived,
            typename = typename std::enable_if<
                std::is_same<Scalar, ReturnScalar<OtherDerived>>::value>::type>
  SOPHUS_FUNC CartesianBase<Derived, M>& operator*=(
      CartesianBase<OtherDerived, M> const& other) {
    *static_cast<Derived*>(this) = *this * other;
    return *this;
  }

  SOPHUS_FUNC ParamsType& params() {
    return static_cast<Derived*>(this)->params();
  }

  SOPHUS_FUNC ParamsType const& params() const {
    return static_cast<Derived const*>(this)->params();
  }
};

template <class Scalar_, int M, int Options>
class Cartesian : public CartesianBase<Cartesian<Scalar_, M, Options>, M> {
  using Base = CartesianBase<Cartesian<Scalar_, M, Options>, M>;

 public:
  static int constexpr DoF = Base::DoF;
  static int constexpr num_parameters = Base::num_parameters;
  static int constexpr N = Base::N;
  static int constexpr Dim = Base::Dim;

  using Scalar = Scalar_;
  using Transformation = typename Base::Transformation;
  using Point = typename Base::Point;
  using HomogeneousPoint = typename Base::HomogeneousPoint;
  using Tangent = typename Base::Tangent;
  using ParamsMember = Sophus::Vector<Scalar, M, Options>;

  using Base::operator=;

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

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  SOPHUS_FUNC Cartesian() { params_.setZero(); }

  SOPHUS_FUNC Cartesian(Cartesian const& other) = default;

  template <class OtherDerived>
  SOPHUS_FUNC Cartesian(CartesianBase<OtherDerived, M> const& other)
      : params_(other.params()) {
    static_assert(std::is_same<typename OtherDerived::Scalar, Scalar>::value,
                  "must be same Scalar type");
  }

  template <class D>
  explicit SOPHUS_FUNC Cartesian(Eigen::MatrixBase<D> const& m) {
    static_assert(
        std::is_same<typename Eigen::MatrixBase<D>::Scalar, Scalar>::value, "");
    if (m.rows() == DoF && m.cols() == 1) {
      // trick so this compiles
      params_ = m.template block<M, 1>(0, 0);
    } else if (m.rows() == N && m.cols() == N) {
      params_ = m.template block<M, 1>(0, M);
    } else {
      SOPHUS_ENSURE(false, "{} {}", m.rows(), m.cols());
    }
  }

  SOPHUS_FUNC Scalar* data() { return params_.data(); }

  SOPHUS_FUNC Scalar const* data() const { return params_.data(); }

  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF>
  Dx_exp_x_at_0() {
    Sophus::Matrix<Scalar, num_parameters, DoF> m;
    m.setIdentity();
    return m;
  }

  SOPHUS_FUNC static Sophus::Matrix<Scalar, num_parameters, DoF> Dx_exp_x(
      Tangent const&) {
    return Dx_exp_x_at_0();
  }

  SOPHUS_FUNC static Sophus::Matrix<Scalar, Dim, DoF> Dx_exp_x_times_point_at_0(
      Point const&) {
    Sophus::Matrix<Scalar, Dim, DoF> J;
    J.setIdentity();
    return J;
  }

  SOPHUS_FUNC static Transformation Dxi_exp_x_matrix_at_0(int i) {
    return generator(i);
  }

  SOPHUS_FUNC ParamsMember& params() { return params_; }

  SOPHUS_FUNC ParamsMember const& params() const { return params_; }

  SOPHUS_FUNC static Transformation generator(int i) {
    SOPHUS_ENSURE(i >= 0 && i <= M, "i should be in range [0,M-1].");
    Tangent e;
    e.setZero();
    e[i] = Scalar(1);
    return hat(e);
  }

  //  from the M-vector.
  SOPHUS_FUNC static Cartesian<Scalar, M> exp(Tangent const& a) {
    return Cartesian<Scalar, M>(a);
  }

  SOPHUS_FUNC static Transformation hat(Tangent const& a) {
    Transformation Omega;
    Omega.setZero();
    Omega.col(M).template head<M>() = a.template head<M>();
    return Omega;
  }

  SOPHUS_FUNC static Tangent lieBracket(Tangent const&, Tangent const&) {
    return Tangent::Zero();
  }

  template <class UniformRandomBitGenerator>
  static Cartesian sampleUniform(UniformRandomBitGenerator& generator) {
    std::uniform_real_distribution<Scalar> uniform(Scalar(-1), Scalar(1));
    Vector<Scalar, M> v;
    for (int i = 0; i < M; ++i) {
      v[i] = uniform(generator);
    }
    return Cartesian(v);
  }

  SOPHUS_FUNC static Tangent vee(Transformation const& m) {
    return m.col(M).template head<M>();
  }

 protected:
  ParamsMember params_;
};

}  // namespace Sophus

namespace Eigen {

template <class Scalar_, int M, int Options>
class Map<Sophus::Cartesian<Scalar_, M>, Options>
    : public Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M>, Options>,
                                   M> {
 public:
  using Base =
      Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M>, Options>, M>;
  using Scalar = Scalar_;
  using Transformation = typename Base::Transformation;
  using Point = typename Base::Point;
  using HomogeneousPoint = typename Base::HomogeneousPoint;
  using Tangent = typename Base::Tangent;

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

  SOPHUS_FUNC explicit Map(Scalar* coeffs) : params_(coeffs) {}

  SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>>& params() {
    return params_;
  }

  SOPHUS_FUNC Map<Sophus::Vector<Scalar, M, Options>> const& params() const {
    return params_;
  }

 protected:
  Map<Sophus::Vector<Scalar, M>, Options> params_;
};

template <class Scalar_, int M, int Options>
class Map<Sophus::Cartesian<Scalar_, M> const, Options>
    : public Sophus::CartesianBase<
          Map<Sophus::Cartesian<Scalar_, M> const, Options>, M> {
 public:
  using Base =
      Sophus::CartesianBase<Map<Sophus::Cartesian<Scalar_, M> const, Options>,
                            M>;
  using Scalar = Scalar_;
  using Transformation = typename Base::Transformation;
  using Point = typename Base::Point;
  using HomogeneousPoint = typename Base::HomogeneousPoint;
  using Tangent = typename Base::Tangent;

  using Base::operator*;

  SOPHUS_FUNC Map(Scalar const* coeffs) : params_(coeffs) {}

  SOPHUS_FUNC Map<Sophus::Vector<Scalar, M> const, Options> const& params()
      const {
    return params_;
  }

 protected:
  Map<Sophus::Vector<Scalar, M> const, Options> const params_;
};
}  // namespace Eigen