From 283fb9658ca16d239e8d1c185bfab0b93baf29f3 Mon Sep 17 00:00:00 2001
From: phesch <ulila@student.kit.edu>
Date: Wed, 18 May 2022 21:03:18 +0200
Subject: [PATCH] ObjectMemory Visu: Predictions with fewer locks
---
.../server/instance/SegmentAdapter.cpp | 27 ++++---
.../armem_objects/server/instance/Visu.cpp | 72 +++++++++++++------
.../armem_objects/server/instance/Visu.h | 12 ++--
3 files changed, 78 insertions(+), 33 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 010263ceb..25bf97603 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -94,6 +94,7 @@ namespace armarx::armem::server::obj::instance
robotHead.fetchDatafields();
visu.arviz = arviz;
+
if (!visu.updateTask)
{
visu.updateTask = new SimpleRunningTask<>([this]()
@@ -103,8 +104,6 @@ namespace armarx::armem::server::obj::instance
visu.updateTask->start();
}
- visu.predictor = [this](const objpose::ObjectPosePredictionRequest& request)
- { return predictObjectPoses({request}).at(0); };
segment.connect(arviz);
}
@@ -527,9 +526,10 @@ namespace armarx::armem::server::obj::instance
TIMING_START(tVisu);
objpose::ObjectPoseMap objectPoses;
+ std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>> poseHistories;
visu.minConfidence = -1;
- segment.doLocked([this, &objectPoses, &objectFinder]()
+ segment.doLocked([this, &objectPoses, &poseHistories, &objectFinder]()
{
const Time now = Time::Now();
@@ -547,13 +547,24 @@ namespace armarx::armem::server::obj::instance
{
visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
}
- });
- /* TODO(phesch): Fetch data required for predictions from the memory
- * in the locked part (above), run visualization code below.
- */
+ // Get histories.
+ for (const auto& [id, objectPose] : objectPoses)
+ {
+ const wm::Entity* entity = segment.findObjectEntity(id);
+ if (entity != nullptr)
+ {
+ poseHistories[id] = instance::Segment::getObjectPosesInRange(
+ *entity,
+ Time::Now() - Duration::SecondsDouble(
+ visu.linearPredictionTimeWindowSeconds),
+ Time::Invalid());
+ }
+ }
+ });
- const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
+ const std::vector<viz::Layer> layers =
+ visu.visualizeCommit(objectPoses, poseHistories, objectFinder);
arviz.commit(layers);
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
index 5f1a8c0ae..7d9c94ded 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.cpp
@@ -9,6 +9,8 @@
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/predictions.h>
+#include <RobotAPI/libraries/armem/client/Prediction.h>
namespace armarx::armem::server::obj::instance
@@ -58,40 +60,67 @@ namespace armarx::armem::server::obj::instance
}
- std::vector<viz::Layer> Visu::visualizeCommit(
+ std::vector<viz::Layer>
+ Visu::visualizeCommit(
const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
+ const std::map<std::string, std::vector<std::map<DateTime, objpose::ObjectPose>>>&
+ poseHistories,
const ObjectFinder& objectFinder) const
{
std::vector<viz::Layer> layers;
for (const auto& [name, poses] : objectPoses)
{
- layers.push_back(visualizeProvider(name, poses, objectFinder));
+ auto poseHistoryMap = poseHistories.find(name);
+ if (poseHistoryMap != poseHistories.end())
+ {
+ layers.push_back(visualizeProvider(name, poses, poseHistoryMap->second, objectFinder));
+ }
+ else
+ {
+ layers.push_back(visualizeProvider(name, poses, {}, objectFinder));
+ }
}
return layers;
}
- std::vector<viz::Layer> Visu::visualizeCommit(
- const objpose::ObjectPoseSeq& objectPoses,
- const ObjectFinder& objectFinder) const
+ std::vector<viz::Layer>
+ Visu::visualizeCommit(const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
+ const ObjectFinder& objectFinder) const
{
std::map<std::string, viz::Layer> stage;
- for (const objpose::ObjectPose& objectPose : objectPoses)
+ for (size_t i = 0; i < objectPoses.size(); ++i)
{
- visualizeObjectPose(getLayer(objectPose.providerName, stage), objectPose, objectFinder);
+ visualizeObjectPose(getLayer(objectPoses.at(i).providerName, stage),
+ objectPoses.at(i),
+ poseHistories.at(i),
+ objectFinder);
}
return simox::alg::get_values(stage);
}
- std::vector<viz::Layer> Visu::visualizeCommit(
+ std::vector<viz::Layer>
+ Visu::visualizeCommit(
const objpose::ObjectPoseMap& objectPoses,
+ const std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder) const
{
std::map<std::string, viz::Layer> stage;
- for (const auto& [id, objectPose] : objectPoses)
+ for (const auto& [id, pose] : objectPoses)
{
- visualizeObjectPose(getLayer(objectPose.providerName, stage), objectPose, objectFinder);
+
+ auto poseHistoryMap = poseHistories.find(id);
+ if (poseHistoryMap != poseHistories.end())
+ {
+ visualizeObjectPose(
+ getLayer(pose.providerName, stage), pose, poseHistoryMap->second, objectFinder);
+ }
+ else
+ {
+ visualizeObjectPose(getLayer(pose.providerName, stage), pose, {}, objectFinder);
+ }
}
return simox::alg::get_values(stage);
}
@@ -110,15 +139,17 @@ namespace armarx::armem::server::obj::instance
}
- viz::Layer Visu::visualizeProvider(
+ viz::Layer
+ Visu::visualizeProvider(
const std::string& providerName,
const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder) const
{
viz::Layer layer = arviz.layer(providerName);
- for (const objpose::ObjectPose& objectPose : objectPoses)
+ for (size_t i = 0; i < poseHistories.size(); ++i)
{
- visualizeObjectPose(layer, objectPose, objectFinder);
+ visualizeObjectPose(layer, objectPoses.at(i), poseHistories.at(i), objectFinder);
}
return layer;
}
@@ -126,6 +157,7 @@ namespace armarx::armem::server::obj::instance
void Visu::visualizeObjectPose(
viz::Layer& layer,
const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
const ObjectFinder& objectFinder) const
{
const bool show = objectPose.confidence >= minConfidence;
@@ -248,13 +280,13 @@ namespace armarx::armem::server::obj::instance
}
if (showLinearPredictions)
{
- objpose::ObjectPosePredictionRequest request;
- toIce(request.objectID, id);
- request.settings.predictionEngineID = "Linear Position Regression";
- toIce(request.timeWindow, Duration::SecondsDouble(linearPredictionTimeWindowSeconds));
- toIce(request.timestamp,
- Time::Now() + Duration::SecondsDouble(linearPredictionTimeOffsetSeconds));
- auto predictionResult = predictor(request);
+ armem::client::PredictionSettings settings;
+ settings.predictionEngineID = "Linear Position Regression";
+ auto predictionResult = objpose::predictObjectPose(
+ poseHistory,
+ Time::Now() + Duration::SecondsDouble(linearPredictionTimeOffsetSeconds),
+ objectPose,
+ settings);
if (predictionResult.success)
{
auto predictedPose =
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
index 56380159b..0341f5144 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Visu.h
@@ -30,22 +30,26 @@ namespace armarx::armem::server::obj::instance
std::vector<viz::Layer> visualizeCommit(
const std::map<std::string, objpose::ObjectPoseSeq>& objectPoses,
- const ObjectFinder& objectFinder
- ) const;
+ const std::map<std::string, std::vector<std::map<DateTime, objpose::ObjectPose>>>&
+ poseHistories,
+ const ObjectFinder& objectFinder) const;
/// Visualize the given object poses, with one layer per provider.
std::vector<viz::Layer> visualizeCommit(
const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder
) const;
std::vector<viz::Layer> visualizeCommit(
const objpose::ObjectPoseMap& objectPoses,
+ const std::map<ObjectID, std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder
) const;
viz::Layer visualizeProvider(
const std::string& providerName,
const objpose::ObjectPoseSeq& objectPoses,
+ const std::vector<std::map<DateTime, objpose::ObjectPose>>& poseHistories,
const ObjectFinder& objectFinder
) const;
@@ -53,6 +57,7 @@ namespace armarx::armem::server::obj::instance
void visualizeObjectPose(
viz::Layer& layer,
const objpose::ObjectPose& objectPose,
+ const std::map<DateTime, objpose::ObjectPose>& poseHistory,
const ObjectFinder& objectFinder
) const;
@@ -94,9 +99,6 @@ namespace armarx::armem::server::obj::instance
SimpleRunningTask<>::pointer_type updateTask;
- std::function<objpose::ObjectPosePredictionResult(const objpose::ObjectPosePredictionRequest&)> predictor;
-
-
struct RemoteGui
{
armarx::RemoteGui::Client::GroupBox group;
--
GitLab