diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
index e871a70d41b3f7932b13cdebe6b7fb0dbef52f9c..9ae2fba6facea867b74367d5e4888f68334fb7d4 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.cpp
@@ -547,6 +547,35 @@ namespace armarx::armem::server::obj::instance
}
+ std::map<DateTime, objpose::ObjectPose>
+ Segment::getObjectPosesInRange(const wm::Entity& entity,
+ const DateTime& start,
+ const DateTime& end)
+ {
+ std::map<DateTime, objpose::ObjectPose> result;
+
+ entity.forEachSnapshotInTimeRange(
+ start,
+ end,
+ [&result](const wm::EntitySnapshot& snapshot)
+ {
+ snapshot.forEachInstance(
+ [&result, &snapshot](const wm::EntityInstance& instance)
+ {
+ arondto::ObjectInstance dto;
+ dto.fromAron(instance.data());
+
+ ObjectPose pose;
+ fromAron(dto, pose);
+
+ result.emplace(snapshot.time(), pose);
+ });
+ });
+
+ return result;
+ }
+
+
std::optional<simox::OrientedBoxf> Segment::getObjectOOBB(const ObjectID& id)
{
return oobbCache.get(id);
@@ -574,78 +603,41 @@ namespace armarx::armem::server::obj::instance
objpose::ObjectPosePredictionResult
- Segment::predictObjectPose(const objpose::ObjectPosePredictionRequest& request)
+ 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 (!request.settings.predictionEngineID.empty() &&
- request.settings.predictionEngineID != "Linear Position Regression")
+ if (!settings.predictionEngineID.empty() &&
+ settings.predictionEngineID != "Linear Position Regression")
{
- result.errorMessage = "Prediction engine '" + request.settings.predictionEngineID +
+ result.errorMessage = "Prediction engine '" + settings.predictionEngineID +
"' not available for object pose prediction.";
return result;
}
- const ObjectID objID = armarx::fromIce<ObjectID>(request.objectID);
-
- const Duration timeWindow = armarx::fromIce<Duration>(request.timeWindow);
-
- const wm::Entity* entity = findObjectEntity(objID);
- if (!entity)
- {
- std::stringstream sstream;
- sstream << "Could not find object with ID " << objID << ".";
- result.errorMessage = sstream.str();
- return result;
- }
-
const Time timeOrigin = Time::Now();
std::vector<double> timestampsSec;
std::vector<Eigen::Vector3d> positions;
- entity->forEachSnapshotInTimeRange(
- Time::Now() - timeWindow,
- Time::Invalid(),
- [&timeOrigin, ×tampsSec, &positions](const wm::EntitySnapshot& snapshot)
- {
- snapshot.forEachInstance(
- [&timeOrigin, &snapshot, &positions, ×tampsSec](
- const wm::EntityInstance& instance)
- {
- // ToDo: How to handle attached objects?
-
- arondto::ObjectInstance dto;
- dto.fromAron(instance.data());
-
- ObjectPose pose;
- fromAron(dto, pose);
-
- timestampsSec.push_back(
- (snapshot.time() - timeOrigin).toSecondsDouble());
- positions.push_back(
- simox::math::position(pose.objectPoseGlobal).cast<double>());
- });
- });
- ARMARX_CHECK_EQUAL(timestampsSec.size(), positions.size());
-
- // 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)
+ // ToDo: How to handle attached objects?
+ for (const auto& [timestamp, pose] : poses)
{
- std::stringstream sstream;
- sstream << "No snapshots of the object " << objID << " were found."
- << " Cannot make a prediction.";
- result.errorMessage = sstream.str();
- return result;
+ 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;
- if (timestampsSec.size() <= 1 || dto.value().pose.isStatic)
+ // 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(dto.value().pose.objectPoseGlobal).cast<double>();
+ prediction = simox::math::position(latestPose.objectPoseGlobal).cast<double>();
}
else
{
@@ -654,23 +646,22 @@ namespace armarx::armem::server::obj::instance
const LinearRegression3d model =
LinearRegression3d::Fit(timestampsSec, positions, inputOffset);
- const Time predictionTime = armarx::fromIce<Time>(request.timestamp);
+ const Time predictionTime = armarx::fromIce<Time>(time);
prediction = model.predict((predictionTime - timeOrigin).toSecondsDouble());
-
}
// Used as a template for the returned pose prediction.
- ObjectPose objectPoseTemplate = getLatestObjectPose(*entity);
+ 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 latestPose = objectPoseTemplate.objectPoseGlobal;
- simox::math::position(latestPose) = prediction.cast<float>();
- objectPoseTemplate.setObjectPoseGlobal(latestPose);
+ Eigen::Matrix4f latest = latestPose.objectPoseGlobal;
+ simox::math::position(latest) = prediction.cast<float>();
+ latestCopy.setObjectPoseGlobal(latest);
result.success = true;
- result.prediction = objectPoseTemplate.toIce();
+ result.prediction = latestCopy.toIce();
return result;
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
index e735eab3316aa11139c390f8615b3b4a73344ae3..e0f97dfce16c33de0c42e2eeec4c5b9c598c655e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Segment.h
@@ -18,6 +18,7 @@
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/segment/SpecializedSegment.h>
@@ -104,8 +105,23 @@ namespace armarx::armem::server::obj::instance
::armarx::armem::articulated_object::ArticulatedObjects getArticulatedObjects();
+ 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 objpose::ObjectPosePredictionRequest& request);
+ predictObjectPose(const std::map<DateTime, ObjectPose>& poses,
+ const DateTime& time,
+ const ObjectPose& latestPose,
+ const armem::client::PredictionSettings& settings);
private:
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 340d945796bcc5cb81db9ab830a436cb2c5185c0..c32be98538f1ba07793536d94dda42637bd5e47e 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -25,8 +25,13 @@
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <RobotAPI/libraries/ArmarXObjects/aron_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
#include <ArmarXCore/observers/variant/Variant.h>
#include <VirtualRobot/Robot.h>
@@ -98,7 +103,7 @@ namespace armarx::armem::server::obj::instance
}
visu.predictor = [this](const objpose::ObjectPosePredictionRequest& request)
- { return segment.predictObjectPose(request); };
+ { return predictObjectPoses({request}).at(0); };
segment.connect(arviz);
}
@@ -439,15 +444,60 @@ namespace armarx::armem::server::obj::instance
const Ice::Current&)
{
objpose::ObjectPosePredictionResultSeq results;
- // TODO(@phesch): Only fetch the required data in the lock, perform the predictions
- // locally in this function's thread.
- segment.doLocked([this,&requests,&results]()
+ std::vector<std::map<DateTime, objpose::ObjectPose>> poses;
+ std::vector<objpose::ObjectPose> latestPoses;
+
+ segment.doLocked(
+ [this, &requests, &results, &poses, &latestPoses]()
+ {
+ for (const auto& request : requests)
+ {
+ auto& result = results.emplace_back();
+ const ObjectID objID = armarx::fromIce(request.objectID);
+
+ const Duration timeWindow = armarx::fromIce<Duration>(request.timeWindow);
+
+ const wm::Entity* entity = segment.findObjectEntity(objID);
+ // Use result.success as a marker for whether to continue later
+ result.success = false;
+ poses.emplace_back();
+ latestPoses.emplace_back();
+ if (!entity)
+ {
+ std::stringstream sstream;
+ sstream << "Could not find object with ID " << objID << ".";
+ result.errorMessage = sstream.str();
+ continue;
+ }
+
+ const auto dto = entity->findLatestInstanceDataAs<arondto::ObjectInstance>();
+ if (!dto)
+ {
+ std::stringstream sstream;
+ sstream << "No snapshots of the object " << objID << " were found."
+ << " Cannot make a prediction.";
+ result.errorMessage = sstream.str();
+ continue;
+ }
+
+ result.success = true;
+ poses.back() = segment.getObjectPosesInRange(
+ *entity, Time::Now() - timeWindow, Time::Invalid());
+ latestPoses.back() = aron::fromAron<objpose::ObjectPose>(dto.value().pose);
+ }
+ });
+
+ for (size_t i = 0; i < requests.size(); ++i)
{
- for (const auto& request : requests)
+ if (results.at(i).success)
{
- results.push_back(segment.predictObjectPose(request));
+ results.at(i) = segment.predictObjectPose(
+ poses.at(i),
+ armarx::fromIce<DateTime>(requests.at(i).timestamp),
+ latestPoses.at(i),
+ armem::client::PredictionSettings::fromIce(requests.at(i).settings));
}
- });
+ }
return results;
}