GMMReducer.cpp

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
*/
 
#include "GMMReducer.h"
 
#include <cfloat>
 
// Eigen
#include <Eigen/Eigenvalues>
 
namespace memoryx
{
    GaussianMixtureDistributionBasePtr GMMReducer::reduceByComponentCount(const GaussianMixtureDistributionBasePtr& fullGMM, int countComp)
    {
        GaussianMixtureDistributionPtr reducedGMM = GaussianMixtureDistributionPtr::dynamicCast(fullGMM->clone());
 
        if (!reducedGMM || countComp < 1)
        {
            return GaussianMixtureDistributionPtr();
        }
 
        reducedGMM->normalize();
 
        const int FULL_GMM_SIZE = fullGMM->size();
 
        while (reducedGMM->size() > countComp)
        {
            float minCost = FLT_MAX;
            int mergeIndexes = -1;
 
            for (int i = 0; i < reducedGMM->size(); ++i)
                for (int j = i + 1; j < reducedGMM->size(); ++j)
                {
                    const float cost = getMergingCost(reducedGMM, i, j);
 
                    if (cost < minCost)
                    {
                        minCost = cost;
                        mergeIndexes = i * FULL_GMM_SIZE + j;
                    }
                }
 
            if (mergeIndexes > 0)
            {
                mergeGMMComponents(reducedGMM, mergeIndexes / FULL_GMM_SIZE, mergeIndexes % FULL_GMM_SIZE);
            }
        }
 
        return reducedGMM;
    }
 
    GaussianMixtureDistributionBasePtr GMMReducer::reduceByMaxAABB(const GaussianMixtureDistributionBasePtr& fullGMM, float maxSideLength)
    {
        return reduceByMaxDeviation(fullGMM, maxSideLength, eAABB);
    }
 
    GaussianMixtureDistributionBasePtr GMMReducer::reduceByMaxOrientedBBox(const GaussianMixtureDistributionBasePtr& fullGMM, float maxSideLength)
    {
        return reduceByMaxDeviation(fullGMM, maxSideLength, eOrientedBBox);
    }
 
    GaussianMixtureDistributionBasePtr GMMReducer::reduceByMaxEqualSphere(const GaussianMixtureDistributionBasePtr& fullGMM, float maxSphereRadius)
    {
        return reduceByMaxDeviation(fullGMM, maxSphereRadius, eEqualSphere);
    }
 
    GaussianMixtureDistributionBasePtr GMMReducer::reduceByMaxDeviation(const GaussianMixtureDistributionBasePtr& fullGMM,
            float maxDeviation, DeviationMeasure devMeasure)
    {
        GaussianMixtureDistributionPtr reducedGMM = GaussianMixtureDistributionPtr::dynamicCast(fullGMM->clone());
 
        if (!reducedGMM || devMeasure < 0)
        {
            return GaussianMixtureDistributionPtr();
        }
 
        // calculate measure-specific variance values in advance to save computation in loop
        float precalcDeviation = 0.f;
 
        switch (devMeasure)
        {
            case eAABB:
            case eOrientedBBox:
                precalcDeviation = maxDeviation * maxDeviation;
                break;
 
            case eEqualSphere:
                precalcDeviation = maxDeviation * maxDeviation * maxDeviation;
                break;
 
            default:
                precalcDeviation = 0.f;
        }
 
        reducedGMM->normalize();
 
        bool merged = false;
 
        struct PairDistanceComparator
        {
            static bool compare(GMMCompPairDistance i, GMMCompPairDistance j)
            {
                return (i.second < j.second);
            }
        };
 
        do
        {
            const int REDUCED_GMM_SIZE = reducedGMM->size();
            GMMCompPairDistanceVector distVector;
            distVector.reserve((REDUCED_GMM_SIZE - 1 * REDUCED_GMM_SIZE) / 2);
            fillMergingCostVector(reducedGMM, distVector);
 
            // sort distVector
            std::sort(distVector.begin(), distVector.end(), PairDistanceComparator::compare);
 
            // TODO change order? check devMeasure BEFORE calc distance -> should be faster
            merged = false;
 
            for (GMMCompPairDistanceVector::const_iterator it = distVector.begin(); it != distVector.end(); ++it)
            {
                const int index1 = it->first / REDUCED_GMM_SIZE;
                const int index2 = it->first % REDUCED_GMM_SIZE;
                GaussianMixtureComponent mergedComp;
                mergeGMMComponents(reducedGMM->getComponent(index1), reducedGMM->getComponent(index2), mergedComp);
                bool doMerge = false;
                Eigen::Matrix3f cov = NormalDistributionPtr::dynamicCast(mergedComp.gaussian)->toEigenCovariance();
 
                switch (devMeasure)
                {
                    case eAABB:
                    {
                        doMerge = (cov(0, 0) < precalcDeviation &&
                                   cov(1, 1) < precalcDeviation &&
                                   cov(2, 2) < precalcDeviation);
                        break;
                    }
 
                    case eOrientedBBox:
                    {
                        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> es(cov);
                        Eigen::Vector3f evals = es.eigenvalues();
                        doMerge = (evals(0) < precalcDeviation &&
                                   evals(1) < precalcDeviation &&
                                   evals(2) < precalcDeviation);
                        break;
                    }
 
                    case eEqualSphere:
                    {
                        doMerge = sqrtf(cov.determinant()) < precalcDeviation;
                        break;
                    }
 
                    default:
                        doMerge = false;
                }
 
                if (doMerge)
                {
                    std::cout << "Adding component: " << mergedComp.gaussian->output() << " (instead of " << index1 << " and " << index2 << ")"  << std::endl;
                    replaceComponentsWithMerged(reducedGMM, index1, index2, mergedComp);
                    merged = true;
                    break;
                }
            }
        }
        while (merged);
 
        return reducedGMM;
    }
 
    void GMMReducer::mergeGMMComponents(const GaussianMixtureComponent& comp1, const GaussianMixtureComponent& comp2, GaussianMixtureComponent& mergedComp)
    {
        NormalDistributionPtr gaussian1 = NormalDistributionPtr::dynamicCast(comp1.gaussian);
        NormalDistributionPtr gaussian2 = NormalDistributionPtr::dynamicCast(comp2.gaussian);
 
        const Eigen::VectorXf x1 = gaussian1->toEigenMean();
        const Eigen::VectorXf x2 = gaussian2->toEigenMean();
        const Eigen::MatrixXf p1 = gaussian1->toEigenCovariance();
        const Eigen::MatrixXf p2 = gaussian2->toEigenCovariance();
 
        const float w1 = comp1.weight;
        const float w2 = comp2.weight;
 
        const float k =  1. / (w1 + w2);
        const Eigen::VectorXf d =  x1 - x2;
 
        const Eigen::VectorXf mean = k * (w1 * x1 + w2 * x2);
        const Eigen::MatrixXf cov = k * (w1 * p1 + w2 * p2 + k * w1 * w2 * d * d.transpose());
 
        mergedComp.gaussian = new MultivariateNormalDistribution(mean, cov);
        mergedComp.weight = w1 + w2;
    }
 
    void GMMReducer::replaceComponentsWithMerged(GaussianMixtureDistributionPtr& gmm, int index1, int index2, const GaussianMixtureComponent& mergedComp)
    {
        gmm->setComponent(index1, mergedComp);
        gmm->removeComponent(index2);
    }
 
    void GMMReducer::mergeGMMComponents(GaussianMixtureDistributionPtr& gmm, int index1, int index2)
    {
        GaussianMixtureComponent comp1 = gmm->getComponent(index1);
        GaussianMixtureComponent comp2 = gmm->getComponent(index2);
 
        GaussianMixtureComponent mergedComp;
        mergeGMMComponents(comp1, comp2, mergedComp);
        replaceComponentsWithMerged(gmm, index1, index2, mergedComp);
    }
 
    float GMMReducer::getMergingCost(const GaussianMixtureDistributionBasePtr& gmm, int c1, int c2)
    {
        return gmmDistance->getDistance(gmm->getComponent(c1), gmm->getComponent(c2));
    }
 
    void GMMReducer::fillMergingCostVector(const GaussianMixtureDistributionBasePtr& gmm, GMMCompPairDistanceVector& costVec)
    {
        const int GMM_SIZE = gmm->size();
 
        for (int i = 0; i < GMM_SIZE - 1; ++i)
            for (int j = i + 1; j < GMM_SIZE; ++j)
            {
                const float cost = getMergingCost(gmm, i, j);
                const int index = i * GMM_SIZE + j;
                costVec.push_back(GMMCompPairDistance(index, cost));
            }
    }
}