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)
    {
        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)
    {
        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()
    {
        return stats;
    }
 
    std::string SnapshotSimilarityFilter::getName()
    {
        return this->NAME;
    }
 
} // namespace armarx::armem::server::ltm::filter