Move predictObjectPose into ArmarXObjects

source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt

@@ -31,6 +31,7 @@ set(LIB_FILES
     plugins/ObjectPoseClientPlugin.cpp
     plugins/RequestedObjects.cpp
 
+    predictions.cpp
     util.cpp
 )
 set(LIB_HEADERS
@@ -57,6 +58,7 @@ set(LIB_HEADERS
     plugins/ObjectPoseClientPlugin.h
     plugins/RequestedObjects.h
 
+    predictions.h
     util.h
 )
 

source/RobotAPI/libraries/ArmarXObjects/predictions.cpp

+/*
+ * 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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects
+ * @author     phesch ( ulila at student dot kit dot edu )
+ * @date       2022
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include "predictions.h"
+
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/regression/linear.h>
+
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
+
+namespace armarx::objpose
+{
+
+    objpose::ObjectPosePredictionResult
+    predictObjectPose(const std::map<DateTime, ObjectPose>& poses,
+                      const DateTime& time,
+                      const ObjectPose& latestPose,
+                      const armem::client::PredictionSettings& settings)
+    {
+        objpose::ObjectPosePredictionResult result;
+        result.success = false;
+
+        if (!settings.predictionEngineID.empty() &&
+            settings.predictionEngineID != "Linear Position Regression")
+        {
+            result.errorMessage = "Prediction engine '" + settings.predictionEngineID +
+                                  "' not available for object pose prediction.";
+            return result;
+        }
+
+        const DateTime timeOrigin = DateTime::Now();
+
+        std::vector<double> timestampsSec;
+        std::vector<Eigen::Vector3d> positions;
+
+        // ToDo: How to handle attached objects?
+        for (const auto& [timestamp, pose] : poses)
+        {
+            timestampsSec.push_back((timestamp - timeOrigin).toSecondsDouble());
+            positions.emplace_back(simox::math::position(pose.objectPoseGlobal).cast<double>());
+        }
+
+        ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
+
+        Eigen::Vector3d prediction;
+        // Static objects don't move. Objects that haven't moved for a while probably won't either.
+        if (timestampsSec.size() <= 1 || latestPose.isStatic)
+        {
+            prediction = simox::math::position(latestPose.objectPoseGlobal).cast<double>();
+        }
+        else
+        {
+            using simox::math::LinearRegression3d;
+            const bool inputOffset = false;
+
+            const LinearRegression3d model =
+                LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
+            const auto predictionTime = armarx::fromIce<DateTime>(time);
+            prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
+        }
+
+        // Used as a template for the returned pose prediction.
+        ObjectPose latestCopy = latestPose;
+
+        // 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.
+        Eigen::Matrix4f latest = latestPose.objectPoseGlobal;
+        simox::math::position(latest) = prediction.cast<float>();
+        latestCopy.setObjectPoseGlobal(latest);
+
+        result.success = true;
+        result.prediction = latestCopy.toIce();
+
+        return result;
+    }
+} // namespace armarx::objpose

source/RobotAPI/libraries/ArmarXObjects/predictions.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/>.
+ *
+ * @package    RobotAPI::ArmarXObjects
+ * @author     phesch ( ulila 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 <ArmarXCore/core/time/DateTime.h>
+
+#include <RobotAPI/interface/objectpose/ObjectPoseStorageInterface.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
+
+namespace armarx::objpose
+{
+    /**
+    * @brief Predict the pose of an object given a history of poses.
+    *
+    * @param poses the history of poses to base the prediction on
+    * @param time the timestamp to make the prediction for
+    * @param latestPose used for metadata so the result is valid even if poses is empty
+    * @param settings the settings to use for the prediction
+    * @return the result of the prediction
+    */
+    objpose::ObjectPosePredictionResult
+    predictObjectPose(const std::map<DateTime, ObjectPose>& poses,
+                      const DateTime& time,
+                      const ObjectPose& latestPose,
+                      const armem::client::PredictionSettings& settings);
+} // namespace armarx::objpose

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

@@ -602,71 +602,6 @@ namespace armarx::armem::server::obj::instance
     }
 
 
-    objpose::ObjectPosePredictionResult
-    Segment::predictObjectPose(const std::map<DateTime, ObjectPose>& poses,
-                               const DateTime& time,
-                               const ObjectPose& latestPose,
-                               const armem::client::PredictionSettings& settings)
-    {
-        objpose::ObjectPosePredictionResult result;
-        result.success = false;
-
-        if (!settings.predictionEngineID.empty() &&
-            settings.predictionEngineID != "Linear Position Regression")
-        {
-            result.errorMessage = "Prediction engine '" + settings.predictionEngineID +
-                                  "' not available for object pose prediction.";
-            return result;
-        }
-
-        const Time timeOrigin = Time::Now();
-
-        std::vector<double> timestampsSec;
-        std::vector<Eigen::Vector3d> positions;
-
-        // ToDo: How to handle attached objects?
-        for (const auto& [timestamp, pose] : poses)
-        {
-            timestampsSec.push_back((timestamp - timeOrigin).toSecondsDouble());
-            positions.push_back(simox::math::position(pose.objectPoseGlobal).cast<double>());
-        }
-
-        ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
-
-        Eigen::Vector3d prediction;
-        // Static objects don't move. Objects that haven't moved for a while probably won't either.
-        if (timestampsSec.size() <= 1 || latestPose.isStatic)
-        {
-            prediction = simox::math::position(latestPose.objectPoseGlobal).cast<double>();
-        }
-        else
-        {
-            using simox::math::LinearRegression3d;
-            const bool inputOffset = false;
-
-            const LinearRegression3d model =
-                LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
-            const Time predictionTime = armarx::fromIce<Time>(time);
-            prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
-        }
-
-        // Used as a template for the returned pose prediction.
-        ObjectPose latestCopy = latestPose;
-
-        // 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.
-        Eigen::Matrix4f latest = latestPose.objectPoseGlobal;
-        simox::math::position(latest) = prediction.cast<float>();
-        latestCopy.setObjectPoseGlobal(latest);
-
-        result.success = true;
-        result.prediction = latestCopy.toIce();
-
-        return result;
-    }
-
-
     objpose::AttachObjectToRobotNodeOutput
     Segment::attachObjectToRobotNode(const objpose::AttachObjectToRobotNodeInput& input)
     {

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

@@ -108,21 +108,6 @@ namespace armarx::armem::server::obj::instance
         static std::map<DateTime, ObjectPose>
         getObjectPosesInRange(const wm::Entity& entity, const DateTime& start, const DateTime& end);
 
-        /**
-         * @brief Predict the pose of an object given a history of poses.
-         *
-         * @param poses the history of poses to base the prediction on
-         * @param time the timestamp to make the prediction for
-         * @param latestPose used for metadata so the result is valid even if poses is empty
-         * @param settings the settings to use for the prediction
-         * @return the result of the prediction
-         */
-        objpose::ObjectPosePredictionResult
-        predictObjectPose(const std::map<DateTime, ObjectPose>& poses,
-                          const DateTime& time,
-                          const ObjectPose& latestPose,
-                          const armem::client::PredictionSettings& settings);
-
     private:
 
         ObjectPoseMap getLatestObjectPoses() const;

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

@@ -26,6 +26,7 @@
 #include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
 #include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
 #include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
 #include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
 #include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
 
@@ -491,7 +492,7 @@ namespace armarx::armem::server::obj::instance
         {
             if (results.at(i).success)
             {
-                results.at(i) = segment.predictObjectPose(
+                results.at(i) = objpose::predictObjectPose(
                     poses.at(i),
                     armarx::fromIce<DateTime>(requests.at(i).timestamp),
                     latestPoses.at(i),