diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp index 6a317ab2763f03a2f2e06bdffcd69918d86c6c9f..3cb09a7b0359bf73b5b577c48bf5545573c20b21 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp @@ -58,6 +58,7 @@ namespace armarx visu.defineProperties(defs, "visu."); calibration.defineProperties(defs, "calibration."); robotHead.defineProperties(defs, "head."); + decay.defineProperties(defs, "decay."); return defs; } diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp index 8edfb4d696a36de07130d21f9b49e61d601700df..e7406a5e30ad69ee6c421b38ec66b30bc72c53f2 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.cpp @@ -8,6 +8,20 @@ namespace armarx::objpose { + void ObjectPoseDecay::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix) + { + defs->optional(enabled, prefix + "enabled", + "If true, object poses decay over time when not localized anymore."); + defs->optional(noDecaySeconds, prefix + "noDecaySeconds", + "Duration after latest localization before decay starts."); + defs->optional(durationSeconds, prefix + "durationSeconds", + "How long to reach minimal confidence."); + defs->optional(maxConfidence, prefix + "maxConfidence", + "Confidence when decay starts."); + defs->optional(minConfidence, prefix + "minConfidence", + "Confidence after decay duration."); + } + void ObjectPoseDecay::updateConfidence(ObjectPose& pose, IceUtil::Time now) { float confidence = calculateConfidence(pose.timestamp, now); @@ -30,7 +44,7 @@ namespace armarx::objpose return simox::math::scale_value_from_to( duration, noDecaySeconds, noDecaySeconds + this->durationSeconds, - minConfidence, maxConfidence); + maxConfidence, minConfidence); } } diff --git a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h index 85d564634d84b18bebca0e6e68d7d8166b9661ad..b55824a7cbfbf230d80a55dac74b924ba595106c 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h +++ b/source/RobotAPI/components/ObjectPoseObserver/detail/ObjectPoseDecay.h @@ -19,6 +19,8 @@ namespace armarx::objpose { public: + void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "decay."); + void updateConfidence(ObjectPose& pose, IceUtil::Time now); private: @@ -28,7 +30,9 @@ namespace armarx::objpose public: - /// Duration after latest localization when decay starts. + bool enabled = false; + + /// Duration after latest localization before decay starts. float noDecaySeconds = 5.0; /// How long to reach minConfidence. float durationSeconds = 20.0;