diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp index 0c5d82c8da19676c0ab3b544ea381ed7ae5f8776..422419b7821762e9b808a23e5946dc49bea14918 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 f1c9264138b4fe7aeae618169dc7201ac1461f16..2d3942250cf43a294a5e599aa2f5e28e454a0960 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 46a7a4fba84f5c81799d6e7330359b4e2ad7a52f..02f93f7e2f73fdd48847a3e19822619a05d3c4a9 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); } } }