Move system models into dynamic scene provider

source/armarx/navigation/components/dynamic_scene_provider/CMakeLists.txt

@@ -10,10 +10,12 @@ armarx_add_component(dynamic_scene_provider
         Component.cpp
         ArVizDrawer.cpp
         HumanTracker.cpp
+        HumanSystemModel.cpp
     HEADERS
         Component.h
         ArVizDrawer.h
         HumanTracker.h
+        HumanSystemModel.h
     DEPENDENCIES
         # ArmarXCore
         ArmarXCore
@@ -21,6 +23,7 @@ armarx_add_component(dynamic_scene_provider
         # ArmarXGui
         ## ArmarXGuiComponentPlugins  # For RemoteGui plugin.
         # RobotAPI
+        ukfm
         armem
         armem_robot
         armem_robot_state

source/armarx/navigation/components/dynamic_scene_provider/HumanSystemModel.cpp

+#include "HumanSystemModel.h"
+
+namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter
+{
+
+    // ------------------------------ SO2xR2 -----------------------------------------
+
+    template <typename floatT>
+    typename SystemModelSO2xR2<floatT>::ObsT
+    SystemModelSO2xR2<floatT>::observationFunction(const SystemModelSO2xR2::StateT& state)
+    {
+        ObsT observation;
+        observation.segment(0, 2) = state.position;
+        observation.segment(2, 1) = state.orientation.log().coeffs();
+        return observation;
+    }
+
+    template <typename floatT>
+    typename SystemModelSO2xR2<floatT>::StateT
+    SystemModelSO2xR2<floatT>::propagationFunction(const SystemModelSO2xR2::StateT& state,
+                                                   const SystemModelSO2xR2::ControlT& control,
+                                                   const SystemModelSO2xR2::ControlNoiseT& noise,
+                                                   FloatT dt)
+    {
+        StateT new_state;
+        new_state.orientation = state.orientation.template rplus(control.angular_velocity * dt);
+        new_state.position = state.position + control.euclidean_velocity * dt;
+        return new_state;
+    }
+
+    template <typename floatT>
+    typename SystemModelSO2xR2<floatT>::StateT
+    SystemModelSO2xR2<floatT>::retraction(const SystemModelSO2xR2::StateT& state,
+                                          const SystemModelSO2xR2::SigmaPointsT& sigmaPoints)
+    {
+        StateT new_state;
+        manif::SO2Tangent<FloatT> tan;
+        tan.coeffs() << sigmaPoints.segment(2, 1);
+        new_state.orientation = state.orientation.lplus(tan);
+
+        new_state.position = state.position + sigmaPoints.segment(0, 2);
+        return new_state;
+    }
+
+    template <typename floatT>
+    typename SystemModelSO2xR2<floatT>::SigmaPointsT
+    SystemModelSO2xR2<floatT>::inverseRetraction(const SystemModelSO2xR2::StateT& state1,
+                                                 const SystemModelSO2xR2::StateT& state2)
+    {
+        SigmaPointsT sigma;
+        // TODO: check if right order of substraction
+        sigma.segment(0, 2) = state1.position - state2.position;
+        sigma.segment(2, 1) = state2.orientation.lminus(state1.orientation).coeffs();
+        return sigma;
+    }
+
+    template struct SystemModelSO2xR2<float>;
+    template struct SystemModelSO2xR2<double>;
+
+} // namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter

source/armarx/navigation/components/dynamic_scene_provider/HumanSystemModel.h

+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Tobias Gröger ( tobias dot groeger at student dot kit dot edu )
+ * @author     Corvin Navarro Ecker ( corvin dot ecker at student dot kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+#pragma once
+
+#include <RobotAPI/libraries/ukfm/SystemModel.h>
+
+#include <manif/SO2.h>
+
+namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter
+{
+    //----------- SO2xR2 -----------------
+
+    template <typename floatT>
+    struct StateSO2xR2
+    {
+        manif::SO2<floatT> orientation;
+        Eigen::Matrix<floatT, 2, 1> position;
+    };
+
+    template <typename floatT>
+    struct ControlSO2xR2
+    {
+        typename manif::SO2<floatT>::Tangent angular_velocity;
+        Eigen::Matrix<floatT, 2, 1> euclidean_velocity;
+    };
+
+    template <typename floatT>
+    struct SystemModelSO2xR2
+    {
+        static_assert(std::is_floating_point_v<floatT>);
+        struct dim
+        {
+            static constexpr long state = 3, control = 3, obs = 3;
+        };
+
+        using FloatT = floatT;
+        using StateT = StateSO2xR2<FloatT>;
+        using ControlT = ControlSO2xR2<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 SystemModelSO2xR2<float>;
+    extern template struct SystemModelSO2xR2<double>;
+
+} // namespace armarx::navigation::components::dynamic_scene_provider::kalman_filter