SimilarityFilter.cpp

Go to the documentation of this file.

#include "SimilarityFilter.h"
 
#include <algorithm>
#include <vector>
 
#include "RobotAPI/libraries/armem/server/ltm/EntitySnapshot.h"
 
#include "RobotAPI/libraries/aron/similarity/data/variant/NDArraySimilarity.h"
#include "RobotAPI/libraries/aron/similarity/data/variant/PrimitiveSimilarity.h"
#include "RobotAPI/libraries/aron/core/Descriptor.h"
#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
#include "RobotAPI/libraries/aron/core/data/variant/container/Dict.h"
#include "RobotAPI/libraries/aron/core/data/variant/container/List.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Bool.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Double.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Float.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Int.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Long.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/String.h"
 
namespace armarx::armem::server::ltm::processor::filter
{
    bool
    SnapshotSimilarityFilter::accept(const armem::wm::EntitySnapshot& e, bool simulatedVersion)
    {
        // Thread safety: lock the filter mutex for all mutable state access
        std::lock_guard<std::mutex> lock(filterMutex_);
 
        auto start = std::chrono::high_resolution_clock::now();
 
        const std::string entityId = e.id().getEntityID().str();
        auto& acceptedSnapshots = lastAcceptedSnapshots[entityId];
 
        if (acceptedSnapshots.empty())
        {
            acceptedSnapshots.push_back(e);
            stats.accepted++;
            auto end = std::chrono::high_resolution_clock::now();
            stats.end_time = end;
            stats.additional_time += (end - start);
            return true;
        }
 
        // Check if the new snapshot is similar to any of the last accepted snapshots
        bool foundSimilarSnapshot = false;
        for (const auto& oldSnapshot : acceptedSnapshots)
        {
            const float similarity = calculateSnapshotSimilarity(oldSnapshot, e);
            // ARMARX_INFO << "Snapshot similarity: " << similarity;
            if (similarity >= threshold)
            {
                foundSimilarSnapshot = true;
                break;
            }
        }
 
        if (foundSimilarSnapshot)
        {
            stats.rejected++;
        }
        else
        {
            acceptedSnapshots.push_back(e);
            if (acceptedSnapshots.size() > numSnapshots)
            {
                acceptedSnapshots.pop_front();
            }
            stats.accepted++;
        }
 
        auto end = std::chrono::high_resolution_clock::now();
        stats.end_time = end;
        stats.additional_time += (end - start);
 
        // if (!foundSimilarSnapshot) ARMARX_INFO << "ACCEPTED";
 
        return !foundSimilarSnapshot;
    }
 
    float
    SnapshotSimilarityFilter::calculateSnapshotSimilarity(
        const armem::wm::EntitySnapshot& oldSnapshot,
        const armem::wm::EntitySnapshot& newSnapshot)
    {
        std::vector<aron::data::VariantPtr> oldInstances;
        std::vector<aron::data::VariantPtr> newInstances;
 
        oldSnapshot.forEachInstance(
            [&oldInstances](armem::wm::EntityInstance& i)
            {
                oldInstances.push_back(i.data());
            }
        );
 
        newSnapshot.forEachInstance(
            [&newInstances](armem::wm::EntityInstance& i)
            {
                newInstances.push_back(i.data());
            }
        );
 
        if (oldInstances.size() != newInstances.size())
        {
            return 0.0f;
        }
 
        float weightedTotal = 0.0f;
        float totalImportance = 0.0f;
        for (size_t i = 0; i < newInstances.size(); i++)
        {
            const auto result =
                calculateInstanceSimilarity(oldInstances[i], newInstances[i], 1.0f);
            weightedTotal += result.similarity * result.importance;
            totalImportance += result.importance;
        }
 
        return totalImportance > 0.0f ? weightedTotal / totalImportance : 1.0f;
    }
 
    SnapshotSimilarityFilter::SimilarityResult
    SnapshotSimilarityFilter::calculateInstanceSimilarity(
        const aron::data::VariantPtr& oldData,
        const aron::data::VariantPtr& newData,
        float parentImportance)
    {
        if (!oldData && !newData)
        {
            return {1.0f, parentImportance};
        }
        if (!oldData || !newData)
        {
            return {0.0f, parentImportance};
        }
 
        const float importance = newData->getImportance().has_value() ?
            parentImportance * newData->getImportance().value() : parentImportance;
 
        // ignoring field if importance is 0
        if (importance <= 0.0f)
        {
            return {1.0f, 0.0f};
        }
 
        auto oldDesc = oldData->getDescriptor();
        auto newDesc = newData->getDescriptor();
        if (oldDesc != newDesc)
        {
            return {0.0f, importance};
        }
 
        // comparison can be extended for new types
        // recursive in case of containers, LIST is order-sensitive
        switch(newDesc)
        {
        case aron::data::Descriptor::INT:
        {
            auto oldInt = aron::data::Int::DynamicCastAndCheck(oldData);
            auto newInt = aron::data::Int::DynamicCastAndCheck(newData);
            return {
                aron::similarity::variant::primitive::calculateSimilarity(*oldInt, *newInt),
                importance};
        }
        case aron::data::Descriptor::FLOAT:
        {
            auto oldFloat = aron::data::Float::DynamicCastAndCheck(oldData);
            auto newFloat = aron::data::Float::DynamicCastAndCheck(newData);
            return {
                aron::similarity::variant::primitive::calculateSimilarity(*oldFloat, *newFloat),
                importance};
        }
        case aron::data::Descriptor::DOUBLE:
        {
            auto oldDouble = aron::data::Double::DynamicCastAndCheck(oldData);
            auto newDouble = aron::data::Double::DynamicCastAndCheck(newData);
            return {
                aron::similarity::variant::primitive::calculateSimilarity(*oldDouble, *newDouble),
                importance};
        }
        case aron::data::Descriptor::LONG:
        {
            auto oldLong = aron::data::Long::DynamicCastAndCheck(oldData);
            auto newLong = aron::data::Long::DynamicCastAndCheck(newData);
            return {
                aron::similarity::variant::primitive::calculateSimilarity(*oldLong, *newLong),
                importance};
        }
        case aron::data::Descriptor::STRING:
        {
            auto oldString = aron::data::String::DynamicCastAndCheck(oldData);
            auto newString = aron::data::String::DynamicCastAndCheck(newData);
            return {
                aron::similarity::variant::primitive::calculateSimilarity(*oldString, *newString),
                importance};
        }
        case aron::data::Descriptor::BOOL:
        {
            auto oldBool = aron::data::Bool::DynamicCastAndCheck(oldData);
            auto newBool = aron::data::Bool::DynamicCastAndCheck(newData);
            return {
                aron::similarity::variant::primitive::calculateSimilarity(*oldBool, *newBool),
                importance};
        }
        case aron::data::Descriptor::NDARRAY:
        {
            auto oldNdarr = aron::data::NDArray::DynamicCastAndCheck(oldData);
            auto newNdarr = aron::data::NDArray::DynamicCastAndCheck(newData);
            return {
                aron::similarity::variant::ndarray::calculateSimilarity(*oldNdarr, *newNdarr),
                importance};
        }
        case aron::data::Descriptor::LIST:
        {
            auto oldList = aron::data::List::DynamicCastAndCheck(oldData);
            auto newList = aron::data::List::DynamicCastAndCheck(newData);
            const auto& oldElems = oldList->getElements();
            const auto& newElems = newList->getElements();
 
            if (oldElems.size() != newElems.size())
            {
                return {0.0f, importance};
            }
 
            float weightedTotal = 0.0f;
            float totalImportance = 0.0f;
            for (size_t i = 0; i < newElems.size(); i++)
            {
                const auto result =
                    calculateInstanceSimilarity(oldElems[i], newElems[i], importance);
                weightedTotal += result.similarity * result.importance;
                totalImportance += result.importance;
            }
            const float similarity = totalImportance > 0.0f ? weightedTotal / totalImportance : 1.0f;
            return {similarity, importance};
        }
        case aron::data::Descriptor::DICT:
        {
            auto oldDict = aron::data::Dict::DynamicCastAndCheck(oldData);
            auto newDict = aron::data::Dict::DynamicCastAndCheck(newData);
            auto oldKeys = oldDict->getAllKeys();
            auto newKeys = newDict->getAllKeys();
 
            if (oldKeys.size() != newKeys.size())
            {
                return {0.0f, importance};
            }
 
            float weightedTotal = 0.0f;
            float totalImportance = 0.0f;
            for (const auto& key : newKeys)
            {
                if (!oldDict->hasElement(key))
                {
                    return {0.0f, importance};
                }
 
                const auto result =
                    calculateInstanceSimilarity(oldDict->at(key), newDict->at(key), importance);
                weightedTotal += result.similarity * result.importance;
                totalImportance += result.importance;
            }
            const float similarity = totalImportance > 0.0f ? weightedTotal / totalImportance : 1.0f;
            return {similarity, importance};
        }
        default:
        {
            ARMARX_INFO << "data-type not yet supported.";
            return {0.0f, importance};
        }
        }
    }
 
    void
    SnapshotSimilarityFilter::configure(const nlohmann::json& json)
    {
        std::lock_guard<std::mutex> lock(filterMutex_);
 
        if (json.find(PARAM_THRESHOLD) != json.end())
        {
            threshold = json.at(PARAM_THRESHOLD);
            // ARMARX_INFO << VAROUT(threshold);
            stats.additional_info += "Similarity threshold: ";
            stats.additional_info += std::to_string(threshold);
        }
 
        if (json.find(PARAM_NUM_SNAPSHOTS) != json.end())
        {
            numSnapshots = json.at(PARAM_NUM_SNAPSHOTS);
        }
 
        numSnapshots = std::max<std::size_t>(1, numSnapshots);
        stats.number_of_compared_objects = static_cast<int>(numSnapshots);
        stats.start_time = std::chrono::high_resolution_clock::now();
        // stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE;
    }
 
    SnapshotFilter::FilterStatistics
    SnapshotSimilarityFilter::getFilterStatistics()
    {
        std::lock_guard<std::mutex> lock(filterMutex_);
        return stats;
    }
 
    std::string
    SnapshotSimilarityFilter::getName()
    {
        return this->NAME;
    }
 
} // namespace armarx::armem::server::ltm::processor::filter