Prototype SE3 predictions in localization

source/RobotAPI/libraries/armem_robot_state/server/CMakeLists.txt

@@ -8,6 +8,8 @@ armarx_set_target("Library: ${LIB_NAME}")
 
 armarx_build_if(Eigen3_FOUND "Eigen3 not available")
 
+find_package(manif QUIET)
+armarx_build_if(manif_FOUND "manif not available")
 
 armarx_add_library(
     LIBS 
@@ -27,6 +29,7 @@ armarx_add_library(
 
         # System / External
         Eigen3::Eigen
+        ${manif_LIBRARIES}
 
     HEADERS
         forward_declarations.h
@@ -69,5 +72,8 @@ armarx_add_library(
         description/Segment.cpp
 )
 
+if(manif_FOUND)
+    target_include_directories("${LIB_NAME}" SYSTEM PUBLIC ${manif_INCLUDE_DIR} ${manif_INCLUDE_DIRS})
+endif()
 
 add_library(RobotAPI::armem_robot_state_server ALIAS armem_robot_state_server)

source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp

@@ -3,6 +3,8 @@
 // STL
 #include <iterator>
 
+#include <manif/SE3.h>
+
 #include <SimoxUtility/math/pose/pose.h>
 #include <SimoxUtility/math/regression/linear.h>
 
@@ -206,46 +208,54 @@ namespace armarx::armem::server::robot_state::localization
             return result;
         }
 
+        static const int tangentDims = 6;
+        using Vector6d = Eigen::Matrix<double, tangentDims, 1>;
+
         const DateTime timeOrigin = DateTime::Now();
         const armarx::Duration timeWindow =
             Duration::SecondsDouble(properties.predictionTimeWindow);
-        LatestSnapshotInfo<Eigen::Vector3d, arondto::Transform> info;
+        LatestSnapshotInfo<Vector6d, arondto::Transform> info;
 
         doLocked(
             [&, this]()
             {
-                info = getSnapshotsInRange<server::wm::CoreSegment,
-                                          Eigen::Vector3d,
-                                          arondto::Transform>(
+                info = getSnapshotsInRange<server::wm::CoreSegment, Vector6d, arondto::Transform>(
                     segmentPtr,
                     request.snapshotID,
                     timeOrigin - timeWindow,
                     timeOrigin,
                     [](const aron::data::DictPtr& data)
-                    { return simox::math::position(arondto::Transform::FromAron(data).transform).cast<double>(); },
+                    {
+                        Eigen::Matrix4d mat =
+                            arondto::Transform::FromAron(data).transform.cast<double>();
+                        manif::SE3d se3(simox::math::position(mat),
+                                        Eigen::Quaterniond(simox::math::orientation(mat)));
+                        return se3.log().coeffs();
+                    },
                     [](const aron::data::DictPtr& data)
                     { return arondto::Transform::FromAron(data); });
             });
 
         if (info.success)
         {
-            Eigen::Vector3f latestPosition = simox::math::position(info.latestValue.transform);
-            Eigen::Vector3f prediction;
+            Eigen::Matrix4f prediction;
             if (info.timestampsSec.size() <= 1)
             {
-                prediction = latestPosition;
+                prediction = info.latestValue.transform;
             }
             else
             {
-                using simox::math::LinearRegression3d;
+                using simox::math::LinearRegression;
                 const bool inputOffset = false;
-                const LinearRegression3d model =
-                    LinearRegression3d::Fit(info.timestampsSec, info.values, inputOffset);
+                const LinearRegression<tangentDims> model = LinearRegression<tangentDims>::Fit(
+                    info.timestampsSec, info.values, inputOffset);
                 const auto predictionTime = request.snapshotID.timestamp;
-                prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble()).cast<float>();
+                Vector6d linearPred =
+                    model.predict((predictionTime - timeOrigin).toSecondsDouble());
+                prediction = manif::SE3Tangentd(linearPred).exp().transform().cast<float>();
             }
 
-            simox::math::position(info.latestValue.transform) = prediction;
+            info.latestValue.transform = prediction;
             result.success = true;
             result.prediction = info.latestValue.toAron();
         }