From 8b2a2101f55f9f4f33336eb81043d24a2ca50678 Mon Sep 17 00:00:00 2001
From: phesch <ulila@student.kit.edu>
Date: Tue, 17 May 2022 21:09:57 +0200
Subject: [PATCH] Move predictObjectPose into ArmarXObjects
---
.../libraries/ArmarXObjects/CMakeLists.txt | 2 +
.../libraries/ArmarXObjects/predictions.cpp | 96 +++++++++++++++++++
.../libraries/ArmarXObjects/predictions.h | 46 +++++++++
.../armem_objects/server/instance/Segment.cpp | 65 -------------
.../armem_objects/server/instance/Segment.h | 15 ---
.../server/instance/SegmentAdapter.cpp | 3 +-
6 files changed, 146 insertions(+), 81 deletions(-)
create mode 100644 source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
create mode 100644 source/RobotAPI/libraries/ArmarXObjects/predictions.h
diff --git a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt b/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
index 2b2fb5f9d..ec6658fa8 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/CMakeLists.txt
+++ b/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
)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
new file mode 100644
index 000000000..f3550f874
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.cpp
@@ -0,0 +1,96 @@
+/*
+ * 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
diff --git a/source/RobotAPI/libraries/ArmarXObjects/predictions.h b/source/RobotAPI/libraries/ArmarXObjects/predictions.h
new file mode 100644
index 000000000..900758590
--- /dev/null
+++ b/source/RobotAPI/libraries/ArmarXObjects/predictions.h
@@ -0,0 +1,46 @@
+/*
+ * 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
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index 9ae2fba6f..2c8ca4e7d 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/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)
{
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index e0f97dfce..0aa9b9304 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index c32be9853..010263ceb 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/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),
--
GitLab