EarlyVisionConverters.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::Helpers
* @author     Kai Welke ( welke at kit dot edu)
* @date       2012
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "EarlyVisionConverters.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace memoryx
{
    Gaussian EarlyVisionConverters::convertToGaussian(const NormalDistributionBasePtr& normalDistribution)
    {
        if (!normalDistribution)
        {
            ARMARX_WARNING_S << "normalDistribution is NULL";
            return Gaussian();
        }
 
        int dimensions = normalDistribution->getDimensions();
        Gaussian result(dimensions);
 
        Eigen::MatrixXd cov(dimensions, dimensions);
        Eigen::VectorXd mean(dimensions);
 
        for (int r = 0 ; r < dimensions ; r++)
        {
            mean(r) = normalDistribution->getMean()[r];
 
            for (int c = 0 ; c < dimensions ; c++)
            {
                cov(r, c) = normalDistribution->getCovariance(r, c);
            }
        }
 
        result.setCovariance(cov);
        result.setMean(mean);
 
        return result;
    }
 
    MultivariateNormalDistributionPtr EarlyVisionConverters::convertToMemoryX_MULTI(const Gaussian& gaussian)
    {
        int dimensions = gaussian.getDimensions();
        MultivariateNormalDistributionPtr result = new MultivariateNormalDistribution(dimensions);
 
        FloatVector mean;
 
        for (int r = 0 ; r < dimensions ; r++)
        {
            Eigen::VectorXd meanV = gaussian.getMean();
            mean.push_back(meanV(r));
 
            for (int c = 0 ; c < dimensions ; c++)
            {
                Eigen::MatrixXd covV = gaussian.getCovariance();
                result->setCovariance(r, c, covV(r, c));
            }
        }
 
        result->setMean(mean);
 
        return result;
    }
 
    IsotropicNormalDistributionPtr EarlyVisionConverters::convertToMemoryX_ISO(const Gaussian& gaussian)
    {
        int dimensions = gaussian.getDimensions();
        IsotropicNormalDistributionPtr result = new IsotropicNormalDistribution(dimensions);
 
        FloatVector mean;
 
        for (int d = 0 ; d < dimensions ; d++)
        {
            mean.push_back(gaussian.getMean()(d));
            result->setVariance(d, gaussian.getCovariance()(d, d));
        }
 
        result->setMean(mean);
 
        return result;
    }
 
    UnivariateNormalDistributionPtr EarlyVisionConverters::convertToMemoryX_UNI(const Gaussian& gaussian)
    {
        UnivariateNormalDistributionPtr result = new UnivariateNormalDistribution(gaussian.getMean()(0), gaussian.getCovariance()(0, 0));
 
        return result;
    }
}