types.hpp

Go to the documentation of this file.

 
#ifndef SOPHUS_TYPES_HPP
#define SOPHUS_TYPES_HPP
 
#include <type_traits>
#include "common.hpp"
 
namespace Sophus {
 
template <class Scalar, int M, int Options = 0>
using Vector = Eigen::Matrix<Scalar, M, 1, Options>;
 
template <class Scalar, int Options = 0>
using Vector2 = Vector<Scalar, 2, Options>;
using Vector2f = Vector2<float>;
using Vector2d = Vector2<double>;
 
template <class Scalar, int Options = 0>
using Vector3 = Vector<Scalar, 3, Options>;
using Vector3f = Vector3<float>;
using Vector3d = Vector3<double>;
 
template <class Scalar>
using Vector4 = Vector<Scalar, 4>;
using Vector4f = Vector4<float>;
using Vector4d = Vector4<double>;
 
template <class Scalar>
using Vector6 = Vector<Scalar, 6>;
using Vector6f = Vector6<float>;
using Vector6d = Vector6<double>;
 
template <class Scalar>
using Vector7 = Vector<Scalar, 7>;
using Vector7f = Vector7<float>;
using Vector7d = Vector7<double>;
 
template <class Scalar, int M, int N>
using Matrix = Eigen::Matrix<Scalar, M, N>;
 
template <class Scalar>
using Matrix2 = Matrix<Scalar, 2, 2>;
using Matrix2f = Matrix2<float>;
using Matrix2d = Matrix2<double>;
 
template <class Scalar>
using Matrix3 = Matrix<Scalar, 3, 3>;
using Matrix3f = Matrix3<float>;
using Matrix3d = Matrix3<double>;
 
template <class Scalar>
using Matrix4 = Matrix<Scalar, 4, 4>;
using Matrix4f = Matrix4<float>;
using Matrix4d = Matrix4<double>;
 
template <class Scalar>
using Matrix6 = Matrix<Scalar, 6, 6>;
using Matrix6f = Matrix6<float>;
using Matrix6d = Matrix6<double>;
 
template <class Scalar>
using Matrix7 = Matrix<Scalar, 7, 7>;
using Matrix7f = Matrix7<float>;
using Matrix7d = Matrix7<double>;
 
template <class Scalar, int N, int Options = 0>
using ParametrizedLine = Eigen::ParametrizedLine<Scalar, N, Options>;
 
template <class Scalar, int Options = 0>
using ParametrizedLine3 = ParametrizedLine<Scalar, 3, Options>;
using ParametrizedLine3f = ParametrizedLine3<float>;
using ParametrizedLine3d = ParametrizedLine3<double>;
 
template <class Scalar, int Options = 0>
using ParametrizedLine2 = ParametrizedLine<Scalar, 2, Options>;
using ParametrizedLine2f = ParametrizedLine2<float>;
using ParametrizedLine2d = ParametrizedLine2<double>;
 
namespace details {
template <class Scalar>
class MaxMetric {
 public:
  static Scalar impl(Scalar s0, Scalar s1) {
    using std::abs;
    return abs(s0 - s1);
  }
};
 
template <class Scalar, int M, int N>
class MaxMetric<Matrix<Scalar, M, N>> {
 public:
  static Scalar impl(Matrix<Scalar, M, N> const& p0,
                     Matrix<Scalar, M, N> const& p1) {
    return (p0 - p1).template lpNorm<Eigen::Infinity>();
  }
};
 
template <class Scalar>
class SetToZero {
 public:
  static void impl(Scalar& s) { s = Scalar(0); }
};
 
template <class Scalar, int M, int N>
class SetToZero<Matrix<Scalar, M, N>> {
 public:
  static void impl(Matrix<Scalar, M, N>& v) { v.setZero(); }
};
 
template <class T1, class Scalar>
class SetElementAt;
 
template <class Scalar>
class SetElementAt<Scalar, Scalar> {
 public:
  static void impl(Scalar& s, Scalar value, int at) {
    SOPHUS_ENSURE(at == 0, "is %", at);
    s = value;
  }
};
 
template <class Scalar, int N>
class SetElementAt<Vector<Scalar, N>, Scalar> {
 public:
  static void impl(Vector<Scalar, N>& v, Scalar value, int at) {
    SOPHUS_ENSURE(at >= 0 && at < N, "is %", at);
    v[at] = value;
  }
};
 
template <class Scalar>
class SquaredNorm {
 public:
  static Scalar impl(Scalar const& s) { return s * s; }
};
 
template <class Scalar, int N>
class SquaredNorm<Matrix<Scalar, N, 1>> {
 public:
  static Scalar impl(Matrix<Scalar, N, 1> const& s) { return s.squaredNorm(); }
};
 
template <class Scalar>
class Transpose {
 public:
  static Scalar impl(Scalar const& s) { return s; }
};
 
template <class Scalar, int M, int N>
class Transpose<Matrix<Scalar, M, N>> {
 public:
  static Matrix<Scalar, M, N> impl(Matrix<Scalar, M, N> const& s) {
    return s.transpose();
  }
};
}  // namespace details
 
template <class T>
auto maxMetric(T const& p0, T const& p1)
    -> decltype(details::MaxMetric<T>::impl(p0, p1)) {
  return details::MaxMetric<T>::impl(p0, p1);
}
 
template <class T>
void setToZero(T& p) {
  return details::SetToZero<T>::impl(p);
}
 
template <class T, class Scalar>
void setElementAt(T& p, Scalar value, int i) {
  return details::SetElementAt<T, Scalar>::impl(p, value, i);
}
 
template <class T>
auto squaredNorm(T const& p) -> decltype(details::SquaredNorm<T>::impl(p)) {
  return details::SquaredNorm<T>::impl(p);
}
 
template <class T>
auto transpose(T const& p) -> decltype(details::Transpose<T>::impl(T())) {
  return details::Transpose<T>::impl(p);
}
 
template <class Scalar>
struct IsFloatingPoint {
  static bool const value = std::is_floating_point<Scalar>::value;
};
 
template <class Scalar, int M, int N>
struct IsFloatingPoint<Matrix<Scalar, M, N>> {
  static bool const value = std::is_floating_point<Scalar>::value;
};
 
template <class Scalar_>
struct GetScalar {
  using Scalar = Scalar_;
};
 
template <class Scalar_, int M, int N>
struct GetScalar<Matrix<Scalar_, M, N>> {
  using Scalar = Scalar_;
};
 
template <typename Vector, int NumDimensions,
          typename = typename std::enable_if<
              Vector::RowsAtCompileTime == NumDimensions &&
              Vector::ColsAtCompileTime == 1>::type>
struct IsFixedSizeVector : std::true_type {};
 
template <class T>
using Plane3 = Eigen::Hyperplane<T, 3>;
using Plane3d = Plane3<double>;
using Plane3f = Plane3<float>;
 
template <class T>
using Line2 = Eigen::Hyperplane<T, 2>;
using Line2d = Line2<double>;
using Line2f = Line2<float>;
 
}  // namespace Sophus
 
#endif  // SOPHUS_TYPES_HPP