Decay.cpp

Go to the documentation of this file.

#include "Decay.h"
 
#include <SimoxUtility/math/scale_value.h>
 
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/core/time/DateTime.h>
 
 
namespace armarx::armem::server::obj::instance
{
 
    void Decay::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(delaySeconds, prefix + "delaySeconds",
                       "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.");
        defs->optional(removeObjectsBelowConfidence, prefix + "removeObjectsBelowConfidence",
                       "Remove objects whose confidence is lower than this value.");
    }
 
    void Decay::updateConfidence(objpose::ObjectPose& pose, const DateTime& now) const
    {
        if (pose.attachment or pose.isStatic)
        {
            pose.confidence = 1.0;
        }
        else
        {
            pose.confidence = calculateConfidence(pose.timestamp, now);
        }
    }
 
    void Decay::updateConfidences(objpose::ObjectPoseSeq& objectPoses, const DateTime& now) const
    {
        for (objpose::ObjectPose& pose : objectPoses)
        {
            updateConfidence(pose, now);
        }
    }
 
    float Decay::calculateConfidence(const DateTime& localization, const DateTime& now) const
    {
        const float duration = static_cast<float>((now - localization).toSecondsDouble());
        if (duration < delaySeconds)
        {
            return maxConfidence;
        }
        else if (duration > delaySeconds + this->durationSeconds)
        {
            return minConfidence;
        }
        else
        {
            return simox::math::scale_value_from_to(
                       duration,
                       delaySeconds, delaySeconds + this->durationSeconds,
                       maxConfidence, minConfidence);
        }
    }
 
 
 
    void Decay::RemoteGui::setup(const Decay& decay)
    {
        using namespace armarx::RemoteGui::Client;
 
        enabled.setValue(decay.enabled);
        {
            float max = 1e6;
            delaySeconds.setRange(0, max);
            delaySeconds.setDecimals(2);
            delaySeconds.setSteps(int(10 * max));  // = 0.1 steps
            delaySeconds.setValue(decay.delaySeconds);
        }
        {
            float max = 1e6;
            durationSeconds.setRange(0, max);
            durationSeconds.setDecimals(2);
            durationSeconds.setSteps(int(10 * max));  // = 0.1 steps
            durationSeconds.setValue(decay.durationSeconds);
        }
        {
            maxConfidence.setRange(0, 1);
            maxConfidence.setSteps(100);
            maxConfidence.setValue(decay.maxConfidence);
        }
        {
            minConfidence.setRange(0, 1);
            minConfidence.setSteps(100);
            minConfidence.setValue(decay.minConfidence);
        }
        {
            removeObjectsBelowConfidence.setRange(0, 1);
            removeObjectsBelowConfidence.setSteps(100);
            removeObjectsBelowConfidence.setValue(decay.removeObjectsBelowConfidence);
        }
 
        GridLayout grid;
        int row = 0;
        grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
        row++;
        grid.add(Label("Delay [sec]"), {row, 0}).add(delaySeconds, {row, 1});
        row++;
        grid.add(Label("Duration [sec]"), {row, 0}).add(durationSeconds, {row, 1});
        row++;
        grid.add(Label("Max Confidence"), {row, 0}).add(maxConfidence, {row, 1});
        row++;
        grid.add(Label("Min Confidence"), {row, 0}).add(minConfidence, {row, 1});
        row++;
        grid.add(Label("Remove Objects with Confidence < "), {row, 0}).add(removeObjectsBelowConfidence, {row, 1});
        row++;
 
        group.setLabel("Decay");
        group.addChild(grid);
    }
 
    void Decay::RemoteGui::update(Decay& decay)
    {
        decay.enabled = enabled.getValue();
        decay.delaySeconds = delaySeconds.getValue();
        decay.durationSeconds = durationSeconds.getValue();
        decay.maxConfidence = maxConfidence.getValue();
        decay.minConfidence = minConfidence.getValue();
        decay.removeObjectsBelowConfidence = removeObjectsBelowConfidence.getValue();
    }
 
}