Skip to content

Commit 71b88a0

Browse files
artivisartivis
committed
add SGal3
Signed-off-by: artivis <[email protected]>
1 parent 8907527 commit 71b88a0

21 files changed

+2274
-18
lines changed

‎README.md

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ At the moment, it provides the groups:
2626
introduced (to the best of knowledge) in this [paper][barrau15].
2727
NOTE: The implementation here differs slightly from
2828
the developments in the [paper][barrau15].
29+
- SGal(3): The Special Galilean group (rotation, translation, velocity and time) in 3D space, described in these papers [[1][fourmy19]] & [[2][kelly24]].
2930
- Bundle<>: allows manipulating a manifold bundle as a single Lie group.
3031
Referred to as a *composite manifold* in Section IV of the
3132
[reference paper](http://arxiv.org/abs/1812.01537).
@@ -170,6 +171,8 @@ Want to contribute? Great! Check out our [contribution guidelines](CONTRIBUTING.
170171
[jsola18]: http://arxiv.org/abs/1812.01537
171172
[jsola18v]: http://arxiv.org/abs/1812.01537v4
172173
[barrau15]: https://arxiv.org/pdf/1410.1465.pdf
174+
[fourmy19]: https://hal.science/hal-02183498/document
175+
[kelly24]: https://arxiv.org/abs/2312.07555
173176
[deray20]: https://joss.theoj.org/papers/10.21105/joss.01371
174177

175178
[jsola-iri-lecture]: https://www.youtube.com/watch?v=nHOcoIyJj2o

‎include/manif/SGal3.h

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
#ifndef _MANIF_SGAL3_H_
2+
#define _MANIF_SGAL3_H_
3+
4+
#include "manif/impl/macro.h"
5+
#include "manif/impl/lie_group_base.h"
6+
#include "manif/impl/tangent_base.h"
7+
8+
#include "manif/impl/sgal3/SGal3_properties.h"
9+
#include "manif/impl/sgal3/SGal3_base.h"
10+
#include "manif/impl/sgal3/SGal3Tangent_base.h"
11+
#include "manif/impl/sgal3/SGal3.h"
12+
#include "manif/impl/sgal3/SGal3Tangent.h"
13+
#include "manif/impl/sgal3/SGal3_map.h"
14+
#include "manif/impl/sgal3/SGal3Tangent_map.h"
15+
16+
#endif // _MANIF_SGAL3_H_

‎include/manif/impl/sgal3/SGal3.h

Lines changed: 254 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,254 @@
1+
#ifndef _MANIF_MANIF_SGAL3_H_
2+
#define _MANIF_MANIF_SGAL3_H_
3+
4+
#include "manif/impl/sgal3/SGal3_base.h"
5+
6+
namespace manif {
7+
8+
// Forward declare for type traits specialization
9+
template <typename _Scalar> struct SGal3;
10+
template <typename _Scalar> struct SGal3Tangent;
11+
12+
namespace internal {
13+
14+
//! Traits specialization
15+
template <typename _Scalar>
16+
struct traits<SGal3<_Scalar>> {
17+
using Scalar = _Scalar;
18+
19+
using LieGroup = SGal3<_Scalar>;
20+
using Tangent = SGal3Tangent<_Scalar>;
21+
22+
using Base = SGal3Base<SGal3<_Scalar>>;
23+
24+
static constexpr int Dim = LieGroupProperties<Base>::Dim;
25+
static constexpr int DoF = LieGroupProperties<Base>::DoF;
26+
static constexpr int RepSize = 11;
27+
28+
/// @todo would be nice to concat vec3 + quaternion + vec3 + t
29+
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
30+
31+
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
32+
using Rotation = Eigen::Matrix<Scalar, 3, 3>;
33+
using Translation = Eigen::Matrix<Scalar, 3, 1>;
34+
using LinearVelocity = Eigen::Matrix<Scalar, 3, 1>;
35+
using Vector = Eigen::Matrix<Scalar, 3, 1>;
36+
};
37+
38+
} // namespace internal
39+
} // namespace manif
40+
41+
namespace manif {
42+
43+
//
44+
// LieGroup
45+
//
46+
47+
/**
48+
* @brief Represent an element of SGal3.
49+
*/
50+
template <typename _Scalar>
51+
struct SGal3 : SGal3Base<SGal3<_Scalar>> {
52+
private:
53+
54+
using Base = SGal3Base<SGal3<_Scalar>>;
55+
using Type = SGal3<_Scalar>;
56+
57+
protected:
58+
59+
using Base::derived;
60+
61+
public:
62+
63+
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
64+
65+
MANIF_COMPLETE_GROUP_TYPEDEF
66+
using Translation = typename Base::Translation;
67+
using Quaternion = Eigen::Quaternion<Scalar>;
68+
using LinearVelocity = typename Base::LinearVelocity;
69+
70+
MANIF_INHERIT_GROUP_API
71+
using Base::rotation;
72+
using Base::normalize;
73+
74+
SGal3() = default;
75+
~SGal3() = default;
76+
77+
MANIF_COPY_CONSTRUCTOR(SGal3)
78+
MANIF_MOVE_CONSTRUCTOR(SGal3)
79+
80+
template <typename _DerivedOther>
81+
SGal3(const LieGroupBase<_DerivedOther>& o);
82+
83+
MANIF_GROUP_ASSIGN_OP(SGal3)
84+
85+
/**
86+
* @brief Constructor given a translation, a unit quaternion and a linear velocity.
87+
* @param[in] t A translation vector.
88+
* @param[in] q A unit quaternion.
89+
* @param[in] v A linear velocity vector.
90+
* @param[in] time A time.
91+
* @throws manif::invalid_argument on un-normalized complex number.
92+
*/
93+
SGal3(
94+
const Translation& t,
95+
const Eigen::Quaternion<Scalar>& q,
96+
const LinearVelocity& v,
97+
const Scalar time
98+
);
99+
100+
/**
101+
* @brief Constructor given a translation, an angle axis and a linear velocity.
102+
* @param[in] t A translation vector.
103+
* @param[in] angle_axis An angle-axis.
104+
* @param[in] v A linear velocity vector.
105+
* @param[in] time A time.
106+
*/
107+
SGal3(
108+
const Translation& t,
109+
const Eigen::AngleAxis<Scalar>& angle_axis,
110+
const LinearVelocity& v,
111+
const Scalar time
112+
);
113+
114+
/**
115+
* @brief Constructor given a translation, SO3 element and a linear velocity.
116+
* @param[in] t A translation vector.
117+
* @param[in] SO3 An element of SO3.
118+
* @param[in] v A linear velocity vector.
119+
* @param[in] time A time.
120+
*/
121+
SGal3(
122+
const Translation& t,
123+
const SO3<Scalar>& SO3,
124+
const LinearVelocity& v,
125+
const Scalar time
126+
);
127+
128+
/**
129+
* @brief Constructor given translation components,
130+
* roll-pitch-yaw angles and linear velocity components
131+
* @param[in] x The x component of the translation.
132+
* @param[in] y The y component of the translation.
133+
* @param[in] z The z component of the translation.
134+
* @param[in] roll The roll angle.
135+
* @param[in] pitch The pitch angle.
136+
* @param[in] yaw The yaw angle.
137+
* @param[in] vx The x component of the linear velocity.
138+
* @param[in] vy The y component of the linear velocity.
139+
* @param[in] vz The z component of the linear velocity.
140+
* @param[in] t time.
141+
*/
142+
SGal3(
143+
const Scalar x, const Scalar y, const Scalar z,
144+
const Scalar roll, const Scalar pitch, const Scalar yaw,
145+
const Scalar vx, const Scalar vy, const Scalar vz,
146+
const Scalar t
147+
);
148+
149+
/**
150+
* @brief Constructor from a 3D Eigen::Isometry<Scalar> relevant to SE(3) and a linear velocity
151+
* @param[in] h a isometry object from Eigen defined for SE(3)
152+
* @param[in] v a linear velocity vector.
153+
* @note overall, this should be a double direct spatial isometry,
154+
*/
155+
SGal3(
156+
const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h,
157+
const LinearVelocity& v,
158+
const Scalar t
159+
);
160+
161+
// LieGroup common API
162+
163+
DataType& coeffs();
164+
const DataType& coeffs() const;
165+
166+
// SGal3 specific API
167+
168+
protected:
169+
170+
DataType data_;
171+
};
172+
173+
MANIF_EXTRA_GROUP_TYPEDEF(SGal3)
174+
175+
template <typename _Scalar>
176+
template <typename _DerivedOther>
177+
SGal3<_Scalar>::SGal3(const LieGroupBase<_DerivedOther>& o) : SGal3(o.coeffs()) {
178+
//
179+
}
180+
181+
template <typename _Scalar>
182+
SGal3<_Scalar>::SGal3(
183+
const Translation& t,
184+
const Eigen::Quaternion<Scalar>& q,
185+
const LinearVelocity& v,
186+
const Scalar time
187+
) : SGal3((DataType() << t, q.coeffs(), v, time).finished()) {
188+
//
189+
}
190+
191+
template <typename _Scalar>
192+
SGal3<_Scalar>::SGal3(
193+
const Translation& t,
194+
const Eigen::AngleAxis<Scalar>& a,
195+
const LinearVelocity& v,
196+
const Scalar time
197+
) : SGal3(t, Quaternion(a), v, time) {
198+
//
199+
}
200+
201+
template <typename _Scalar>
202+
SGal3<_Scalar>::SGal3(
203+
const Scalar x, const Scalar y, const Scalar z,
204+
const Scalar roll, const Scalar pitch, const Scalar yaw,
205+
const Scalar vx, const Scalar vy, const Scalar vz,
206+
const Scalar t
207+
) : SGal3(
208+
Translation(x,y,z),
209+
Eigen::Quaternion<Scalar>(
210+
Eigen::AngleAxis<Scalar>(yaw, Eigen::Matrix<Scalar, 3, 1>::UnitZ()) *
211+
Eigen::AngleAxis<Scalar>(pitch, Eigen::Matrix<Scalar, 3, 1>::UnitY()) *
212+
Eigen::AngleAxis<Scalar>(roll, Eigen::Matrix<Scalar, 3, 1>::UnitX())
213+
),
214+
LinearVelocity(vx, vy, vz),
215+
t
216+
) {
217+
//
218+
}
219+
220+
template <typename _Scalar>
221+
SGal3<_Scalar>::SGal3(
222+
const Translation& t,
223+
const SO3<Scalar>& so3,
224+
const LinearVelocity& v,
225+
const Scalar time
226+
) : SGal3(t, so3.quat(), v, time) {
227+
//
228+
}
229+
230+
template <typename _Scalar>
231+
SGal3<_Scalar>::SGal3(
232+
const Eigen::Transform<_Scalar, 3, Eigen::Isometry>& h,
233+
const LinearVelocity& v,
234+
const Scalar t
235+
) : SGal3(h.translation(), Eigen::Quaternion<_Scalar>(h.rotation()), v, t) {
236+
//
237+
}
238+
239+
240+
template <typename _Scalar>
241+
typename SGal3<_Scalar>::DataType&
242+
SGal3<_Scalar>::coeffs() {
243+
return data_;
244+
}
245+
246+
template <typename _Scalar>
247+
const typename SGal3<_Scalar>::DataType&
248+
SGal3<_Scalar>::coeffs() const {
249+
return data_;
250+
}
251+
252+
} // namespace manif
253+
254+
#endif // _MANIF_MANIF_SGAL3_H_

‎include/manif/impl/sgal3/SGal3Tangent.h

Lines changed: 106 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,106 @@
1+
#ifndef _MANIF_MANIF_SGAL3TANGENT_H_
2+
#define _MANIF_MANIF_SGAL3TANGENT_H_
3+
4+
#include "manif/impl/sgal3/SGal3Tangent_base.h"
5+
6+
namespace manif {
7+
namespace internal {
8+
9+
//! Traits specialization
10+
template <typename _Scalar>
11+
struct traits<SGal3Tangent<_Scalar>> {
12+
using Scalar = _Scalar;
13+
14+
using LieGroup = SGal3<_Scalar>;
15+
using Tangent = SGal3Tangent<_Scalar>;
16+
17+
using Base = SGal3TangentBase<Tangent>;
18+
19+
static constexpr int Dim = LieGroupProperties<Base>::Dim;
20+
static constexpr int DoF = LieGroupProperties<Base>::DoF;
21+
static constexpr int RepSize = DoF;
22+
23+
using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
24+
25+
using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
26+
using LieAlg = Eigen::Matrix<Scalar, 5, 5>;
27+
};
28+
29+
} // namespace internal
30+
} // namespace manif
31+
32+
namespace manif {
33+
34+
//
35+
// Tangent
36+
//
37+
38+
/**
39+
* @brief Represents an element of tangent space of SGal3.
40+
*/
41+
template <typename _Scalar>
42+
struct SGal3Tangent : SGal3TangentBase<SGal3Tangent<_Scalar>> {
43+
private:
44+
45+
using Base = SGal3TangentBase<SGal3Tangent<_Scalar>>;
46+
using Type = SGal3Tangent<_Scalar>;
47+
48+
protected:
49+
50+
using Base::derived;
51+
52+
public:
53+
54+
MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
55+
56+
MANIF_TANGENT_TYPEDEF
57+
MANIF_INHERIT_TANGENT_API
58+
MANIF_INHERIT_TANGENT_OPERATOR
59+
60+
SGal3Tangent() = default;
61+
~SGal3Tangent() = default;
62+
63+
MANIF_COPY_CONSTRUCTOR(SGal3Tangent)
64+
MANIF_MOVE_CONSTRUCTOR(SGal3Tangent)
65+
66+
template <typename _DerivedOther>
67+
SGal3Tangent(const TangentBase<_DerivedOther>& o);
68+
69+
MANIF_TANGENT_ASSIGN_OP(SGal3Tangent)
70+
71+
// Tangent common API
72+
73+
DataType& coeffs();
74+
const DataType& coeffs() const;
75+
76+
// SGal3Tangent specific API
77+
78+
protected:
79+
80+
DataType data_;
81+
};
82+
83+
MANIF_EXTRA_TANGENT_TYPEDEF(SGal3Tangent);
84+
85+
template <typename _Scalar>
86+
template <typename _DerivedOther>
87+
SGal3Tangent<_Scalar>::SGal3Tangent(const TangentBase<_DerivedOther>& o)
88+
: data_(o.coeffs()) {
89+
//
90+
}
91+
92+
template <typename _Scalar>
93+
typename SGal3Tangent<_Scalar>::DataType&
94+
SGal3Tangent<_Scalar>::coeffs() {
95+
return data_;
96+
}
97+
98+
template <typename _Scalar>
99+
const typename SGal3Tangent<_Scalar>::DataType&
100+
SGal3Tangent<_Scalar>::coeffs() const {
101+
return data_;
102+
}
103+
104+
} // namespace manif
105+
106+
#endif // _MANIF_MANIF_SGAL3TANGENT_H_

0 commit comments

Comments
 (0)