From fc184cc7c4b079f51e2359fa16776875c4f77d57 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Mon, 22 Feb 2021 14:29:35 +0100
Subject: [PATCH] Implement decay
---
.../ObjectPoseObserver/ObjectPoseObserver.cpp | 4 +--
.../detail/ObjectPoseDecay.cpp | 30 +++++++++++++++++++
.../detail/ObjectPoseDecay.h | 30 ++++++++++++++++---
3 files changed, 58 insertions(+), 6 deletions(-)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 13e240f38..a7640731b 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -747,12 +747,12 @@ namespace armarx
{
{
std::scoped_lock lock(dataMutex);
+ IceUtil::Time now = TimeUtil::GetTime();
for (auto& [providerName, objectPoses] : data.objectPoses)
{
for (objpose::ObjectPose& pose : objectPoses)
{
- // ToDo: update their confidence according to their
- // timestamp and remove objects whose confidence is 0.
+ decay.updateConfidence(pose, now);
}
}
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
index c716765ab..8edfb4d69 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp
@@ -1,7 +1,37 @@
#include "ObjectPoseDecay.h"
+#include <SimoxUtility/math/scale_value.h>
+
+#include <ArmarXCore/core/time/TimeUtil.h>
+
namespace armarx::objpose
{
+ void ObjectPoseDecay::updateConfidence(ObjectPose& pose, IceUtil::Time now)
+ {
+ float confidence = calculateConfidence(pose.timestamp, now);
+ pose.confidence = confidence;
+ }
+
+ float ObjectPoseDecay::calculateConfidence(IceUtil::Time localization, IceUtil::Time now)
+ {
+ float duration = (now - localization).toSeconds();
+ if (duration < noDecaySeconds)
+ {
+ return maxConfidence;
+ }
+ else if (duration > noDecaySeconds + this->durationSeconds)
+ {
+ return minConfidence;
+ }
+ else
+ {
+ return simox::math::scale_value_from_to(
+ duration,
+ noDecaySeconds, noDecaySeconds + this->durationSeconds,
+ minConfidence, maxConfidence);
+ }
+ }
+
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
index a68223beb..85d564634 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h
@@ -1,22 +1,44 @@
#pragma once
+#include <IceUtil/Time.h>
+
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectPose.h>
+
namespace armarx::objpose
{
- class ObjectPoseDecay
+ /**
+ * @brief Models decay of object localizations by decreasing the confidence
+ * the longer the object was not localized.
+ */
+ class ObjectPoseDecay : public armarx::Logging
{
public:
- /// Time for an object pose to decay.
+ void updateConfidence(ObjectPose& pose, IceUtil::Time now);
+
+ private:
+
+ float calculateConfidence(IceUtil::Time localization, IceUtil::Time now);
+
+
+ public:
+
+ /// Duration after latest localization when decay starts.
+ float noDecaySeconds = 5.0;
+ /// How long to reach minConfidence.
float durationSeconds = 20.0;
- /// Frequency of updating the current decay.
- float updateFrequencyHz = 10.0;
+ float maxConfidence = 1.0;
+ float minConfidence = 0.0;
// bool removeDecayedObjects = false;
+ /// Frequency of updating the current decay.
+ float updateFrequencyHz = 10.0;
SimpleRunningTask<>::pointer_type updateTask;
};
--
GitLab