From 6ff443482e911e48f0cf1b7dd475ab67310155fe Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Thu, 5 May 2022 10:49:44 +0200
Subject: [PATCH] Shift timestamps around current time to avoid floating point
 imprecision

---
 .../armem_objects/server/instance/Segment.cpp    | 16 ++++++++++------
 .../armem_objects/server/instance/Visu.cpp       |  5 ++++-
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 8d645a742..483a5065a 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -584,8 +584,6 @@ namespace armarx::armem::server::obj::instance
 
         const ObjectID objID = armarx::fromIce<ObjectID>(request.objectID);
 
-        const double predictionTime = static_cast<double>(
-            armarx::fromIce<Time>(request.timestamp).toMicroSecondsSinceEpoch());
         const Duration lookbackDuration = Duration::SecondsDouble(p.linearPredictionLookbackSeconds);
 
         const wm::Entity* entity = findObjectEntity(objID);
@@ -597,16 +595,20 @@ namespace armarx::armem::server::obj::instance
             return result;
         }
 
+        const Time timeOrigin = Time::Now();
+
         std::vector<double> timestampsSec;
         std::vector<Eigen::Vector3d> positions;
 
         entity->forEachSnapshotInTimeRange(
             Time::Now() - lookbackDuration, Time::Invalid(),
-            [&timestampsSec, &positions](const wm::EntitySnapshot& snapshot)
+            [&timeOrigin, &timestampsSec, &positions](const wm::EntitySnapshot& snapshot)
             {
                 snapshot.forEachInstance(
-                    [&snapshot, &positions, &timestampsSec](const wm::EntityInstance& instance)
+                    [&timeOrigin, &snapshot, &positions, &timestampsSec](const wm::EntityInstance& instance)
                     {
+                        // ToDo: How to handle attached objects?
+
                         arondto::ObjectInstance dto;
                         dto.fromAron(instance.data());
 
@@ -614,7 +616,7 @@ namespace armarx::armem::server::obj::instance
                         fromAron(dto, pose);
 
                         timestampsSec.push_back(
-                            snapshot.time().toDurationSinceEpoch().toSecondsDouble());
+                            (snapshot.time() - timeOrigin).toSecondsDouble());
                         positions.push_back(
                             simox::math::position(pose.objectPoseGlobal).cast<double>());
                     });
@@ -635,7 +637,9 @@ namespace armarx::armem::server::obj::instance
             using simox::math::LinearRegression3D;
             const bool inputOffset = false;
             const LinearRegression3D model = LinearRegression3D::Fit(timestampsSec, positions, inputOffset);
-            prediction = model.predict(predictionTime);
+
+            const Time predictionTime = armarx::fromIce<Time>(request.timestamp);
+            prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
 
             // ToDo: Remove.
             ARMARX_IMPORTANT << "Prediction: " << prediction.transpose() << "\n (model: " << model << ")";
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 5b7defda0..9d662129b 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -248,7 +248,8 @@ namespace armarx::armem::server::obj::instance
             request.objectID = armarx::toIce<armarx::data::ObjectID>(id);
             request.settings.predictionEngineID = "Linear Position Regression";
             request.timestamp = armarx::toIce<armarx::core::time::dto::DateTime>(
-                Time::Now() - Duration::MilliSeconds(100));
+                // ToDo: Make parametrizable.
+                Time::Now() + Duration::MilliSeconds(1000));
             auto predictionResult = predictor(request);
             if (predictionResult.success)
             {
@@ -300,6 +301,8 @@ namespace armarx::armem::server::obj::instance
 
         useArticulatedModels.setValue(visu.useArticulatedModels);
 
+        showLinearPredictions.setValue(visu.showLinearPredictions);
+
         GridLayout grid;
         int row = 0;
         grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
-- 
GitLab