ObjectShapeClassification.cpp

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
 */
 
#include <Math/Math3d.h>
 
#include "ObjectShapeClassification.h"
 
#include <sstream>
#include <iterator> //Needed for vector-to-string. CPP is ugly
#include <iostream>
#include <stdio.h>
#include <boost/filesystem.hpp>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
 
#include "Features/Features.h"
#include "Features/FeatureCalculator.hpp"
#include "convexHull.hpp"
#include "IVPointParser.hpp"
#include "ICP.h"
 
 
using std::cout;
using std::endl;
 
const std::string ATTR_FEATURES = "RecognitionFeatures";
const std::string ATTR_IV_FILE = "IvFile";
 
using namespace visionx;
using namespace memoryx;
 
void ObjectShapeClassification::onInitComponent()
{
    settings_priorMemory = getProperty<std::string>("PriorKnowledgeMemoryProxyName").getValue();
    usingProxy(settings_priorMemory);
 
    featureCalculator.addFeature<CHArea>();
    featureCalculator.addFeature<CHVolume>();
 
    featureCalculator.addFeature<BBVolume>();
    featureCalculator.addFeature<BBArea>();
    featureCalculator.addFeature<BBLongSide>();
    featureCalculator.addFeature<BBShortSide>();
    featureCalculator.addFeature<BBMediumSide>();
    featureCalculator.addFeature<BBLongShortRatio>();
    featureCalculator.addFeature<BBMediumShortRatio>();
 
    featureCalculator.addFeature<D2Histogram>();
    featureCalculator.addFeature<A3Histogram>();
}
 
void ObjectShapeClassification::onConnectComponent()
{
    memoryPrx = getProxy<PriorKnowledgeInterfacePrx>(settings_priorMemory);
    classesSegmentPrx = memoryPrx->getObjectClassesSegment();
    databasePrx = memoryPrx->getCommonStorage();
 
    fileManager.reset(new GridFileManager(databasePrx));
 
    connected = databasePrx->isConnected();
}
 
void ObjectShapeClassification::refetchObjects()
{
    ARMARX_VERBOSE << "Refetching objects from DB" << armarx::flush;
    boost::recursive_mutex::scoped_lock lock(mutexEntities);
    EntityIdList ids = classesSegmentPrx->getAllEntityIds();
    dbObjects.clear();
 
    for (EntityIdList::const_iterator it = ids.begin(); it != ids.end(); ++it)
    {
        const EntityBasePtr entity = classesSegmentPrx->getEntityById(*it);
        const ObjectClassPtr objClass = ObjectClassPtr::dynamicCast(entity);
 
        if (objClass)
        {
            ARMARX_VERBOSE << *objClass << armarx::flush;
            dbObjects.push_back(objClass);
        }
    }
}
 
void ObjectShapeClassification::checkFeatures()
{
    for (auto optr : dbObjects)
    {
        //Check if the object has the feature attribute
        if (!optr->hasAttribute(ATTR_FEATURES))
        {
            //Calculate the attributes for this object
        }
 
        //Compare to the attributes of the current object
    }
}
 
Points ObjectShapeClassification::getPointsFromIV(ObjectClassPtr obj)
{
    //Cache the file
    std::string ivFile = "";
    Points ps;
 
    if (obj->hasAttribute(ATTR_IV_FILE))
    {
        fileManager->ensureFileInCache(obj->getAttribute(ATTR_IV_FILE), ivFile, false);
        ps = IVPointParser::fromFile(ivFile);
    }
    else
    {
        ARMARX_WARNING << "No IV file found for object" << obj->getName();
    }
 
    return ps;
}
 
struct by_second
{
    template <typename Pair>
    bool operator()(const Pair& a, const Pair& b)
    {
        return a.second < b.second;
    }
};
 
TaggedPoints ObjectShapeClassification::compareToDB(Points& points)
{
    std::vector<std::pair<std::string, Points>> objectsPoints;
 
    //Get the points from every object
    for (auto& obj : dbObjects)
    {
        auto points = getPointsFromIV(obj);
 
        if (points.size() != 0)
        {
            objectsPoints.push_back(TaggedPoints(obj->getName(), points));
        }
    }
 
    ARMARX_VERBOSE << "Calculating features of new points...";
    //Calculuate the features of the new object
    auto newObjectFeatures = featureCalculator.getFeatures(points);
    ARMARX_VERBOSE << "Calculated new features";
 
    //Print the points and features (for parsing and learning) ======================
    //Points pointsTransposed = Points(points.size());
    //std::transform(points.begin(), points.end(), pointsTransposed.end(), [&](Point& p) {return p.transpose();}); //Transpose the points, so they are printed horizontally
    //std::ostringstream oss;
    //Put the vector into a string, so that we can give it to ARMARX_* in a single line....
    //if (!pointsTransposed.empty()) {
    //    // Convert all but the last element to avoid a trailing ","
    //    std::copy(pointsTransposed.begin(), pointsTransposed.end()-1,
    //              std::ostream_iterator<Point>(oss, ","));
    //    // Now add the last element with no delimiter
    //    oss << pointsTransposed.back();
    //}
    //ARMARX_VERBOSE << "-- Points: [" << oss.str() << "]";
    //Finished printing points =======================================================
 
    //Now also print the features of the new object
    for (const auto& feat : newObjectFeatures)
    {
        ARMARX_VERBOSE << "-- " << feat.first << ": " << *(feat.second);
    }
 
 
    //Now actually go on doing useful work....
    std::map<std::string, std::map<std::string, double>> objectDiffs;
    std::map<std::string, double> maxDiffs;
 
    ARMARX_VERBOSE << "Now iterating over the db objects...";
 
    //For every object, get the features, or calculate them
    for (const auto& obj : objectsPoints)
    {
        ARMARX_VERBOSE << "    Getting features for: " << obj.first;
        auto features = featureCalculator.getFeatures(obj.second);
        std::map<std::string, double> diffs;
 
        //Compare to the new object, iterating over every feature
        for (const auto& p : newObjectFeatures)
        {
            auto& feature = p.second;
            double diff = feature->compare(*features[feature->name()]);
            diffs[feature->name()] = diff;
 
            //Store the maximum value of each feature, to normalize later.
            double oldValue = maxDiffs[feature->name()];
            maxDiffs[feature->name()] = (diff > oldValue) ? diff : oldValue;
        }
 
        objectDiffs[obj.first] = diffs;
    }
 
    std::map<std::string, double> objectDiffsTotal;
 
    //For each object get the total diff
    for (const auto& obj : objectDiffs)
    {
        //Get the sum of the differences of each feature, normalized.
        ARMARX_VERBOSE << "--\n-- " << obj.first << ": ";
        double totalDiff = std::accumulate(obj.second.begin(), obj.second.end(), 0.0, [&](double prev, const std::pair<std::string, double> p)
        {
            double diff = (p.second / maxDiffs[p.first]);
            ARMARX_VERBOSE_S << "-- " << p.first << ": " << diff;
            return prev + (p.second / maxDiffs[p.first]);
        });
        objectDiffsTotal[obj.first] = totalDiff;
        ARMARX_VERBOSE << "--+ Total score(" << obj.first << "): " << totalDiff << "\n";
    }
 
 
    auto bestMatch = *std::min_element(objectDiffsTotal.begin(), objectDiffsTotal.end(), by_second());
 
    ARMARX_INFO << "Best match found: " << bestMatch.first << " with score of " << bestMatch.second;
 
    return bestMatch;
}
 
void ObjectShapeClassification::matchToFoundPointCloud(const Points& newPoints, const Points& foundPointCloud)
{
    ARMARX_VERBOSE << "Running ICP...";
    ICP icp;
    icp.SetScenePointcloud(foundPointCloud);
    icp.SetObjectPointcloud(newPoints);
 
    Mat3d rotation;
    Vec3d translation;
    double msd = icp.SearchObject(rotation, translation);
    ARMARX_IMPORTANT << "Mean square difference to found point cloud: " << msd;
}
 
std::string ObjectShapeClassification::FindSimilarKnownObject(const types::PointList& segmentedObjectPoints, const Ice::Current&)
{
    Points points;
 
    for (size_t i = 0; i < segmentedObjectPoints.size(); i++)
    {
        points.push_back(armarx::Vector3Ptr::dynamicCast(segmentedObjectPoints.at(i))->toEigen());
    }
 
    ARMARX_VERBOSE << "Loaded "
                   << points.size()
                   << " data points.";
 
    if (connected)
    {
        ARMARX_INFO << "Connected to DB, starting classification";
 
        //Make sure we have all the objects
        refetchObjects();
 
        //Compare to the database and return the name of the best matched object
        TaggedPoints foundPointCloud = compareToDB(points);
        matchToFoundPointCloud(points, foundPointCloud.second);
 
        ARMARX_IMPORTANT << "The hypothesis had "
                         << points.size()
                         << " points";
 
        return foundPointCloud.first;
    }
    else
    {
        ARMARX_WARNING << "No connection to DB. Unable to start classification";
    }
 
    return "ERROR: No similar object found";
}