ISDDistance.h

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    MemoryX::GaussianMixtureHelpers
* @author     Alexey Kozlov <kozlov@kit.edu>
* @copyright  2013 Alexey Kozlov
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#pragma once
 
#include <ArmarXCore/core/system/ImportExport.h>
 
#include <MemoryX/core/entity/ProbabilityMeasures.h>
#include "GMMDistance.h"
 
#include <Eigen/LU>
 
#include <memory>
#include <string>
 
namespace memoryx
{
 
    /**
    * @class ISDDistance
    * @ingroup CommonPlacesLearner
    */
    class ARMARXCORE_IMPORT_EXPORT ISDDistance: public GMMDistance
    {
    public:
        /**
         * Creates a new ISDDistance
         */
        ISDDistance(): GMMDistance()
        {
        }
 
        /**
         *
         *
         */
        float getDistance(const GaussianMixtureDistributionBasePtr& gmm1, const GaussianMixtureDistributionBasePtr& gmm2,
                          bool normalize)
        {
            // preprocessing
            std::vector<NormalDistributionPtr> g1, g2;
            std::vector<float> w1, w2;
            preprocessGMM(gmm1, g1, w1);
            preprocessGMM(gmm2, g2, w2);
 
            const float d11 = calcCrossSum(gmm1, g1, w1, gmm1, g1, w1);
            const float d22 = calcCrossSum(gmm2, g2, w2, gmm2, g2, w2);
            const float d12 = calcCrossSum(gmm1, g1, w1, gmm2, g2, w2);
 
            const float isd = d11 + d22 - 2 * d12;
            return normalize ? (isd / (d11 + d22)) : isd;
        }
 
        float getDistance(const GaussianMixtureDistributionBasePtr& gmm1, const GaussianMixtureDistributionBasePtr& gmm2) override
        {
            return getDistance(gmm1, gmm2, false);
        }
 
        float getDistance(const GaussianMixtureDistributionBasePtr& gmm1, const GaussianMixtureDistributionBasePtr& gmm2,
                          float d11)
        {
            // preprocessing
            std::vector<NormalDistributionPtr> g1, g2;
            std::vector<float> w1, w2;
            preprocessGMM(gmm1, g1, w1);
            preprocessGMM(gmm2, g2, w2);
 
            const float d22 = calcCrossSum(gmm2, g2, w2, gmm2, g2, w2);
            const float d12 = calcCrossSum(gmm1, g1, w1, gmm2, g2, w2);
 
            return d11 + d22 - 2 * d12;
        }
 
        float getDistance(const GaussianMixtureComponent& comp1, const GaussianMixtureComponent& comp2) override
        {
            return 0.; // NYI
        }
 
        float calcSelfLikeness(const GaussianMixtureDistributionBasePtr& gmm)
        {
            std::vector<NormalDistributionPtr> g;
            std::vector<float> w;
            preprocessGMM(gmm, g, w);
 
            return calcCrossSum(gmm, g, w, gmm, g, w);
        }
 
 
    private:
        void preprocessGMM(const GaussianMixtureDistributionBasePtr& gmm, std::vector<NormalDistributionPtr>& g,
                           std::vector<float>& w)
        {
 
            for (int i = 0; i < gmm->size(); ++i)
            {
                GaussianMixtureComponent c = gmm->getComponent(i);
                g.push_back(NormalDistributionPtr::dynamicCast(c.gaussian));
                w.push_back(c.weight);
            }
        }
 
        float calcCrossSum(const GaussianMixtureDistributionBasePtr& gmm1, std::vector<NormalDistributionPtr>& g1,
                           std::vector<float>& w1,
                           const GaussianMixtureDistributionBasePtr& gmm2, std::vector<NormalDistributionPtr>& g2,
                           std::vector<float>& w2)
        {
            float s = 0;
 
            const int GMM2_SIZE = gmm2->size();
 
            for (int i = 0; i < gmm1->size(); ++i)
            {
                const float wi = w1[i];
                const Eigen::Vector3f mi = g1[i]->toEigenMean();
                const Eigen::Matrix3f pi = g1[i]->toEigenCovariance();
 
                for (int j = 0; j < GMM2_SIZE; ++j)
                {
                    const float f = evaluate3DGaussian(mi, g2[j]->toEigenMean(), pi + g2[j]->toEigenCovariance());
                    s += wi * w2[j] * f;
                }
            }
 
            return s;
        }
 
        float evaluate3DGaussian(const Eigen::Vector3f& point, const Eigen::Vector3f& mean, const Eigen::Matrix3f& cov)
        {
            return evaluate3DGaussian(point, mean, cov.inverse(), cov.determinant());
        }
 
        float evaluate3DGaussian(const Eigen::Vector3f& point, const Eigen::Vector3f& mean, const Eigen::Matrix3f& covInv, float covDet)
        {
            const float inner = -0.5 * (point - mean).transpose() * covInv * (point - mean);
            const float e = expf(inner);
 
            static const float pm = powf(2 * M_PI, -1.5f);
            return pm * powf(covDet, -0.5) * e;
        }
 
    };
 
    using ISDDistancePtr = std::shared_ptr<ISDDistance>;
}