From ea64a429b1669c98c56c155636535b6feef18bdc Mon Sep 17 00:00:00 2001
From: phesch <ulila@student.kit.edu>
Date: Mon, 9 May 2022 21:39:55 +0200
Subject: [PATCH] Make ObjPosClientEx visualize multiple predictions
---
.../ObjectPoseClientExample.cpp | 74 ++++++++++++++++++-
.../ObjectPoseClientExample.h | 10 +++
2 files changed, 83 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
index 6359111c2..b4f8abe9b 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.cpp
@@ -22,7 +22,13 @@
#include "ObjectPoseClientExample.h"
+#include <SimoxUtility/math/pose.h>
+
+#include <ArmarXCore/core/ice_conversions/ice_conversions_templates.h>
#include <ArmarXCore/core/time/CycleUtil.h>
+#include <ArmarXCore/core/time/ice_conversions.h>
+
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
namespace armarx
@@ -32,6 +38,12 @@ namespace armarx
{
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+ defs->optional(p.predictionsPerObject, "predictions.NumberPerObject",
+ "How many predictions with increasing time offsets to make per object.");
+
+ defs->optional(p.millisecondPredictionIncrement, "predictions.TimeIncrement",
+ "The size of the prediction time offset increment in milliseconds.");
+
return defs;
}
@@ -93,8 +105,68 @@ namespace armarx
}
arviz.commit({layer});
}
+ visualizePredictions(client, objectPoses);
cycle.waitForCycleDuration();
}
}
-}
+
+ void
+ ObjectPoseClientExample::visualizePredictions(const objpose::ObjectPoseClient& client,
+ const objpose::ObjectPoseSeq& objectPoses)
+ {
+
+ viz::Layer layer = arviz.layer("PredictionArray");
+ objpose::ObjectPosePredictionRequestSeq requests;
+ for (const objpose::ObjectPose& objectPose : objectPoses)
+ {
+ for (int i = 0; i < p.predictionsPerObject; ++i)
+ {
+ objpose::ObjectPosePredictionRequest request;
+ toIce(request.objectID, objectPose.objectID);
+ request.settings.predictionEngineID = "Linear Position Regression";
+ toIce(request.timestamp,
+ DateTime::Now() +
+ Duration::MilliSeconds((i + 1) * p.millisecondPredictionIncrement));
+ toIce(request.timeWindow, Duration::Seconds(2));
+ requests.push_back(request);
+ }
+ }
+
+ objpose::ObjectPosePredictionResultSeq results;
+ try
+ {
+ results = client.objectPoseStorage->predictObjectPoses(requests);
+ }
+ catch (const Ice::LocalException& e)
+ {
+ ARMARX_INFO << "Failed to get predictions for object poses: " << e.what();
+ }
+
+ for (size_t i = 0; i < results.size(); ++i)
+ {
+ const objpose::ObjectPosePredictionResult& result = results.at(i);
+ if (result.success)
+ {
+ auto predictedPose = armarx::fromIce<objpose::ObjectPose>(result.prediction);
+ Eigen::Vector3f predictedPosition =
+ simox::math::position(predictedPose.objectPoseGlobal);
+ int alpha =
+ (p.predictionsPerObject - (static_cast<int>(i) % p.predictionsPerObject)) *
+ 255 / p.predictionsPerObject; // NOLINT
+ layer.add(
+ viz::Arrow(predictedPose.objectID.str() + " Linear Prediction " +
+ std::to_string(i % p.predictionsPerObject))
+ .fromTo(simox::math::position(
+ objectPoses.at(i / p.predictionsPerObject).objectPoseGlobal),
+ predictedPosition)
+ .color(viz::Color::green(255, alpha))); // NOLINT
+ }
+ else
+ {
+ ARMARX_INFO << "Prediction for object '" << result.prediction.objectID
+ << "' failed: " << result.errorMessage;
+ }
+ }
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
index c49e28f35..6bfffbf35 100644
--- a/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
+++ b/source/RobotAPI/components/ObjectPoseClientExample/ObjectPoseClientExample.h
@@ -75,10 +75,20 @@ namespace armarx
void objectProcessingTaskRun();
+ void visualizePredictions(const objpose::ObjectPoseClient& client,
+ const objpose::ObjectPoseSeq& objectPoses);
+
private:
armarx::SimpleRunningTask<>::pointer_type objectProcessingTask;
+ struct Properties
+ {
+ int predictionsPerObject = 5; // NOLINT
+ int millisecondPredictionIncrement = 200; // NOLINT
+ };
+ Properties p;
+
};
}
--
GitLab