add SGal3

Signed-off-by: artivis <[email protected]>

README.md

@@ -26,6 +26,7 @@ At the moment, it provides the groups:
   introduced (to the best of knowledge) in this [paper][barrau15].
   NOTE: The implementation here differs slightly from
   the developments in the [paper][barrau15].
+- SGal(3): The Special Galilean group (rotation, translation, velocity and time) in 3D space, described in these papers [[1][fourmy19]] & [[2][kelly24]].
 - Bundle<>: allows manipulating a manifold bundle as a single Lie group.
   Referred to as a *composite manifold* in Section IV of the
   [reference paper](http://arxiv.org/abs/1812.01537).
@@ -170,6 +171,8 @@ Want to contribute? Great! Check out our [contribution guidelines](CONTRIBUTING.
 [jsola18]: http://arxiv.org/abs/1812.01537
 [jsola18v]: http://arxiv.org/abs/1812.01537v4
 [barrau15]: https://arxiv.org/pdf/1410.1465.pdf
+[fourmy19]: https://hal.science/hal-02183498/document
+[kelly24]: https://arxiv.org/abs/2312.07555
 [deray20]: https://joss.theoj.org/papers/10.21105/joss.01371
 
 [jsola-iri-lecture]: https://www.youtube.com/watch?v=nHOcoIyJj2o

include/manif/SGal3.h

@@ -0,0 +1,16 @@
+#ifndef _MANIF_SGAL3_H_
+#define _MANIF_SGAL3_H_
+
+#include "manif/impl/macro.h"
+#include "manif/impl/lie_group_base.h"
+#include "manif/impl/tangent_base.h"
+
+#include "manif/impl/sgal3/SGal3_properties.h"
+#include "manif/impl/sgal3/SGal3_base.h"
+#include "manif/impl/sgal3/SGal3Tangent_base.h"
+#include "manif/impl/sgal3/SGal3.h"
+#include "manif/impl/sgal3/SGal3Tangent.h"
+#include "manif/impl/sgal3/SGal3_map.h"
+#include "manif/impl/sgal3/SGal3Tangent_map.h"
+
+#endif // _MANIF_SGAL3_H_

include/manif/impl/sgal3/SGal3.h

@@ -0,0 +1,254 @@
+#ifndef _MANIF_MANIF_SGAL3_H_
+#define _MANIF_MANIF_SGAL3_H_
+
+#include "manif/impl/sgal3/SGal3_base.h"
+
+namespace manif {
+
+// Forward declare for type traits specialization
+template <typename _Scalar> struct SGal3;
+template <typename _Scalar> struct SGal3Tangent;
+
+namespace internal {
+
+//! Traits specialization
+template <typename _Scalar>
+struct traits<SGal3<_Scalar>> {
+  using Scalar = _Scalar;
+
+  using LieGroup = SGal3<_Scalar>;
+  using Tangent  = SGal3Tangent<_Scalar>;
+
+  using Base = SGal3Base<SGal3<_Scalar>>;
+
+  static constexpr int Dim = LieGroupProperties<Base>::Dim;
+  static constexpr int DoF = LieGroupProperties<Base>::DoF;
+  static constexpr int RepSize = 11;
+
+  /// @todo would be nice to concat vec3 + quaternion + vec3 + t
+  using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
+
+  using Jacobian       = Eigen::Matrix<Scalar, DoF, DoF>;
+  using Rotation       = Eigen::Matrix<Scalar, 3, 3>;
+  using Translation    = Eigen::Matrix<Scalar, 3, 1>;
+  using LinearVelocity = Eigen::Matrix<Scalar, 3, 1>;
+  using Vector         = Eigen::Matrix<Scalar, 3, 1>;
+};
+
+} // namespace internal
+} // namespace manif
+
+namespace manif {
+
+//
+// LieGroup
+//
+
+/**
+ * @brief Represent an element of SGal3.
+ */
+template <typename _Scalar>
+struct SGal3 : SGal3Base<SGal3<_Scalar>> {
+private:
+
+  using Base = SGal3Base<SGal3<_Scalar>>;
+  using Type = SGal3<_Scalar>;
+
+protected:
+
+  using Base::derived;
+
+public:
+
+  MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
+
+  MANIF_COMPLETE_GROUP_TYPEDEF
+  using Translation = typename Base::Translation;
+  using Quaternion  = Eigen::Quaternion<Scalar>;
+  using LinearVelocity = typename Base::LinearVelocity;
+
+  MANIF_INHERIT_GROUP_API
+  using Base::rotation;
+  using Base::normalize;
+
+  SGal3()  = default;
+  ~SGal3() = default;
+
+  MANIF_COPY_CONSTRUCTOR(SGal3)
+  MANIF_MOVE_CONSTRUCTOR(SGal3)
+
+  template <typename _DerivedOther>
+  SGal3(const LieGroupBase<_DerivedOther>& o);
+
+  MANIF_GROUP_ASSIGN_OP(SGal3)
+
+  /**
+   * @brief Constructor given a translation, a unit quaternion and a linear velocity.
+   * @param[in] t A translation vector.
+   * @param[in] q A unit quaternion.
+   * @param[in] v A linear velocity vector.
+   * @param[in] time A time.
+   * @throws manif::invalid_argument on un-normalized complex number.
+   */
+  SGal3(
+    const Translation& t,
+    const Eigen::Quaternion<Scalar>& q,
+    const LinearVelocity& v,
+    const Scalar time
+  );
+
+  /**
+   * @brief Constructor given a translation, an angle axis and a linear velocity.
+   * @param[in] t A translation vector.
+   * @param[in] angle_axis An angle-axis.
+   * @param[in] v A linear velocity vector.
+   * @param[in] time A time.
+   */
+  SGal3(
+    const Translation& t,
+    const Eigen::AngleAxis<Scalar>& angle_axis,
+    const LinearVelocity& v,
+    const Scalar time
+  );
+
+  /**
+   * @brief Constructor given a translation, SO3 element and a linear velocity.
+   * @param[in] t A translation vector.
+   * @param[in] SO3 An element of SO3.
+   * @param[in] v A linear velocity vector.
+   * @param[in] time A time.
+   */
+  SGal3(
+    const Translation& t,
+    const SO3<Scalar>& SO3,
+    const LinearVelocity& v,
+    const Scalar time
+  );
+
+  /**
+   * @brief Constructor given translation components,
+   * roll-pitch-yaw angles and linear velocity components
+   * @param[in] x The x component of the translation.
+   * @param[in] y The y component of the translation.
+   * @param[in] z The z component of the translation.
+   * @param[in] roll The roll angle.
+   * @param[in] pitch The pitch angle.
+   * @param[in] yaw The yaw angle.
+   * @param[in] vx The x component of the linear velocity.
+   * @param[in] vy The y component of the linear velocity.
+   * @param[in] vz The z component of the linear velocity.
+   * @param[in] t time.
+   */
+  SGal3(
+    const Scalar x, const Scalar y, const Scalar z,
+    const Scalar roll, const Scalar pitch, const Scalar yaw,
+    const Scalar vx, const Scalar vy, const Scalar vz,
+    const Scalar t
+  );
+
+  /**
+   * @brief Constructor from a 3D Eigen::Isometry<Scalar> relevant to SE(3) and a linear velocity
+   * @param[in] h a isometry object from Eigen defined for SE(3)
+   * @param[in] v a linear velocity vector.
+   * @note overall, this should be a double direct spatial isometry,
+   */
+  SGal3(
+    const Eigen::Transform<_Scalar,3,Eigen::Isometry>& h,
+    const LinearVelocity& v,
+    const Scalar t
+  );
+
+  // LieGroup common API
+
+  DataType& coeffs();
+  const DataType& coeffs() const;
+
+  // SGal3 specific API
+
+protected:
+
+  DataType data_;
+};
+
+MANIF_EXTRA_GROUP_TYPEDEF(SGal3)
+
+template <typename _Scalar>
+template <typename _DerivedOther>
+SGal3<_Scalar>::SGal3(const LieGroupBase<_DerivedOther>& o) : SGal3(o.coeffs()) {
+  //
+}
+
+template <typename _Scalar>
+SGal3<_Scalar>::SGal3(
+  const Translation& t,
+  const Eigen::Quaternion<Scalar>& q,
+  const LinearVelocity& v,
+  const Scalar time
+) : SGal3((DataType() << t, q.coeffs(), v, time).finished()) {
+  //
+}
+
+template <typename _Scalar>
+SGal3<_Scalar>::SGal3(
+  const Translation& t,
+  const Eigen::AngleAxis<Scalar>& a,
+  const LinearVelocity& v,
+  const Scalar time
+) : SGal3(t, Quaternion(a), v, time) {
+  //
+}
+
+template <typename _Scalar>
+SGal3<_Scalar>::SGal3(
+  const Scalar x, const Scalar y, const Scalar z,
+  const Scalar roll, const Scalar pitch, const Scalar yaw,
+  const Scalar vx, const Scalar vy, const Scalar vz,
+  const Scalar t
+) : SGal3(
+  Translation(x,y,z),
+  Eigen::Quaternion<Scalar>(
+    Eigen::AngleAxis<Scalar>(yaw,   Eigen::Matrix<Scalar, 3, 1>::UnitZ()) *
+    Eigen::AngleAxis<Scalar>(pitch, Eigen::Matrix<Scalar, 3, 1>::UnitY()) *
+    Eigen::AngleAxis<Scalar>(roll,  Eigen::Matrix<Scalar, 3, 1>::UnitX())
+  ),
+  LinearVelocity(vx, vy, vz),
+  t
+) {
+  //
+}
+
+template <typename _Scalar>
+SGal3<_Scalar>::SGal3(
+  const Translation& t,
+  const SO3<Scalar>& so3,
+  const LinearVelocity& v,
+  const Scalar time
+) : SGal3(t, so3.quat(), v, time) {
+  //
+}
+
+template <typename _Scalar>
+SGal3<_Scalar>::SGal3(
+  const Eigen::Transform<_Scalar, 3, Eigen::Isometry>& h,
+  const LinearVelocity& v,
+  const Scalar t
+) : SGal3(h.translation(), Eigen::Quaternion<_Scalar>(h.rotation()), v, t) {
+  //
+}
+
+
+template <typename _Scalar>
+typename SGal3<_Scalar>::DataType&
+SGal3<_Scalar>::coeffs() {
+  return data_;
+}
+
+template <typename _Scalar>
+const typename SGal3<_Scalar>::DataType&
+SGal3<_Scalar>::coeffs() const {
+  return data_;
+}
+
+} // namespace manif
+
+#endif // _MANIF_MANIF_SGAL3_H_

include/manif/impl/sgal3/SGal3Tangent.h

@@ -0,0 +1,106 @@
+#ifndef _MANIF_MANIF_SGAL3TANGENT_H_
+#define _MANIF_MANIF_SGAL3TANGENT_H_
+
+#include "manif/impl/sgal3/SGal3Tangent_base.h"
+
+namespace manif {
+namespace internal {
+
+//! Traits specialization
+template <typename _Scalar>
+struct traits<SGal3Tangent<_Scalar>> {
+  using Scalar = _Scalar;
+
+  using LieGroup = SGal3<_Scalar>;
+  using Tangent  = SGal3Tangent<_Scalar>;
+
+  using Base = SGal3TangentBase<Tangent>;
+
+  static constexpr int Dim     = LieGroupProperties<Base>::Dim;
+  static constexpr int DoF     = LieGroupProperties<Base>::DoF;
+  static constexpr int RepSize = DoF;
+
+  using DataType = Eigen::Matrix<Scalar, RepSize, 1>;
+
+  using Jacobian = Eigen::Matrix<Scalar, DoF, DoF>;
+  using LieAlg   = Eigen::Matrix<Scalar, 5, 5>;
+};
+
+} // namespace internal
+} // namespace manif
+
+namespace manif {
+
+//
+// Tangent
+//
+
+/**
+ * @brief Represents an element of tangent space of SGal3.
+ */
+template <typename _Scalar>
+struct SGal3Tangent : SGal3TangentBase<SGal3Tangent<_Scalar>> {
+private:
+
+  using Base = SGal3TangentBase<SGal3Tangent<_Scalar>>;
+  using Type = SGal3Tangent<_Scalar>;
+
+protected:
+
+    using Base::derived;
+
+public:
+
+  MANIF_MAKE_ALIGNED_OPERATOR_NEW_COND
+
+  MANIF_TANGENT_TYPEDEF
+  MANIF_INHERIT_TANGENT_API
+  MANIF_INHERIT_TANGENT_OPERATOR
+
+  SGal3Tangent()  = default;
+  ~SGal3Tangent() = default;
+
+  MANIF_COPY_CONSTRUCTOR(SGal3Tangent)
+  MANIF_MOVE_CONSTRUCTOR(SGal3Tangent)
+
+  template <typename _DerivedOther>
+  SGal3Tangent(const TangentBase<_DerivedOther>& o);
+
+  MANIF_TANGENT_ASSIGN_OP(SGal3Tangent)
+
+  // Tangent common API
+
+  DataType& coeffs();
+  const DataType& coeffs() const;
+
+  // SGal3Tangent specific API
+
+protected:
+
+  DataType data_;
+};
+
+MANIF_EXTRA_TANGENT_TYPEDEF(SGal3Tangent);
+
+template <typename _Scalar>
+template <typename _DerivedOther>
+SGal3Tangent<_Scalar>::SGal3Tangent(const TangentBase<_DerivedOther>& o)
+  : data_(o.coeffs()) {
+  //
+}
+
+template <typename _Scalar>
+typename SGal3Tangent<_Scalar>::DataType&
+SGal3Tangent<_Scalar>::coeffs() {
+  return data_;
+}
+
+template <typename _Scalar>
+const typename SGal3Tangent<_Scalar>::DataType&
+SGal3Tangent<_Scalar>::coeffs() const {
+  return data_;
+}
+
+} // namespace manif
+
+#endif // _MANIF_MANIF_SGAL3TANGENT_H_