From 006d3411623d7a51542bfbd2266a40c2c38d67a2 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 29 Jun 2021 15:41:00 +0200
Subject: [PATCH] Fix decay and its settings, stream confidences to debug
observer
---
.../server/ObjectMemory/ObjectMemory.cpp | 6 ++---
.../armem_objects/server/instance/Decay.cpp | 9 ++++---
.../server/instance/SegmentAdapter.cpp | 26 ++++++++++++++-----
3 files changed, 28 insertions(+), 13 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 0c5d82c8d..422419b78 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -130,7 +130,6 @@ namespace armarx::armem::server::obj
{
attachmentSegment.init();
});
-
}
void ObjectMemory::onConnectComponent()
@@ -146,6 +145,9 @@ namespace armarx::armem::server::obj
getProxyFromProperty(kinematicUnitObserver, "cmp.KinematicUnitObserverName", false, "", false);
+ // Create first to use the original values.
+ createRemoteGuiTab();
+
instance::SegmentAdapter::connect(
robotStateComponent,
robot,
@@ -169,9 +171,7 @@ namespace armarx::armem::server::obj
ArVizComponentPluginUser::getArvizClient()
);
- createRemoteGuiTab();
RemoteGui_startRunningTask();
-
}
void ObjectMemory::onDisconnectComponent()
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
index f1c926413..2d3942250 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/Decay.cpp
@@ -47,7 +47,7 @@ namespace armarx::armem::server::obj::instance
float Decay::calculateConfidence(IceUtil::Time localization, IceUtil::Time now) const
{
- float duration = (now - localization).toSeconds();
+ const float duration = static_cast<float>((now - localization).toSecondsDouble());
if (duration < delaySeconds)
{
return maxConfidence;
@@ -88,17 +88,17 @@ namespace armarx::armem::server::obj::instance
}
{
maxConfidence.setRange(0, 1);
- maxConfidence.setSteps(20);
+ maxConfidence.setSteps(100);
maxConfidence.setValue(decay.maxConfidence);
}
{
minConfidence.setRange(0, 1);
- minConfidence.setSteps(20);
+ minConfidence.setSteps(100);
minConfidence.setValue(decay.minConfidence);
}
{
removeObjectsBelowConfidence.setRange(0, 1);
- removeObjectsBelowConfidence.setSteps(20);
+ removeObjectsBelowConfidence.setSteps(100);
removeObjectsBelowConfidence.setValue(decay.removeObjectsBelowConfidence);
}
@@ -128,6 +128,7 @@ namespace armarx::armem::server::obj::instance
decay.durationSeconds = durationSeconds.getValue();
decay.maxConfidence = maxConfidence.getValue();
decay.minConfidence = minConfidence.getValue();
+ decay.removeObjectsBelowConfidence = removeObjectsBelowConfidence.getValue();
}
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
index 46a7a4fba..02f93f7e2 100644
--- a/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/instance/SegmentAdapter.cpp
@@ -406,6 +406,8 @@ namespace armarx::armem::server::obj::instance
void SegmentAdapter::visualizeRun()
{
+ ObjectFinder objectFinder;
+
CycleUtil cycle(static_cast<int>(1000 / visu.frequencyHz));
while (visu.updateTask && !visu.updateTask->isStopped())
{
@@ -414,33 +416,45 @@ namespace armarx::armem::server::obj::instance
if (visu.enabled)
{
- TIMING_START(Visu);
+ TIMING_START(tVisu);
objpose::ObjectPoseSeq objectPoses;
- ObjectFinder objectFinder;
visu.minConfidence = -1;
{
std::scoped_lock lock(memoryMutex);
const IceUtil::Time now = TimeUtil::GetTime();
+
+ // Also include decayed objects in result
+ // Store original setting.
+ const float minConf = segment.decay.removeObjectsBelowConfidence;
+ segment.decay.removeObjectsBelowConfidence = -1;
+ // Get result.
objectPoses = segment.getObjectPoses(now);
+ // Restore original setting.
+ segment.decay.removeObjectsBelowConfidence = minConf;
+
objectFinder = segment.objectFinder;
if (segment.decay.enabled)
{
visu.minConfidence = segment.decay.removeObjectsBelowConfidence;
}
}
+
const std::vector<viz::Layer> layers = visu.visualizeCommit(objectPoses, objectFinder);
arviz.commit(layers);
- TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+ TIMING_END_STREAM(tVisu, ARMARX_VERBOSE);
if (debugObserver)
{
- debugObserver->setDebugChannel(getName(),
+ armarx::StringVariantBaseMap debugValues;
+ debugValues["t Visualize [ms]"] = new Variant(tVisu.toMilliSecondsDouble());
+ for (const auto& [id, pose] : objectPoses)
{
- { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- });
+ debugValues["confidence(" + id.str() + ")"] = new Variant(pose.confidence);
+ }
+ debugObserver->setDebugChannel(getName(), debugValues);
}
}
}
--
GitLab