Add SE2 SystemModel

source/armarx/navigation/human/HumanSystemModel.cpp

@@ -65,4 +65,52 @@ namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter
     template struct SystemModelSO2xR2<float>;
     template struct SystemModelSO2xR2<double>;
 
+
+    // ------------------------------ SE2 -----------------------------------------
+
+    template <typename floatT>
+    typename SystemModelSE2<floatT>::ObsT
+    SystemModelSE2<floatT>::observationFunction(const SystemModelSE2::StateT& state)
+    {
+        ObsT observation = state.pose.log().coeffs();
+        return observation;
+    }
+
+    template <typename floatT>
+    typename SystemModelSE2<floatT>::StateT
+    SystemModelSE2<floatT>::propagationFunction(const SystemModelSE2::StateT& state,
+                                                const SystemModelSE2::ControlT& control,
+                                                const SystemModelSE2::ControlNoiseT& noise,
+                                                FloatT dt)
+    {
+        StateT new_state;
+        new_state.pose = state.pose.template rplus(control.velocity * dt);
+        return new_state;
+    }
+
+    template <typename floatT>
+    typename SystemModelSE2<floatT>::StateT
+    SystemModelSE2<floatT>::retraction(const SystemModelSE2::StateT& state,
+                                       const SystemModelSE2::SigmaPointsT& sigmaPoints)
+    {
+        StateT new_state;
+        manif::SE2Tangent<FloatT> tan;
+        tan.coeffs() << sigmaPoints;
+        new_state.pose = state.pose.lplus(tan);
+        return new_state;
+    }
+
+    template <typename floatT>
+    typename SystemModelSE2<floatT>::SigmaPointsT
+    SystemModelSE2<floatT>::inverseRetraction(const SystemModelSE2::StateT& state1,
+                                              const SystemModelSE2::StateT& state2)
+    {
+        SigmaPointsT sigma;
+        sigma = state2.pose.lminus(state1.pose).coeffs();
+        return sigma;
+    }
+
+    template struct SystemModelSE2<float>;
+    template struct SystemModelSE2<double>;
+
 } // namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter

source/armarx/navigation/human/HumanSystemModel.h

@@ -21,6 +21,7 @@
  */
 #pragma once
 
+#include <manif/SE2.h>
 #include <manif/SO2.h>
 
 namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter
@@ -73,4 +74,51 @@ namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter
     extern template struct SystemModelSO2xR2<float>;
     extern template struct SystemModelSO2xR2<double>;
 
+
+    //----------- SE2 -----------------
+
+    template <typename floatT>
+    struct StateSE2
+    {
+        manif::SE2<floatT> pose;
+    };
+
+    template <typename floatT>
+    struct ControlSE2
+    {
+        typename manif::SE2<floatT>::Tangent velocity;
+    };
+
+    template <typename floatT>
+    struct SystemModelSE2
+    {
+        static_assert(std::is_floating_point_v<floatT>);
+        struct dim
+        {
+            static constexpr long state = 3, control = 3, obs = 3;
+        };
+
+        using FloatT = floatT;
+        using StateT = StateSE2<FloatT>;
+        using ControlT = ControlSE2<FloatT>;
+        using ObsT = Eigen::Matrix<FloatT, dim::obs, 1>;
+        using ControlNoiseT = Eigen::
+            Matrix<FloatT, dim::control, 1>; // todo: correct name? Is it the same as ControlT?
+        using SigmaPointsT = Eigen::Matrix<FloatT, dim::state, 1>; // todo: rename
+
+        static ObsT observationFunction(const StateT& state);
+
+        static StateT propagationFunction(const StateT& state,
+                                          const ControlT& control,
+                                          const ControlNoiseT& noise,
+                                          FloatT dt);
+
+        static StateT retraction(const StateT& state, const SigmaPointsT& sigmaPoints);
+
+        static SigmaPointsT inverseRetraction(const StateT& state1, const StateT& state2);
+    };
+
+    extern template struct SystemModelSE2<float>;
+    extern template struct SystemModelSE2<double>;
+
 } // namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter