Directory: | ./ |
---|---|
File: | include/sot/core/matrix-geometry.hh |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 12 | 12 | 100.0% |
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 |
2/4✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
|
114 | Eigen::Vector3d _t = MH.translation(); |
90 |
2/4✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
|
114 | MatrixRotation R(MH.linear()); |
91 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
114 | Eigen::Matrix3d Tx; |
92 |
15/30✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 114 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 114 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 114 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 114 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 114 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 114 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 114 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 114 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 114 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 114 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 114 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 114 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 114 times.
✗ Branch 44 not taken.
|
114 | Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0; |
93 |
1/2✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
|
114 | Eigen::Matrix3d sk; |
94 |
2/4✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
|
114 | sk = Tx * R; |
95 | |||
96 |
2/4✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
|
114 | MT.block<3, 3>(0, 0) = R; |
97 |
2/4✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
|
114 | MT.block<3, 3>(0, 3) = sk; |
98 |
2/4✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
|
114 | MT.block<3, 3>(3, 0).setZero(); |
99 |
2/4✓ Branch 1 taken 114 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 114 times.
✗ Branch 5 not taken.
|
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__ */ | ||
106 |