definitions.hpp

Go to the documentation of this file.

 
#ifndef STATEOBSERVATIONDEFINITIONSHPP
#define STATEOBSERVATIONDEFINITIONSHPP
 
// #define STATEOBSERVATION_VERBOUS_CONSTRUCTORS
 
#include <chrono>
#include <deque>
#include <stdexcept>
#include <vector>
 
#ifdef STATEOBSERVATION_VERBOUS_CONSTRUCTORS
#  include <iostream>
#endif
 
#include <boost/assert.hpp>
#include <boost/timer/timer.hpp>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#ifndef M_PI
#  include <boost/math/constants/constants.hpp>
#  define M_PI boost::math::constants::pi<double>()
#endif
 
#include <state-observation/api.h>
 
// basic file operations
#include <fstream>
 
namespace stateObservation
{
template<typename T>
struct isEigen
{
  static constexpr bool value = std::is_base_of<Eigen::EigenBase<T>, T>::value;
};
 
template<typename T>
struct isMatrix : std::false_type
{
};
 
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct isMatrix<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>> : std::true_type
{
};
 
template<typename T>
struct EigenType : std::enable_if<isEigen<T>::value, T>
{
};
 
template<typename T>
struct MatrixType : std::enable_if<isMatrix<T>::value, T>
{
};
 
typedef Eigen::VectorXd Vector;
 
typedef Eigen::Matrix<double, 1, 1> Vector1;
 
typedef Eigen::Matrix<double, 2, 1> Vector2;
 
typedef Eigen::Vector3d Vector3;
 
typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> Vector3Unaligned;
 
typedef Eigen::Vector4d Vector4;
 
typedef Eigen::Matrix<double, 5, 1> Vector5;
 
typedef Eigen::Matrix<double, 6, 1> Vector6;
 
typedef Eigen::MatrixXd Matrix;
 
typedef Eigen::Matrix<double, 1, 1> Matrix1;
 
typedef Eigen::Matrix2d Matrix2;
 
typedef Eigen::Matrix3d Matrix3;
 
typedef Eigen::Matrix<double, 3, 3, Eigen::DontAlign> Matrix3Unaligned;
 
typedef Eigen::Matrix4d Matrix4;
 
typedef Eigen::Matrix<double, 5, 5> Matrix5;
 
typedef Eigen::Matrix<double, 6, 6> Matrix6;
 
typedef Eigen::Matrix<double, 12, 12> Matrix12;
 
typedef Eigen::Quaterniond Quaternion;
 
typedef Eigen::Quaternion<double, Eigen::DontAlign> QuaternionUnaligned;
 
typedef Eigen::AngleAxis<double> AngleAxis;
 
typedef Eigen::Rotation2D<double> Rotation2D;
 
typedef Eigen::Index Index;
typedef long int TimeIndex;
typedef Index TimeSize;
 
#ifndef NDEBUG
static const bool isDebug = true;
#else
static const bool isDebug = false;
#endif // NDEBUG
 
template<int compileTimeRows = -1, int compileTimeCols = -1>
class FixOrDynMatrixToolsBySize
{
public:
  template<typename CustomNullaryOp>
  static const Eigen::CwiseNullaryOp<CustomNullaryOp, typename Eigen::Matrix<double, compileTimeRows, compileTimeCols>>
      nullaryExp(const CustomNullaryOp & func, Index rows = compileTimeRows, Index cols = compileTimeCols)
  {
    (void)rows;
    (void)cols;
 
    return Eigen::Matrix<double, compileTimeRows, compileTimeCols>::NullaryExpr(func);
  }
};
 
template<int compileTimeCols>
class FixOrDynMatrixToolsBySize<-1, compileTimeCols>
{
public:
  template<typename CustomNullaryOp>
  static const Eigen::CwiseNullaryOp<CustomNullaryOp, typename Eigen::Matrix<double, -1, compileTimeCols>> nullaryExp(
      const CustomNullaryOp & func,
      Index rows = -1,
      Index cols = compileTimeCols)
  {
    return Eigen::Matrix<double, -1, compileTimeCols>::NullaryExpr(rows, cols, func);
  }
};
 
template<int compileTimeRows>
class FixOrDynMatrixToolsBySize<compileTimeRows, -1>
{
public:
  template<typename CustomNullaryOp>
  static const Eigen::CwiseNullaryOp<CustomNullaryOp, typename Eigen::Matrix<double, compileTimeRows, -1>> nullaryExp(
      const CustomNullaryOp & func,
      Index rows = compileTimeRows,
      Index cols = -1)
  {
    return Eigen::Matrix<double, compileTimeRows, -1>::NullaryExpr(rows, cols, func);
  }
};
 
template<>
class FixOrDynMatrixToolsBySize<-1, -1>
{
public:
  template<typename CustomNullaryOp>
  static const Eigen::CwiseNullaryOp<CustomNullaryOp, typename Eigen::Matrix<double, -1, -1>> nullaryExp(
      const CustomNullaryOp & func,
      Index rows = -1,
      Index cols = -1)
  {
    return Eigen::Matrix<double, -1, -1>::NullaryExpr(rows, cols, func);
  }
};
 
template<typename MatrixT>
class FixOrDynMatrixTools : public FixOrDynMatrixToolsBySize<MatrixType<MatrixT>::type::RowsAtCompileTime,
                                                             MatrixType<MatrixT>::type::ColsAtCompileTime>
{
};
 
template<typename T, const T defaultValue = T()>
class DebugItemDefaultValue
{
public:
  static const T v;
};
 
template<typename T, const T defaultValue>
const T DebugItemDefaultValue<T, defaultValue>::v = defaultValue;
 
namespace detail
{
typedef DebugItemDefaultValue<bool, true> defaultTrue;
 
enum errorType
{
  message,
  exception,
  exceptionAddr
};
 
template<errorType i = message, int dummy = 0>
class DebugItemDefaultError
{
};
 
template<int dummy>
class DebugItemDefaultError<message, dummy>
{
public:
  static const char * v;
};
 
template<int dummy>
class DebugItemDefaultError<exception, dummy>
{
public:
  static const std::runtime_error v;
};
 
template<int dummy>
class DebugItemDefaultError<exceptionAddr, dummy>
{
public:
  static const std::runtime_error * v;
};
 
template<int dummy>
const char * DebugItemDefaultError<message, dummy>::v = "The Object is not initialized. \
       If this happened during initialization then run command chckitm_set() \
       to switch it to set. And if the initialization is incomplete, run \
       chckitm_reset() afterwards.";
 
template<int dummy>
const std::runtime_error DebugItemDefaultError<exception, dummy>::v =
    std::runtime_error(DebugItemDefaultError<message>::v);
 
template<int dummy>
const std::runtime_error * DebugItemDefaultError<exceptionAddr, dummy>::v = &DebugItemDefaultError<exception>::v;
 
typedef DebugItemDefaultError<message> defaultErrorMSG;
typedef DebugItemDefaultError<exception> defaultException;
typedef DebugItemDefaultError<exceptionAddr> defaultExceptionAddr;
 
void STATE_OBSERVATION_DLLAPI defaultSum(const Vector & stateVector, const Vector & tangentVector, Vector & sum);
void STATE_OBSERVATION_DLLAPI defaultDifference(const Vector & stateVector1,
                                                const Vector & stateVector2,
                                                Vector & difference);
 
} // namespace detail
 
template<typename T, typename defaultValue = DebugItemDefaultValue<T>, bool debug = true>
class DebugItem
{
public:
  DebugItem() : b_(defaultValue::v) {}
  explicit DebugItem(const T & v) : b_(v) {}
  inline T & operator=(T v)
  {
    return b_ = v;
  }
  inline operator T() const
  {
    return b_;
  }
  inline T set(const T & v)
  {
    return b_ = v;
  }
  T get() const
  {
    return b_;
  }
 
private:
  T b_;
};
 
template<typename T, typename defaultValue>
class DebugItem<T, defaultValue, false>
{
public:
  DebugItem() {}
  explicit DebugItem(T) {}
  inline T & operator=(T v)
  {
    return v;
  }
  inline operator T() const
  {
    return defaultValue::v;
  }
  inline T set(const T &)
  {
    return defaultValue::v;
  }
  T get() const
  {
    return defaultValue::v;
  }
 
private:
  T b_;
};
 
struct EmptyChecker
{
  template<typename T>
  static bool check(const T &)
  {
    return true;
  }
  template<typename T>
  static bool checkAssert(const T &)
  {
    return true;
  }
  inline static constexpr char errorMessage[] = "";
  using ExceptionT = std::runtime_error;
};
 
template<typename T,
         bool lazy = false,
         bool alwaysCheck = false,
         bool assertion = true,
         bool eigenAlignedNew = false,
         typename additionalChecker = EmptyChecker>
class CheckedItem
{
public:
  CheckedItem(bool initialize = true);
  explicit CheckedItem(const T &);
  CheckedItem(const CheckedItem &);
  virtual ~CheckedItem() {}
 
  inline CheckedItem & operator=(const CheckedItem &);
  inline T & operator=(const T &);
  inline operator T() const;
  inline operator const T &() const;
 
  inline const T & chckitm_getValue() const;
 
  inline T & operator()();
  inline const T & operator()() const;
 
  inline T & getRefUnchecked();
  inline const T & getRefUnchecked() const;
 
  inline bool isSet() const;
  inline void reset();
 
  inline void set(bool value);
 
  void setAssertMessage(std::string s);
  void setExceptionPtr(std::exception * e);
 
  inline T & set();
 
protected:
  static const bool do_check_ = !lazy || isDebug;
  static const bool do_assert_ = do_check_ && assertion;
  static const bool do_exception_ = do_check_ && !assertion;
 
  typedef DebugItem<bool, detail::defaultTrue, do_check_> IsSet;
  typedef DebugItem<const char *, detail::defaultErrorMSG, do_assert_> AssertMsg;
  typedef DebugItem<const std::exception *, detail::defaultExceptionAddr, do_exception_> ExceptionPtr;
 
  IsSet isSet_;
  AssertMsg assertMsg_;
  ExceptionPtr exceptionPtr_;
 
  bool chckitm_check_() const; 
  T v_;
 
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(eigenAlignedNew)
};
 
struct CheckNaN
{
  template<typename T>
  static bool check(const T & m)
  {
    return !m.hasNaN();
  }
  template<typename T>
  static bool checkAssert(const T & m)
  {
    BOOST_ASSERT(check(m) && errorMessage);
    return check(m);
  }
  inline static constexpr char errorMessage[] = "Matrix contains a NaN.";
  using ExceptionT = std::runtime_error;
};
 
typedef CheckedItem<Matrix3, false, false, true, true, CheckNaN> CheckedMatrix3;
typedef CheckedItem<Matrix6, false, false, true, true, CheckNaN> CheckedMatrix6;
typedef CheckedItem<Matrix12, false, false, true, true, CheckNaN> CheckedMatrix12;
typedef CheckedItem<Vector3, false, false, true, true, CheckNaN> CheckedVector3;
typedef CheckedItem<Vector6, false, false, true, true, CheckNaN> CheckedVector6;
typedef CheckedItem<Quaternion, false, false, true, true> CheckedQuaternion;
 
template<typename MatrixType = Matrix, bool lazy = false>
class IndexedMatrixT : protected DebugItem<bool, detail::defaultTrue, !lazy || isDebug>
{
  typedef DebugItem<bool, detail::defaultTrue, !lazy || isDebug> IsSet;
 
public:
  IndexedMatrixT();
 
  IndexedMatrixT(const MatrixType & v, TimeIndex k);
 
  inline IndexedMatrixT & set(const MatrixType & v, TimeIndex k);
 
  inline void set(bool value = true);
 
  inline void setIndex(TimeIndex index);
 
  inline const MatrixType & operator()() const;
 
  inline MatrixType & operator()();
 
  inline TimeIndex getTime() const;
 
  inline bool isSet() const;
 
  inline void reset();
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
protected:
  inline bool check_() const;
  TimeIndex k_;
  MatrixType v_;
};
 
typedef IndexedMatrixT<Matrix> IndexedMatrix;
typedef IndexedMatrixT<Vector> IndexedVector;
typedef IndexedMatrixT<Vector3> IndexedVector3;
typedef IndexedMatrixT<Matrix3> IndexedMatrix3;
 
template<typename MatrixType = Matrix, typename Allocator = std::allocator<MatrixType>>
class IndexedMatrixArrayT
{
public:
  IndexedMatrixArrayT();
 
  IndexedMatrixArrayT(TimeSize size, TimeIndex initTime = 0);
 
  typedef std::vector<MatrixType, Allocator> Array;
 
  inline void setValue(const MatrixType & v, TimeIndex k);
 
  inline void pushBack(const MatrixType & v);
 
  inline void popFront();
 
  inline MatrixType operator[](TimeIndex index) const;
 
  inline MatrixType & operator[](TimeIndex index);
 
  inline const MatrixType & front() const;
 
  inline MatrixType & front();
 
  inline const MatrixType & back() const;
 
  inline MatrixType & back();
 
  void truncateAfter(TimeIndex timeIndex);
 
  void truncateBefore(TimeIndex timeIndex);
 
  inline void resize(TimeSize i, const MatrixType & m = MatrixType());
 
  inline TimeIndex getLastIndex() const;
 
  inline TimeIndex getNextIndex() const;
 
  inline TimeIndex setLastIndex(int index);
 
  inline TimeIndex getFirstIndex() const;
 
  inline TimeIndex setFirstIndex(int index);
 
  inline TimeSize size() const;
 
  inline void reset();
 
  inline void clear();
 
  Array getArray() const;
 
  inline bool checkIndex(TimeIndex k) const;
 
  void readFromFile(const char * filename, Index rows, Index cols = 1, bool withTimeStamp = true);
  void readFromFile(const std::string & filename, Index rows, Index cols = 1, bool withTimeStamp = true);
 
  void readVectorsFromFile(const std::string & filename, bool withTimeStamp = true);
  void readVectorsFromFile(const char * filename, bool withTimeStamp = true);
 
  void writeInFile(const char * filename, bool clear = false, bool append = false);
 
  void writeInFile(const std::string & filename, bool clear = false, bool append = false);
 
protected:
  typedef std::deque<MatrixType, Allocator> Deque;
 
  inline void check_(TimeIndex time) const;
 
  inline void check_() const;
 
  inline void checkNext_(TimeIndex time) const;
 
  TimeIndex k_;
 
  Deque v_;
};
 
typedef IndexedMatrixArrayT<Matrix> IndexedMatrixArray;
typedef IndexedMatrixArrayT<Vector> IndexedVectorArray;
 
namespace cst
{
constexpr double gravityConstant = 9.80665;
 
const Vector gravity = gravityConstant * Vector3::UnitZ();
 
constexpr double epsilonAngle = 1e-16;
 
constexpr double epsilon1 = std::numeric_limits<double>::epsilon();
 
} // namespace cst
 
inline bool isApprox(double a, double b, double relativePrecision = cst::epsilon1);
 
inline bool isApproxAbs(double a, double b, double absolutePrecision = cst::epsilon1);
 
typedef boost::timer::auto_cpu_timer auto_cpu_timer;
typedef boost::timer::cpu_timer cpu_timer;
typedef boost::timer::cpu_timer cpu_times;
 
namespace tools
{
struct STATE_OBSERVATION_DLLAPI SimplestStopwatch
{
  using clock = typename std::conditional<std::chrono::high_resolution_clock::is_steady,
                                          std::chrono::high_resolution_clock,
                                          std::chrono::steady_clock>::type;
  using time_ns = clock::time_point;
  time_ns startTime;
 
  inline void start()
  {
    startTime = clock::now();
  }
 
  inline double stop()
  {
    auto elapsed = clock::now() - startTime;
    return static_cast<double>(elapsed.count());
  }
};
 
std::string STATE_OBSERVATION_DLLAPI matrixToString(const Matrix & mat);
 
std::string STATE_OBSERVATION_DLLAPI vectorToString(const Vector & v);
 
Matrix STATE_OBSERVATION_DLLAPI stringToMatrix(const std::string & str, Index rows, Index cols);
 
Vector STATE_OBSERVATION_DLLAPI stringToVector(const std::string & str, Index length);
 
Vector STATE_OBSERVATION_DLLAPI stringToVector(const std::string & str);
} // namespace tools
 
#include <state-observation/tools/definitions.hxx>
} // namespace stateObservation
 
#endif // STATEOBSERVATIONDEFINITIONSHPP