MahalanobisDistance.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 <string>
#include <Eigen/LU>
 
#include <ArmarXCore/core/system/ImportExport.h>
 
#include <MemoryX/core/entity/ProbabilityMeasures.h>
#include "GMMDistance.h"
 
namespace memoryx
{
 
    /**
    * @class MahalanobisDistance
    * @ingroup CommonPlacesLearner
    */
    class ARMARXCORE_IMPORT_EXPORT MahalanobisDistance: public GMMDistance
    {
    public:
        /**
         * Creates a new GMMDistance
         */
        MahalanobisDistance(): GMMDistance()
        {
        }
 
        /**
         *
         *
         */
        float getDistance(const GaussianMixtureDistributionBasePtr& gmm1, const GaussianMixtureDistributionBasePtr& gmm2) override
        {
            return 0;  // NYI
        }
 
        float getDistance(const NormalDistributionPtr& g1, const NormalDistributionPtr& g2)
        {
            const Eigen::VectorXf d = g2->toEigenMean() - g1->toEigenMean();
            const Eigen::MatrixXf s = 2 * (g1->toEigenCovariance() + g2->toEigenCovariance());
            const Eigen::MatrixXf distm = d.transpose() * s.inverse() * d;
 
            return sqrtf(distm(0, 0));
        }
 
        float getWeigtedDistance(const GaussianMixtureDistributionBasePtr& gmm, const NormalDistributionBasePtr& g)
        {
            float result = 0;
 
            NormalDistributionPtr g1 = NormalDistributionPtr::dynamicCast(g);
 
            for (int i = 0; i < gmm->size(); ++i)
            {
                const GaussianMixtureComponent comp = gmm->getComponent(i);
                NormalDistributionPtr g2 = NormalDistributionPtr::dynamicCast(comp.gaussian);
 
                result += getDistance(g1, g2) * comp.weight;
            }
 
            return result;
        }
 
        float getDistance(const GaussianMixtureComponent& comp1, const GaussianMixtureComponent& comp2) override
        {
            NormalDistributionPtr gaussian1 = NormalDistributionPtr::dynamicCast(comp1.gaussian);
            NormalDistributionPtr gaussian2 = NormalDistributionPtr::dynamicCast(comp2.gaussian);
 
            Eigen::VectorXf d = gaussian2->toEigenMean() - gaussian1->toEigenMean();
            Eigen::MatrixXf s = 2 * (comp1.weight * gaussian1->toEigenCovariance() + comp2.weight * gaussian2->toEigenCovariance()) / (comp1.weight + comp2.weight);
            Eigen::MatrixXf distm = d.transpose() * s.inverse() * d;
 
            // TODO account for weights
            //            float w = (oldComp.weight + newComp.weight) / 2.;
 
            return sqrtf(distm(0, 0));
        }
 
    };
}