added cosine classes for aron/similarity

source/RobotAPI/libraries/aron/similarity/cosine.cpp

+#include "cosine.h"
+
+#include <SimoxUtility/algorithm/string.h>
+#include <cmath>
+
+namespace armarx::aron::similarity
+{
+    double
+    cosine::compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2)
+    {
+        //this does only work for RGB images not RGBDepth at the moment!
+        int width = p1->getShape().at(0);
+        int height = p1->getShape().at(1);
+        int dimensions = p1->getShape().at(2);
+        ARMARX_CHECK(dimensions == p2->getShape().at(2));
+        if(dimensions > 3){
+            ARMARX_INFO << "Trying to calculate cosine similarity for more than 3 channels, only first three channels will be looked at";
+        }
+        Eigen::MatrixXf m1r(width, height);
+        Eigen::MatrixXf m2r(width, height);
+        Eigen::MatrixXf m1g(width, height);
+        Eigen::MatrixXf m2g(width, height);
+        Eigen::MatrixXf m1b(width, height);
+        Eigen::MatrixXf m2b(width, height);
+
+        auto image1 = p1->getDataAsVector();
+        auto image2 = p2->getDataAsVector();
+
+
+        for(int x = 0; x < width; x++){
+            for(int y = 0; y < height; y++){
+                //R-value matices:
+                int index = x + width * (y + 0 * height);
+                double element1 = image1.at(index);
+                double element2 = image2.at(index);
+                m1r(x, y) = element1;
+                m2r(x, y) = element2;
+                //G-value matices:
+                index = x + width * (y + 1 * height);
+                element1 = image1.at(index);
+                element2 = image2.at(index);
+                m1g(x, y) = element1;
+                m2g(x, y) = element2;
+                //B-value matices:
+                index = x + width * (y + 2 * height);
+                element1 = image1.at(index);
+                element2 = image2.at(index);
+                m1b(x, y) = element1;
+                m2b(x, y) = element2;
+            }
+        }
+
+
+        double dotProductR = (m1r.array() * m2r.array()).sum();
+        double normProductR = m1r.norm() * m2r.norm();
+        double cosineR = 1.0 - (dotProductR / normProductR);
+        //ARMARX_INFO << VAROUT(cosineR);
+
+        double dotProductG = (m1g.array() * m2g.array()).sum();
+        double normProductG = m1g.norm() * m2g.norm();
+        double cosineG = 1.0 - (dotProductG / normProductG);
+        //ARMARX_INFO << VAROUT(cosineG);
+
+        double dotProductB = (m1b.array() * m2b.array()).sum();
+        double normProductB = m1b.norm() * m2b.norm();
+        double cosineB = 1.0 - (dotProductB / normProductB);
+        //ARMARX_INFO << VAROUT(cosineB);
+
+        return (cosineR + cosineG + cosineB)/ 3;
+    }
+
+}

source/RobotAPI/libraries/aron/similarity/cosine.h

+#pragma once
+
+#include <algorithm>
+#include <map>
+#include <vector>
+
+#include <RobotAPI/libraries/aron/core/data/variant/All.h>
+
+namespace armarx::aron::similarity::cosine
+{
+    double compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2);
+
+}