chernoff.cpp

Go to the documentation of this file.

#include "chernoff.h"
 
#include <cmath>
 
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::aron::similarity::chernoff
{
 
    double
    compute_similarity(const aron::data::NDArrayPtr p1, const aron::data::NDArrayPtr p2)
    {
        //TODO: there seems to be an error that leads to the mean vectors always having the same
        //value, which leads to the diff being zero which leads to distance being 0/inf/nan
        //aron::data::NDArray image1 = *p1;
        //aron::data::NDArray image2 = *p2;
 
        //then normalize them to have values between 0 and 1
        //TODO: do not rturn new array but change existing one
        auto f = normalize_ndarray(p1, 255);
        auto s = normalize_ndarray(p2, 255);
        //ARMARX_INFO << VAROUT(f);
        //ARMARX_INFO << VAROUT(s);
        //then calculate the mean vectors
        std::vector<double> mean_one = calculate_mean_values(f);
        std::vector<double> mean_two = calculate_mean_values(s);
        //ARMARX_INFO << VAROUT(mean_one);
        //then calculate the covariance matrices
        std::vector<std::vector<double>> cov_one = calculate_covariance_matrix(f, mean_one);
        std::vector<std::vector<double>> cov_two = calculate_covariance_matrix(s, mean_two);
        //then calculate the Bhattacharyya distance and return
        double distance = calculate_bhattacharyya_distance(mean_one, mean_two, cov_one, cov_two);
        ARMARX_INFO << "Chernoff distance: " << std::to_string(distance);
        return distance;
    }
 
    std::vector<double>
    calculate_mean_values(data::NDArray array)
    {
        std::vector<double> mean(3, 0.0);
        int width = array.getShape().at(0);
        int height = array.getShape().at(1);
        int colors = array.getShape().at(2);
        auto data = array.getDataAsVector();
 
        for (int row = 0; row < height; row++)
        {
            for (int col = 0; col < width; col++)
            {
                double red = data.at(row * width * colors + col * colors);
                double green = data.at(row * width * colors + col * colors + 1);
                double blue = data.at(row * width * colors + col * colors + 2);
                mean[0] += red;
                mean[1] += green;
                mean[2] += blue;
            }
        }
        //ARMARX_INFO << VAROUT(mean);
        int numPixels = width * height; //only per color
        mean[0] /= numPixels;
        mean[1] /= numPixels;
        mean[2] /= numPixels;
 
        return mean;
    }
 
    data::NDArray
    normalize_ndarray(data::NDArrayPtr array, int j)
    {
        std::vector<unsigned char> data = array->getDataAsVector();
        std::vector<unsigned char> new_data(data.size());
 
        for (int i = 0; i < data.size(); i++)
        {
            double a = data.at(i);
            double n = a / j;
            new_data.at(i) = n;
        }
 
        armarx::aron::data::NDArray n(
            array->getShape(), array->getType(), new_data, array->getPath());
        return n;
    }
 
    std::vector<std::vector<double>>
    calculate_covariance_matrix(data::NDArray array,
                                std::vector<double> mean = std::vector<double>())
    {
        int width = array.getShape().at(0);
        int height = array.getShape().at(1);
        int colors = array.getShape().at(2);
        auto data = array.getDataAsVector();
 
        std::vector<double> mean_vec(3, 0.0);
        if (mean.empty())
        {
            mean_vec = calculate_mean_values(array);
        }
        else
        {
            mean_vec = mean;
        }
 
        int numPixels = width * height; //only per color
 
        std::vector<std::vector<double>> covariance(3, std::vector<double>(3, 0.0));
 
        for (int row = 0; row < height; row++)
        {
            for (int col = 0; col < width; col++)
            {
                double red = data.at(row * width * colors + col * colors);
                double green = data.at(row * width * colors + col * colors + 1);
                double blue = data.at(row * width * colors + col * colors + 2);
 
                double redDeviation = red - mean[0];
                double greenDeviation = green - mean[1];
                double blueDeviation = blue - mean[2];
 
                covariance[0][0] += redDeviation * redDeviation;
                covariance[0][1] += redDeviation * greenDeviation;
                covariance[0][2] += redDeviation * blueDeviation;
                covariance[1][1] += greenDeviation * greenDeviation;
                covariance[1][2] += greenDeviation * blueDeviation;
                covariance[2][2] += blueDeviation * blueDeviation;
            }
        }
 
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < i; j++)
            {
                covariance[i][j] /= numPixels;
                covariance[j][i] = covariance[i][j];
            }
            covariance[i][i] /= numPixels;
        }
 
        return covariance;
    }
 
    double
    calculate_bhattacharyya_distance(std::vector<double> mean_one,
                                     std::vector<double> mean_two,
                                     std::vector<std::vector<double>> covariance_one,
                                     std::vector<std::vector<double>> covariance_two)
    {
        //first make mean vectors and covariance matrices in Eigen:: objects
        Eigen::VectorXd meanOne(3);
        meanOne << mean_one[0], mean_one[1], mean_one[2];
        Eigen::VectorXd meanTwo(3);
        meanTwo << mean_two[0], mean_two[1], mean_two[2];
 
        //ARMARX_INFO << VAROUT(meanOne);
        //ARMARX_INFO << VAROUT(meanTwo);
 
        //these are column-major instead or row-major, but we have a quadratic, symmetric matrix, so it does not matter
        //this conversion does not work correctly!!
        //Eigen::MatrixXd cov_one = Eigen::Map<Eigen::MatrixXd>(covariance_one[0].data(), covariance_one.size(), covariance_one[0].size());
        //Eigen::MatrixXd cov_two = Eigen::Map<Eigen::MatrixXd>(covariance_two[0].data(), covariance_two.size(), covariance_two[0].size());
 
        Eigen::Matrix3d cov_one;
        cov_one << covariance_one[0][0], covariance_one[0][1], covariance_one[0][2],
            covariance_one[1][0], covariance_one[1][1], covariance_one[1][2], covariance_one[2][0],
            covariance_one[2][1], covariance_one[2][2];
 
        Eigen::Matrix3d cov_two;
        cov_two << covariance_two[0][0], covariance_two[0][1], covariance_two[0][2],
            covariance_two[1][0], covariance_two[1][1], covariance_two[1][2], covariance_two[2][0],
            covariance_two[2][1], covariance_two[2][2];
 
 
        //ARMARX_INFO << VAROUT(cov_one);
 
        Eigen::VectorXd meanDiff = meanOne - meanTwo;
        //ARMARX_INFO << VAROUT(meanDiff);
 
        Eigen::MatrixXd sigma = 0.5 * (cov_one + cov_two);
        Eigen::MatrixXd sigma_inverse = sigma.inverse();
 
        double det_cov_one = cov_one.determinant();
        double det_cov_two = cov_two.determinant();
        double det_sigma = sigma.determinant();
 
        //formula from https://www.kaggle.com/code/debanga/statistical-distances
        double bigger = meanDiff.transpose() * (sigma_inverse)*meanDiff;
        //ARMARX_INFO << "Bigger: " << std::to_string(bigger);
        double term_one = 0.125 * bigger;
        double sqr = std::sqrt(det_cov_one * det_cov_two);
        //ARMARX_INFO << "Sqr: " << std::to_string(sqr);
        double before_log = det_sigma / (sqr);
        //ARMARX_INFO << "Before log: " << std::to_string(before_log);
        double term_two = 0.5 * std::log(before_log);
 
        double distance = term_one + term_two;
 
        return distance;
    }
 
 
} // namespace armarx::aron::similarity::chernoff