EqualityFilter.cpp

Go to the documentation of this file.

#include "EqualityFilter.h"
 
#include <list>
 
#include <IceUtil/Time.h>
 
#include "RobotAPI/libraries/aron/core/data/variant/complex/NDArray.h"
#include "RobotAPI/libraries/aron/core/data/variant/primitive/Float.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();
 
        int num_instances = 0;
        std::vector<armarx::aron::data::NDArrayPtr> images_snapshot;
        std::vector<armarx::aron::data::FloatPtr> floats_snapshot;
        std::vector<float> distances;
 
        e.forEachInstance(
            [&num_instances, &images_snapshot, &floats_snapshot](armem::wm::EntityInstance& i)
            {
                auto data = aron::data::Dict::DynamicCastAndCheck(i.data());
                for (auto key : data->getAllKeys())
                {
                    aron::data::Descriptor img_desc;
                    try
                    {
                        auto d = data->at(key);
                        ;
                        img_desc = data->at(key)->getDescriptor();
                    }
                    catch (...)
                    {
                        ARMARX_INFO << "Problem with accessing image description";
                        img_desc = aron::data::Descriptor::NDARRAY;
                    }
                    if (img_desc == aron::data::Descriptor::NDARRAY)
                    {
                        auto img_nd = aron::data::NDArray::DynamicCastAndCheck(data->at(key));
                        images_snapshot.insert(images_snapshot.end(), img_nd);
                        num_instances++;
                    }
                    else if (img_desc == aron::data::Descriptor::FLOAT)
                    {
                        auto fl = aron::data::Float::DynamicCastAndCheck(data->at(key));
                        floats_snapshot.push_back(fl);
                        num_instances++;
                    }
                    else
                    {
                        ARMARX_INFO << "data-type not yet supported. \n"
                                    << "Only ndarray and float data types are supported for "
                                       "equality filters yet.";
                    }
                }
            });
 
        if (images.size() < 2)
        {
            ARMARX_INFO << "Adding first images, because nothing to compare";
            images.push_back(images_snapshot);
            this->stats.accepted += 1;
            auto end = std::chrono::high_resolution_clock::now();
            stats.end_time = end;
            stats.additional_time += (end - start);
            return true;
        }
        else if (images.size() < max_images)
        {
            ARMARX_INFO << "Not enough elements yet to do full comparison of last " << max_images
                        << " elements";
            images.push_back(images_snapshot);
            this->stats.accepted += 1;
            auto end = std::chrono::high_resolution_clock::now();
            stats.end_time = end;
            stats.additional_time += (end - start);
            return true;
        }
 
 
        std::vector<armarx::aron::data::NDArrayPtr> lastCommittedImages;
        int sizeOfCommited = 0;
        for (int i = 0; i < max_images; i++)
        {
            std::vector<armarx::aron::data::NDArrayPtr> lastCommitImages =
                images.at(images.size() - i - 1);
            sizeOfCommited = lastCommitImages.size();
            for (int j = 0; j < lastCommitImages.size(); j++)
            {
                lastCommittedImages.push_back(lastCommitImages.at(j));
            }
            // we just concatenate all images (instances)
        }
 
        ARMARX_CHECK(sizeOfCommited == images_snapshot.size()); //make sure we have enough instances
 
        for (int i = 0; i < images_snapshot.size(); i++)
        {
            armarx::aron::data::NDArrayPtr new_image = images_snapshot.at(i);
            std::vector<armarx::aron::data::NDArrayPtr> commited_images;
            for (int j = 0; j < max_images; j++)
            {
                int index = i + 2 * j;
                auto image = lastCommittedImages.at(index);
                commited_images.emplace_back(image);
            }
 
            float distance = aron::similarity::NDArraySimilarity::calculate_similarity_multi(
                commited_images, new_image, this->similarity_type);
 
            distances.insert(distances.end(), distance);
        }
 
        //check for criterion:
        float sum_distances = 0;
        float max_distance = 0;
        for (auto d : distances)
        {
            sum_distances += d;
            if (d > max_distance)
            {
                max_distance = d;
            }
        }
 
        bool max =
            true; //set true if only maximum distance value is important and false if sum of distances is important
        bool accept = false;
 
        if (max)
        {
            accept = (max_distance > this->threshold);
        }
        else
        {
            accept = (sum_distances > this->threshold);
        }
 
        if (accept)
        {
            images.pop_front(); //delete first element
            images.push_back(images_snapshot);
            this->stats.accepted += 1;
            auto end = std::chrono::high_resolution_clock::now();
            stats.additional_time += (end - start);
            stats.end_time = end;
            return true;
        }
        else
        {
            this->stats.rejected += 1;
            auto end = std::chrono::high_resolution_clock::now();
            stats.additional_time += (end - start);
            stats.end_time = end;
            return false;
        }
    }
 
    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 += "Threshold-Parameter: ";
            stats.additional_info += std::to_string(threshold);
        }
        if (json.find(PARAM_SIM_MEASURE) != json.end())
        {
            std::string type_string = json.at(PARAM_SIM_MEASURE);
            if (type_string == "MSE")
            {
                this->similarity_type = aron::similarity::NDArraySimilarity::Type::MSE;
                this->float_similarity_type = aron::similarity::FloatSimilarity::Type::MSE;
                stats.similarity_type = aron::similarity::NDArraySimilarity::Type::MSE;
            }
            else if (type_string == "MAE")
            {
                this->similarity_type = aron::similarity::NDArraySimilarity::Type::MAE;
                this->float_similarity_type = aron::similarity::FloatSimilarity::Type::MAE;
                stats.similarity_type = aron::similarity::NDArraySimilarity::Type::MAE;
            }
            else if (type_string == "Chernoff")
            {
                this->similarity_type = aron::similarity::NDArraySimilarity::Type::CHERNOFF;
                this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
                stats.similarity_type = aron::similarity::NDArraySimilarity::Type::CHERNOFF;
            }
            else if (type_string == "Cosine")
            {
                this->similarity_type = aron::similarity::NDArraySimilarity::Type::COSINE;
                this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
                stats.similarity_type = aron::similarity::NDArraySimilarity::Type::COSINE;
            }
            else
            {
                ARMARX_WARNING << "Undefined similarity measure detected in JSON file";
                stats.similarity_type = aron::similarity::NDArraySimilarity::Type::NONE;
                this->float_similarity_type = aron::similarity::FloatSimilarity::Type::NONE;
            }
            ARMARX_INFO << VAROUT(this->similarity_type);
        }
        if (json.find(PARAM_MAX_OBJECTS) != json.end())
        {
            max_images = json.at(PARAM_MAX_OBJECTS);
            ARMARX_INFO << VAROUT(max_images);
            stats.number_of_compared_objects = max_images;
        }
 
        stats.start_time = std::chrono::high_resolution_clock::now();
    }
 
    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