8 #ifndef __SOT_MATRIX_GEOMETRY_H__ 9 #define __SOT_MATRIX_GEOMETRY_H__ 12 #include <dynamic-graph/eigen-io.h> 13 #include <dynamic-graph/linear-algebra.h> 16 #include <Eigen/Geometry> 19 #define MRAWDATA(x) x.data() 28 #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \ 30 typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \ 32 typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \ 34 typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix; 36 #define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \ 38 typedef Eigen::Matrix<Type, Size, Eigen::Dynamic> \ 39 Matrix##Size##X##TypeSuffix; \ 41 typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix; 43 #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \ 44 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \ 45 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \ 46 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \ 47 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \ 48 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \ 49 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \ 50 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \ 51 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7) 59 #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES 60 #undef EIGEN_MAKE_TYPEDEFS 62 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
89 Eigen::Vector3d _t = MH.translation();
90 MatrixRotation R(MH.linear());
92 Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
96 MT.block<3, 3>(0, 0) = R;
97 MT.block<3, 3>(0, 3) = sk;
98 MT.block<3, 3>(3, 0).setZero();
99 MT.block<3, 3>(3, 3) = R;
Eigen::Transform< double, 3, Eigen::Affine > SOT_CORE_EXPORT MatrixHomogeneous
Definition: matrix-geometry.hh:75
const Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:70
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixRXd
Definition: matrix-geometry.hh:63
Eigen::Quaternion< double > SOT_CORE_EXPORT VectorQuaternion
Definition: matrix-geometry.hh:78
const Eigen::Map< const Eigen::VectorXd > const_SigVectorXd
Definition: matrix-geometry.hh:67
Eigen::Map< Eigen::VectorXd > SigVectorXd
Definition: matrix-geometry.hh:65
#define SOT_CORE_EXPORT
Definition: api.hh:20
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
Definition: matrix-geometry.hh:88
const Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:72
Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw
Definition: matrix-geometry.hh:80
Eigen::Map< MatrixRXd > SigMatrixXd
Definition: matrix-geometry.hh:64
Eigen::AngleAxis< double > SOT_CORE_EXPORT VectorUTheta
Definition: matrix-geometry.hh:77
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
Definition: matrix-geometry.hh:82
Eigen::Vector3d SOT_CORE_EXPORT VectorRotation
Definition: matrix-geometry.hh:79
Eigen::Ref< Eigen::MatrixXd > RefMatrix
Definition: matrix-geometry.hh:71
Eigen::Quaternion< double > SOT_CORE_EXPORT Quaternion
Definition: matrix-geometry.hh:85
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:76
Eigen::Matrix< double, 7, 1 > SOT_CORE_EXPORT Vector7
Definition: matrix-geometry.hh:84
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:69
const Eigen::Map< const MatrixRXd > const_SigMatrixXd
Definition: matrix-geometry.hh:66
Eigen::Map< Quaternion > SOT_CORE_EXPORT QuaternionMap
Definition: matrix-geometry.hh:86
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixForce
Definition: matrix-geometry.hh:81
#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix)
Definition: matrix-geometry.hh:43
Definition: abstract-sot-external-interface.hh:17