8 #ifndef __SOT_MATRIX_GEOMETRY_H__ 9 #define __SOT_MATRIX_GEOMETRY_H__ 13 #include <Eigen/Geometry> 14 #include <dynamic-graph/eigen-io.h> 15 #include <dynamic-graph/linear-algebra.h> 18 #define MRAWDATA(x) x.data() 27 #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \ 29 typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \ 31 typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \ 33 typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix; 35 #define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \ 37 typedef Eigen::Matrix<Type, Size, Eigen::Dynamic> \ 38 Matrix##Size##X##TypeSuffix; \ 40 typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix; 42 #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \ 43 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \ 44 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \ 45 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \ 46 EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \ 47 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \ 48 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \ 49 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \ 50 EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7) 58 #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES 59 #undef EIGEN_MAKE_TYPEDEFS 61 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:74
const Eigen::Ref< const Eigen::VectorXd > & ConstRefVector
Definition: matrix-geometry.hh:69
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixRXd
Definition: matrix-geometry.hh:62
Eigen::Quaternion< double > SOT_CORE_EXPORT VectorQuaternion
Definition: matrix-geometry.hh:77
const Eigen::Map< const Eigen::VectorXd > const_SigVectorXd
Definition: matrix-geometry.hh:66
Eigen::Map< Eigen::VectorXd > SigVectorXd
Definition: matrix-geometry.hh:64
#define SOT_CORE_EXPORT
Definition: api.hh:20
void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT)
Definition: matrix-geometry.hh:87
const Eigen::Ref< const Eigen::MatrixXd > ConstRefMatrix
Definition: matrix-geometry.hh:71
Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw
Definition: matrix-geometry.hh:79
Eigen::Map< MatrixRXd > SigMatrixXd
Definition: matrix-geometry.hh:63
Eigen::AngleAxis< double > SOT_CORE_EXPORT VectorUTheta
Definition: matrix-geometry.hh:76
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixTwist
Definition: matrix-geometry.hh:81
Eigen::Vector3d SOT_CORE_EXPORT VectorRotation
Definition: matrix-geometry.hh:78
Eigen::Ref< Eigen::MatrixXd > RefMatrix
Definition: matrix-geometry.hh:70
Eigen::Quaternion< double > SOT_CORE_EXPORT Quaternion
Definition: matrix-geometry.hh:84
Eigen::Matrix< double, 3, 3 > SOT_CORE_EXPORT MatrixRotation
Definition: matrix-geometry.hh:75
Eigen::Matrix< double, 7, 1 > SOT_CORE_EXPORT Vector7
Definition: matrix-geometry.hh:83
Eigen::Ref< Eigen::VectorXd > RefVector
Definition: matrix-geometry.hh:68
const Eigen::Map< const MatrixRXd > const_SigMatrixXd
Definition: matrix-geometry.hh:65
Eigen::Map< Quaternion > SOT_CORE_EXPORT QuaternionMap
Definition: matrix-geometry.hh:85
Eigen::Matrix< double, 6, 6 > SOT_CORE_EXPORT MatrixForce
Definition: matrix-geometry.hh:80
#define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix)
Definition: matrix-geometry.hh:42
Definition: abstract-sot-external-interface.hh:17