From 864034c751c55772f5f834c4a1f420ff746d49a2 Mon Sep 17 00:00:00 2001
From: phesch <ulila@student.kit.edu>
Date: Mon, 9 May 2022 20:19:19 +0200
Subject: [PATCH] Add fallbacks to linear prediction for edge cases

---
 .../armem_objects/server/instance/Segment.cpp | 25 +++++++++++++------
 1 file changed, 17 insertions(+), 8 deletions(-)

diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 32398f8c4..e871a70d4 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -606,11 +606,13 @@ namespace armarx::armem::server::obj::instance
         std::vector<Eigen::Vector3d> positions;
 
         entity->forEachSnapshotInTimeRange(
-            Time::Now() - timeWindow, Time::Invalid(),
+            Time::Now() - timeWindow,
+            Time::Invalid(),
             [&timeOrigin, &timestampsSec, &positions](const wm::EntitySnapshot& snapshot)
             {
                 snapshot.forEachInstance(
-                    [&timeOrigin, &snapshot, &positions, &timestampsSec](const wm::EntityInstance& instance)
+                    [&timeOrigin, &snapshot, &positions, &timestampsSec](
+                        const wm::EntityInstance& instance)
                     {
                         // ToDo: How to handle attached objects?
 
@@ -628,26 +630,33 @@ namespace armarx::armem::server::obj::instance
             });
         ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
 
-        if (timestampsSec.empty())
+        // We always retrieve the latest instance in addition to the positions to check
+        // the isStatic attribute.
+        const auto dto = entity->findLatestInstanceDataAs<arondto::ObjectInstance>();
+        if (!dto)
         {
             std::stringstream sstream;
-            sstream << "No snapshots of the object " << objID << " were found in the last "
-                    << timeWindow << ". Cannot make a prediction.";
+            sstream << "No snapshots of the object " << objID << " were found."
+                    << " Cannot make a prediction.";
             result.errorMessage = sstream.str();
             return result;
         }
 
         Eigen::Vector3d prediction;
+        if (timestampsSec.size() <= 1 || dto.value().pose.isStatic)
+        {
+            prediction = simox::math::position(dto.value().pose.objectPoseGlobal).cast<double>();
+        }
+        else
         {
             using simox::math::LinearRegression3d;
             const bool inputOffset = false;
 
-            const LinearRegression3d model = LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
+            const LinearRegression3d model =
+                LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
             const Time predictionTime = armarx::fromIce<Time>(request.timestamp);
             prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
 
-            // ToDo: Remove.
-            ARMARX_IMPORTANT << "Prediction: " << prediction.transpose() << "\n (model: " << model << ")";
         }
 
         // Used as a template for the returned pose prediction.
-- 
GitLab