GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/core/matrix-geometry.hh Lines: 12 12 100.0 %
Date: 2023-03-13 12:09:37 Branches: 31 62 50.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,2019
3
 *  CNRS/AIST
4
 * François Bleibel, Olivier Stasse, François Bailly
5
 *
6
 */
7
8
#ifndef __SOT_MATRIX_GEOMETRY_H__
9
#define __SOT_MATRIX_GEOMETRY_H__
10
11
/* --- Matrix --- */
12
#include <dynamic-graph/eigen-io.h>
13
#include <dynamic-graph/linear-algebra.h>
14
15
#include <Eigen/Core>
16
#include <Eigen/Geometry>
17
#include <sot/core/api.hh>
18
19
#define MRAWDATA(x) x.data()
20
21
/* --------------------------------------------------------------------- */
22
/* --------------------------------------------------------------------- */
23
/* --------------------------------------------------------------------- */
24
25
namespace dynamicgraph {
26
namespace sot {
27
28
#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix)           \
29
  /** \ingroup matrixtypedefs */                                          \
30
  typedef Eigen::Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
31
  /** \ingroup matrixtypedefs */                                          \
32
  typedef Eigen::Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix;    \
33
  /** \ingroup matrixtypedefs */                                          \
34
  typedef Eigen::Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
35
36
#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
37
  /** \ingroup matrixtypedefs */                          \
38
  typedef Eigen::Matrix<Type, Size, Eigen::Dynamic>       \
39
      Matrix##Size##X##TypeSuffix;                        \
40
  /** \ingroup matrixtypedefs */                          \
41
  typedef Eigen::Matrix<Type, Eigen::Dynamic, Size> Matrix##X##Size##TypeSuffix;
42
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)
52
53
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i)
54
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f)
55
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d)
56
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<float>, cf)
57
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
58
59
#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
60
#undef EIGEN_MAKE_TYPEDEFS
61
62
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
63
    MatrixRXd;
64
typedef Eigen::Map<MatrixRXd> SigMatrixXd;
65
typedef Eigen::Map<Eigen::VectorXd> SigVectorXd;
66
typedef const Eigen::Map<const MatrixRXd> const_SigMatrixXd;
67
typedef const Eigen::Map<const Eigen::VectorXd> const_SigVectorXd;
68
69
typedef Eigen::Ref<Eigen::VectorXd> RefVector;
70
typedef const Eigen::Ref<const Eigen::VectorXd> &ConstRefVector;
71
typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
72
typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;
73
74
typedef Eigen::Transform<double, 3, Eigen::Affine> SOT_CORE_EXPORT
75
    MatrixHomogeneous;
76
typedef Eigen::Matrix<double, 3, 3> SOT_CORE_EXPORT MatrixRotation;
77
typedef Eigen::AngleAxis<double> SOT_CORE_EXPORT VectorUTheta;
78
typedef Eigen::Quaternion<double> SOT_CORE_EXPORT VectorQuaternion;
79
typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRotation;
80
typedef Eigen::Vector3d SOT_CORE_EXPORT VectorRollPitchYaw;
81
typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixForce;
82
typedef Eigen::Matrix<double, 6, 6> SOT_CORE_EXPORT MatrixTwist;
83
84
typedef Eigen::Matrix<double, 7, 1> SOT_CORE_EXPORT Vector7;
85
typedef Eigen::Quaternion<double> SOT_CORE_EXPORT Quaternion;
86
typedef Eigen::Map<Quaternion> SOT_CORE_EXPORT QuaternionMap;
87
88
114
inline void buildFrom(const MatrixHomogeneous &MH, MatrixTwist &MT) {
89

114
  Eigen::Vector3d _t = MH.translation();
90

114
  MatrixRotation R(MH.linear());
91
114
  Eigen::Matrix3d Tx;
92







114
  Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
93
114
  Eigen::Matrix3d sk;
94

114
  sk = Tx * R;
95
96

114
  MT.block<3, 3>(0, 0) = R;
97

114
  MT.block<3, 3>(0, 3) = sk;
98

114
  MT.block<3, 3>(3, 0).setZero();
99

114
  MT.block<3, 3>(3, 3) = R;
100
114
}
101
102
}  // namespace sot
103
}  // namespace dynamicgraph
104
105
#endif /* #ifndef __SOT_MATRIX_GEOMETRY_H__ */