A3Histogram.hpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include <vector>
#include <limits>
#include <algorithm>
#include <math.h>
 
#include "Feature.hpp"
#include "../point.hpp"
 
class A3Histogram : public Feature
{
public:
    A3Histogram(): MAX_SAMPLES(100000), MAX_TRIOS(100000) {}
    A3Histogram(const std::vector<Eigen::Vector3f>& points, int bins = 10)
        : MAX_SAMPLES(100000), MAX_TRIOS(100000)
    {
        m_name = "A3Histogram";
        m_a3Histogram = makeHistogram(bins, calculateAngles(points));
    }
 
    A3Histogram(const std::pair<std::string, std::vector<Eigen::Vector3f>>& points, int bins = 10)
        : MAX_SAMPLES(100000), MAX_TRIOS(100000)
    {
        m_name = "A3Histogram";
        m_a3Histogram = makeHistogram(bins, calculateAngles(points.second));
    }
 
 
    std::vector<double> a3Histogram() const
    {
        return m_a3Histogram;
    }
 
    FeaturePtr calculate(const Points& points)
    {
        return FeaturePtr(new A3Histogram(points));
    }
    FeaturePtr calculate(const TaggedPoints& points)
    {
        return FeaturePtr(new A3Histogram(points));
    }
 
    double compare(const Feature& other) const
    {
        const A3Histogram* casted = dynamic_cast<const A3Histogram*>(&other);
 
        if (casted)
        {
            return compareHistograms(m_a3Histogram, casted->a3Histogram());
        }
        else
        {
            return std::numeric_limits<double>::max();
        }
    }
 
    virtual void serialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&) const
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
        AbstractObjectSerializerPtr featureObj;
 
        //Check if object already has a "features" field, else create a new one
        if (obj->hasElement(FEATURE_FIELD_NAME))
        {
            featureObj = obj->getElement(FEATURE_FIELD_NAME);
            featureObj->setDoubleArray(m_name, m_a3Histogram);
        }
        else
        {
            featureObj = obj->createElement();
            featureObj->setDoubleArray(m_name, m_a3Histogram);
            obj->setElement(FEATURE_FIELD_NAME, featureObj);
        }
    }
 
    virtual void deserialize(const ObjectSerializerBasePtr& serializer, const Ice::Current&)
    {
        AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
        AbstractObjectSerializerPtr featureObj = obj->getElement(FEATURE_FIELD_NAME);
        //Now copy the result array from the DB
        featureObj->getDoubleArray(m_name, m_a3Histogram);
    }
 
    virtual std::ostream& output(std::ostream& out) const
    {
        out << "[";
 
        if (!m_a3Histogram.empty())
        {
            out << m_a3Histogram[0];
 
            for (unsigned int i = 1; i < m_a3Histogram.size(); i++)
            {
                out << ", " << m_a3Histogram[i];
            }
        }
 
        out << "]";
        return out;
    }
 
private:
    const int MAX_SAMPLES;
    const int MAX_TRIOS;
 
    std::vector<double> normalize(const std::vector<double>& x) const
    {
        std::vector<double> n(x.size());
        double max = *std::max_element(x.begin(), x.end());
 
        for (unsigned int i = 0; i < n.size(); i++)
        {
            n[i] = x[i] / max;
        }
 
        return n;
    }
 
    double compareHistograms(const std::vector<double>& a, const std::vector<double>& b) const
    {
        double diff = 0.0;
        std::vector<double> an = normalize(a);
        std::vector<double> bn = normalize(b);
 
        for (unsigned int i = 0; i < an.size(); i++)
        {
            diff += std::pow(an[i] - bn[i], 2);
        }
 
        return diff;
    }
 
    std::vector<double> calculateAngles(const std::vector<Eigen::Vector3f>& points) const
    {
        int numPoints = points.size();
        std::vector<int> indices = std::vector<int>(std::min(numPoints, MAX_SAMPLES));
 
        if (numPoints < MAX_SAMPLES)
        {
            //Take all the points
            for (unsigned int i = 0; i < indices.size(); i++)
            {
                indices[i] = i;
            }
        }
        else
        {
            //Sample enough points
            for (unsigned int i = 0; i < indices.size(); i++)
            {
                indices[i] = rand() % numPoints;
            }
        }
 
        std::vector<double> angles(MAX_TRIOS);
        int first;
        int second;
        int third;
        int numIndices = indices.size();
 
        //Now make the trios and calculate the angles
        for (int i = 0; i < MAX_TRIOS; i++)
        {
            first = rand() % numIndices;
 
            //Sample a `different` point
            do
            {
                second = rand() % numIndices;
            }
            while (first == second);
 
            do
            {
                third = rand() % numIndices;
            }
            while (first == third || second == third);
 
            //Calculate the angle
            angles[i] = angle(points[indices[first]], points[indices[second]], points[indices[third]]);
        }
 
        return angles;
    }
 
    std::vector<double> makeHistogram(const int bins, const std::vector<double>& values) const
    {
        std::vector<double> histogram(bins);
        double width = *std::max_element(values.begin(), values.end()) / (double) bins;
 
        for (unsigned int i = 0; i < values.size(); i++)
        {
            histogram[std::min((int)(values.at(i) / width), bins - 1)]++;
        }
 
        return histogram;
    }
    std::vector<double> m_a3Histogram;
};