Minor changes to prediction code

source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp

@@ -581,60 +581,61 @@ namespace armarx::armem::server::obj::instance
         objpose::ObjectPosePredictionResult result;
         result.success = false;
 
-        ObjectID objID = armarx::fromIce<ObjectID>(request.objectID);
-        auto* entity = findObjectEntity(objID);
+        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);
         if (!entity)
         {
             std::stringstream sstream;
-            sstream << "Could not find object with ID " << objID;
+            sstream << "Could not find object with ID " << objID << ".";
             result.errorMessage = sstream.str();
             return result;
         }
 
-        // Used as a template for the returned pose prediction
-        auto objectPoseTemplate = getLatestObjectPose(*entity);
-
-        std::vector<double> timestamps;
+        std::vector<double> timestampsSec;
         std::vector<Eigen::Vector3d> positions;
 
         entity->forEachSnapshotInTimeRange(
-            Time::Now() - Time(Duration::SecondsDouble(p.linearPredictionLookbackSeconds)),
-            Time(0),
-            [&timestamps, &positions](const wm::EntitySnapshot& snapshot)
+            Time::Now() - lookbackDuration, Time::Invalid(),
+            [&timestampsSec, &positions](const wm::EntitySnapshot& snapshot)
             {
                 snapshot.forEachInstance(
-                    [&snapshot, &positions, &timestamps](const wm::EntityInstance& instance)
+                    [&snapshot, &positions, &timestampsSec](const wm::EntityInstance& instance)
                     {
                         arondto::ObjectInstance dto;
-                        ObjectPose pose;
                         dto.fromAron(instance.data());
+
+                        ObjectPose pose;
                         fromAron(dto, pose);
-                        timestamps.push_back(
-                            static_cast<double>(snapshot.time().toMicroSecondsSinceEpoch()));
+
+                        timestampsSec.push_back(
+                            snapshot.time().toDurationSinceEpoch().toSecondsDouble());
                         positions.push_back(
                             simox::math::position(pose.objectPoseGlobal).cast<double>());
                     });
             });
+        ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
 
-        ARMARX_CHECK_EQUAL(timestamps.size(), positions.size());
-
-        if (timestamps.empty())
+        if (timestampsSec.empty())
         {
             std::stringstream sstream;
             sstream << "No snapshots of the object " << objID << " were found in the last "
-                    << p.linearPredictionLookbackSeconds << " seconds."
-                    << " Cannot make a prediction.";
+                    << lookbackDuration << ". Cannot make a prediction.";
             result.errorMessage = sstream.str();
             return result;
         }
 
-        double predictionTime = static_cast<double>(
-            armarx::fromIce<Time>(request.timestamp).toMicroSecondsSinceEpoch());
+        const Eigen::Vector3d prediction = makeLinearPrediction(timestampsSec, positions, predictionTime);
+        // ToDo: Remove.
+        ARMARX_IMPORTANT << "Prediction: " << prediction.transpose();
 
-        Eigen::Vector3d prediction = makeLinearPrediction(timestamps, positions, predictionTime);
+        // Used as a template for the returned pose prediction.
+        ObjectPose objectPoseTemplate = getLatestObjectPose(*entity);
 
-        ARMARX_IMPORTANT << "Prediction: " << prediction.x() << ", " << prediction.y() << ", "
-                         << prediction.z();
         // Reuse the rotation from the latest pose.
         // This is a pretty generous assumption, but the linear model doesn't cover rotations,
         // so it's our best guess.