matrix-geometry.hh

Go to the documentation of this file.

/*
 * Copyright 2010,2019
 *  CNRS/AIST
 * François Bleibel, Olivier Stasse, François Bailly
 *
 */
 
#ifndef __SOT_MATRIX_GEOMETRY_H__
#define __SOT_MATRIX_GEOMETRY_H__
 
/* --- Matrix --- */
#include <dynamic-graph/eigen-io.h>
#include <dynamic-graph/linear-algebra.h>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sot/core/api.hh>
 
#define MRAWDATA(x) x.data()
 
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
namespace dynamicgraph {
namespace sot {
 
#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)           \
                                          \
  typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
                                          \
  typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix;    \
                                          \
  typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
 
#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
                          \
  typedef Eigen::Matrix<Type, Size, Eigen::Dynamic>       \
      Matrix##Size##X##TypeSuffix;                        \
                          \
  typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix;
 
#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \
  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1)           \
  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5)           \
  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6)           \
  EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7)           \
  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1)        \
  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5)        \
  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6)        \
  EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7)
 
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
 
#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
#undef EIGEN_MAKE_TYPEDEFS
 
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
    MatrixRXd;
typedef Eigen::Map<MatrixRXd> SigMatrixXd;
typedef Eigen::Map<Eigen::VectorXd> SigVectorXd;
typedef const Eigen::Map<const MatrixRXd> const_SigMatrixXd;
typedef const Eigen::Map<const Eigen::VectorXd> const_SigVectorXd;
 
typedef Eigen::Ref<Eigen::VectorXd> RefVector;
typedef const Eigen::Ref<const Eigen::VectorXd> &ConstRefVector;
typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;
 
typedef Eigen::Transform<double, 3, Eigen::Affine> SOT_CORE_EXPORT
    MatrixHomogeneous;
typedef Eigen::Matrix<double, 3, 3> SOT_CORE_EXPORT MatrixRotation;
typedef Eigen::AngleAxis<double> SOT_CORE_EXPORT VectorUTheta;
typedef Eigen::Quaternion<double> SOT_CORE_EXPORT VectorQuaternion;
typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRotation;
typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw;
typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixForce;
typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixTwist;
 
typedef Eigen::Matrix<double, 7, 1> SOT_CORE_EXPORT Vector7;
typedef Eigen::Quaternion<double> SOT_CORE_EXPORT Quaternion;
typedef Eigen::Map<Quaternion> SOT_CORE_EXPORT QuaternionMap;
 
inline void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT) {
  Eigen::Vector3d _t = MH.translation();
  MatrixRotation R(MH.linear());
  Eigen::Matrix3d Tx;
  Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
  Eigen::Matrix3d sk;
  sk = Tx * R;
 
  MT.block<3, 3>(0, 0) = R;
  MT.block<3, 3>(0, 3) = sk;
  MT.block<3, 3>(3, 0).setZero();
  MT.block<3, 3>(3, 3) = R;
}
 
}  // namespace sot
}  // namespace dynamicgraph
 
#endif /* #ifndef __SOT_MATRIX_GEOMETRY_H__ */