PrimitiveExtractor.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    VisionX
 * @author     Eren Aksoy ( eren dot aksoy at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PrimitiveExtractor.h"
 
#include <set>
 
#include <boost/lexical_cast.hpp>
 
#include <QSettings>
 
#include <pcl/common/colors.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
#include <VisionX/libraries/AffordanceKitArmarX/PrimitiveSetArmarX.h>
 
#include <AffordanceKit/PrimitiveExtractor.h>
#include <AffordanceKit/primitives/Box.h>
#include <AffordanceKit/primitives/Cylinder.h>
#include <AffordanceKit/primitives/Plane.h>
#include <AffordanceKit/primitives/Sphere.h>
 
using namespace memoryx;
using namespace armarx;
using namespace pcl;
 
void
PrimitiveExtractor::onInitPointCloudProcessor()
{
    lastProcessedTimestamp = 0;
 
    providerName = getProperty<std::string>("segmenterName").getValue();
 
    // get required primitive parameters
    minSegmentSize = getProperty<double>("minimumSegmentSize").getValue();
    maxSegmentSize = getProperty<double>("maximumSegmentSize").getValue();
    pMaxIter = getProperty<double>("planeMaximumIteration").getValue();
    pDistThres = getProperty<float>("planeDistanceThreshold").getValue();
    pNorDist = getProperty<float>("planeNormalDistance").getValue();
    cMaxIter = getProperty<double>("cylinderMaximumIteration").getValue();
    cDistThres = getProperty<float>("cylinderDistanceThreshold").getValue();
    cRadLim = getProperty<float>("cylinderRadiousLimit").getValue();
    sMaxIter = getProperty<double>("sphereMaximumIteration").getValue();
    sDistThres = getProperty<float>("sphereDistanceThreshold").getValue();
    sNorDist = getProperty<float>("sphereNormalDistance").getValue();
    verbose = getProperty<bool>("vebose").getValue();
    euclideanClusteringTolerance = getProperty<float>("euclideanClusteringTolerance").getValue();
    outlierThreshold = getProperty<float>("outlierThreshold").getValue();
    circleDistThres = getProperty<float>("circularDistanceThreshold").getValue();
 
    spatialSamplingDistance = getProperty<float>("SpatialSamplingDistance").getValue();
    numOrientationalSamplings = getProperty<int>("NumOrientationalSamplings").getValue();
 
    // reset all point cloud pointers
    labeledCloudPtr.reset(new PointCloud<PointXYZL>);
    inliersCloudPtr.reset(new PointCloud<PointXYZL>);
    graspCloudPtr.reset(new PointCloud<PointXYZL>);
 
    // init the primitive class
    primitiveExtractor.reset(new AffordanceKit::PrimitiveExtractor());
    primitiveExtractor->updateParameters(verbose,
                                         minSegmentSize,
                                         maxSegmentSize,
                                         pMaxIter,
                                         pDistThres,
                                         pNorDist,
                                         cMaxIter,
                                         cDistThres,
                                         cRadLim,
                                         sMaxIter,
                                         sDistThres,
                                         sNorDist,
                                         euclideanClusteringTolerance,
                                         outlierThreshold,
                                         circleDistThres);
    usingPointCloudProvider(providerName);
 
    if (getProperty<std::string>("RobotStateComponentName").getValue() != "")
    {
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
        sourceFrameName = getProperty<std::string>("sourceFrameName").getValue();
    }
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
 
    mappingEnabled = true;
 
    enablePrimitiveFusion = getProperty<bool>("enablePrimitiveFusion").getValue();
 
    offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
    offeringTopic(getProperty<std::string>("SegmentedPointCloudTopicName").getValue());
 
    offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
}
 
void
PrimitiveExtractor::onConnectPointCloudProcessor()
{
    enableResultPointClouds<PointXYZL>("GraspPointCloudResult");
    enableResultPointClouds<PointXYZL>("PrimitiveExtractorResult");
    enableResultPointClouds<PointXYZL>("Inliers");
 
    workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(
        getProperty<std::string>("WorkingMemoryName").getValue());
    if (!workingMemory)
    {
        ARMARX_ERROR << "Failed to obtain working memory proxy";
    }
 
    environmentalPrimitiveSegment = workingMemory->getEnvironmentalPrimitiveSegment();
    if (!environmentalPrimitiveSegment)
    {
        ARMARX_ERROR << "Failed to obtain environmental primitive segment pointer";
    }
 
    pointCloudSegmentationPrx = getTopic<visionx::PointCloudSegmentationListenerPrx>(
        getProperty<std::string>("SegmentedPointCloudTopicName").getValue());
 
    debugDrawerTopicPrx = getTopic<DebugDrawerInterfacePrx>(
        getProperty<std::string>("DebugDrawerTopicName").getValue());
 
    if (getProperty<std::string>("RobotStateComponentName").getValue() != "")
    {
        robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
            getProperty<std::string>("RobotStateComponentName").getValue());
        if (!robotStateComponent->getSynchronizedRobot()->hasRobotNode(sourceFrameName))
        {
            ARMARX_ERROR << "source node does not exist!";
            robotStateComponent = nullptr;
        }
    }
 
    lastUsedLabel = 0;
 
    debugDrawerTopicPrx->clearLayer("primitiveMapper");
 
    debugObserver = getTopic<DebugObserverInterfacePrx>(
        getProperty<std::string>("DebugObserverName").getValue());
    numViews = 0;
 
 
    StringVariantBaseMap debugValues;
    debugValues["processingTime"] = new Variant(0.0);
    debugValues["oldPrimitives"] = new Variant(0.0);
    debugValues["visiblePrimitives"] = new Variant(0.0);
    debugValues["newPrimitives"] = new Variant(0.0);
    debugValues["modifiedPrimitives"] = new Variant(0.0);
    debugValues["unmodifiedPrimitives"] = new Variant(0.0);
    debugValues["removedPrimitives"] = new Variant(0.0);
    debugValues["numViews"] = new Variant(0.0);
    debugObserver->setDebugChannel(getName(), debugValues);
}
 
void
PrimitiveExtractor::onDisconnectPointCloudProcessor()
{
}
 
void
PrimitiveExtractor::onExitPointCloudProcessor()
{
}
 
void
PrimitiveExtractor::enablePipelineStep(const Ice::Current& c)
{
    std::unique_lock lock(enableMutex);
    mappingEnabled = true;
}
 
void
PrimitiveExtractor::disablePipelineStep(const Ice::Current& c)
{
    std::unique_lock lock(enableMutex);
    mappingEnabled = false;
}
 
void
PrimitiveExtractor::process()
{
    if (!waitForPointClouds(providerName, 10000))
    {
        return;
    }
 
    {
        std::unique_lock lock(enableMutex);
 
        if (!mappingEnabled)
        {
            return;
        }
    }
 
    // start timer
    IceUtil::Time startTime = IceUtil::Time::now();
 
    pcl::PointCloud<pcl::PointXYZRGBL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGBL>());
    if (!getPointClouds(providerName, temp))
    {
        ARMARX_WARNING << "Unable to get point cloud data.";
        return;
    }
 
    pcl::copyPointCloud(*temp, *labeledCloudPtr);
    if (labeledCloudPtr->size() == 0)
    {
        ARMARX_WARNING << "input point cloud is empty";
 
        provideResultPointClouds(labeledCloudPtr, "GraspPointCloudResult");
        provideResultPointClouds(labeledCloudPtr, "PrimitiveExtractorResult");
 
        return;
    }
 
 
    inliersCloudPtr = primitiveExtractor->computeScenePrimitives(labeledCloudPtr);
    graspCloudPtr = primitiveExtractor->getDetectedGraspPoints();
 
 
    const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& planeInliers =
        primitiveExtractor->getPlaneInliers();
    const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& cylinderInliers =
        primitiveExtractor->getCylinderInliers();
    const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& sphereInliers =
        primitiveExtractor->getSphereInliers();
    const std::vector<float>& circularityProbabilities =
        primitiveExtractor->getCircularityProbabilities();
 
    ARMARX_INFO << " ********* Detected Plane number: " << planeInliers.size();
    ARMARX_INFO << " ********* Detected Cylinder number: " << cylinderInliers.size();
    ARMARX_INFO << " ********* Detected Sphere number: " << sphereInliers.size();
    ARMARX_INFO << " ********* Detected Circle Probabilities: " << circularityProbabilities.size();
 
    // publish the extracted inliers
    provideResultPointClouds(graspCloudPtr, "GraspPointCloudResult");
    provideResultPointClouds(inliersCloudPtr, "PrimitiveExtractorResult");
 
    ARMARX_INFO << "Primitive Extraction took "
                << (IceUtil::Time::now() - startTime).toSecondsDouble() << " secs";
 
    // Push each extracted primitive to the memory
    startTime = IceUtil::Time::now();
 
    bool enabled;
    {
        std::unique_lock lock(enableMutex);
        enabled = mappingEnabled;
    }
 
    if (enabled)
    {
        if (getProperty<bool>("UseAffordanceExtractionLibExport").getValue())
        {
            pushPrimitivesToMemoryAffordanceKit(temp->header.stamp);
        }
        else
        {
            pushPrimitivesToMemory(temp->header.stamp);
        }
 
        {
            std::unique_lock lock(timestampMutex);
            lastProcessedTimestamp = temp->header.stamp;
        }
    }
 
    // Signal the availability of a new segmentation
    pointCloudSegmentationPrx->reportNewPointCloudSegmentation();
 
 
    ARMARX_INFO << "Mapping took " << (IceUtil::Time::now() - startTime).toSecondsDouble()
                << " secs";
}
 
float
PrimitiveExtractor::getGraspPointInlierRatio(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive,
                                             memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive)
{
    FramedPosePtr pose = FramedPosePtr::dynamicCast(rightPrimitive->getPose());
    Eigen::Matrix4f worldToPrimitive = pose->toEigen().inverse();
 
    Eigen::Vector3f oBBDimensions =
        Vector3Ptr::dynamicCast(rightPrimitive->getOBBDimensions())->toEigen();
    Eigen::Vector3f extent = 0.5 * oBBDimensions + (Eigen::Vector3f::Ones() * 50);
 
    float numPoints = 0.0;
    memoryx::PointList graspPoints = leftPrimitive->getGraspPoints();
 
    for (Vector3BasePtr& p : graspPoints)
    {
        Eigen::Vector4f temp = Eigen::Vector4f(p->x, p->y, p->z, 1.0);
        Eigen::Vector3f point = (worldToPrimitive * temp).head<3>();
 
        if ((point.array().abs() < extent.array()).all())
        {
            numPoints++;
        }
    }
 
    return numPoints / graspPoints.size();
}
 
void
PrimitiveExtractor::fusePrimitives(std::vector<memoryx::EntityBasePtr>& newPrimitives,
                                   long timestamp)
{
    //    memoryx::SegmentLockBasePtr segmentLock = environmentalPrimitiveSegment->lockSegment();
    std::vector<EnvironmentalPrimitiveBasePtr> primitives =
        environmentalPrimitiveSegment->getEnvironmentalPrimitives();
    //    environmentalPrimitiveSegment->unlockSegment(segmentLock);
 
    std::vector<memoryx::EnvironmentalPrimitiveBasePtr> nonVisiblePrimitives;
    std::vector<memoryx::EnvironmentalPrimitiveBasePtr> visiblePrimitives;
 
    std::vector<memoryx::EnvironmentalPrimitiveBasePtr> modifiedPrimitives;
    std::set<std::string> processedPrimitives;
    std::set<std::string> primitivesToRemove;
 
    if (robotStateComponent)
    {
        Eigen::Matrix4f cameraToWorld = Eigen::Matrix4f::Identity();
        if (sourceFrameName != armarx::GlobalFrame)
        {
            auto snapshot = robotStateComponent->getRobotSnapshotAtTimestamp(timestamp);
            snapshot->ref();
            FramedPoseBasePtr pose = snapshot->getRobotNode(sourceFrameName)->getGlobalPose();
            snapshot->unref();
            cameraToWorld = armarx::FramedPosePtr::dynamicCast(pose)->toEigen();
        }
 
        primitiveFilter.updateCameraPosition(cameraToWorld);
        // if (verbose)
        // {
        primitiveFilter.visualizeFrustum(debugDrawerTopicPrx);
        // }
        primitiveFilter.getPrimitivesInFieldOfView(
            primitives, visiblePrimitives, nonVisiblePrimitives);
    }
    else
    {
 
        ARMARX_ERROR << "Frustum culling not possible without RobotStateComponent assuming that "
                        "all primitives are visible";
        visiblePrimitives = primitives;
    }
 
 
    if (getProperty<bool>("EnableBoxPrimitives").getValue())
    {
 
        // fusionStrategy.findBoxPrimitives(primitives, newPrimitives, environmentalPrimitiveSegment);
        for (const EnvironmentalPrimitiveBasePtr& p : primitives)
        {
            if (p->ice_isA(memoryx::BoxPrimitiveBase::ice_staticId()))
            {
                primitivesToRemove.insert(p->getId());
            }
        }
    }
 
    if (nonVisiblePrimitives.size())
    {
        ARMARX_INFO << "found " << nonVisiblePrimitives.size()
                    << " non visible primitives in field of view.";
        for (memoryx::EnvironmentalPrimitiveBasePtr& p : nonVisiblePrimitives)
        {
            p->setProbability(std::max(0.0, p->getProbability() - 0.1));
            if (p->getProbability() <= 0.4)
            {
                primitivesToRemove.insert(p->getId());
            }
            else
            {
                modifiedPrimitives.push_back(p);
            }
        }
    }
 
 
    ARMARX_DEBUG << "total primitives " << primitives.size() << " visible primitives "
                 << visiblePrimitives.size() << "new primitives" << newPrimitives.size();
 
    std::vector<memoryx::EntityBasePtr> uniqueEntities;
    for (const memoryx::EntityBasePtr& newEntity : newPrimitives)
    {
        memoryx::EnvironmentalPrimitiveBasePtr newPrimitive =
            memoryx::EnvironmentalPrimitiveBasePtr::dynamicCast(newEntity);
 
        std::vector<EnvironmentalPrimitiveBasePtr> intersectingPrimitives;
        fusionStrategy.getIntersectingSimilarPrimitives(
            primitives, newPrimitive, intersectingPrimitives, 50.0);
        // fusionStrategy.getIntersectingSimilarPrimitives(visiblePrimitives, newPrimitive, intersectingPrimitives, 50.0);
 
 
        bool addPrimitive = true;
        for (EnvironmentalPrimitiveBasePtr& intersectingPrimitive : intersectingPrimitives)
        {
            if (primitivesToRemove.count(intersectingPrimitive->getId()))
            {
                continue;
            }
 
            int currentLabel = 0;
            float inlierRatioLeft = getGraspPointInlierRatio(newPrimitive, intersectingPrimitive);
            float inlierRatioRight = getGraspPointInlierRatio(intersectingPrimitive, newPrimitive);
 
            if (inlierRatioLeft <= 0.1 && inlierRatioRight <= 0.1)
            {
                processedPrimitives.insert(intersectingPrimitive->getId());
            }
 
            if (inlierRatioLeft >= 0.9 && inlierRatioRight >= 0.9)
            {
                intersectingPrimitive->setTime(newPrimitive->getTime());
                float newProbability = std::fmax(newPrimitive->getProbability(),
                                                 intersectingPrimitive->getProbability() + 0.15);
                intersectingPrimitive->setProbability(std::fmin(1.0, newProbability));
                modifiedPrimitives.push_back(intersectingPrimitive);
 
                addPrimitive = false;
                break;
            }
            else if (inlierRatioLeft <= 0.3 && inlierRatioRight >= 0.7)
            {
 
                primitivesToRemove.insert(intersectingPrimitive->getId());
            }
            else if (inlierRatioLeft >= 0.7 && inlierRatioRight <= 0.3)
            {
 
                addPrimitive = false;
                break;
            }
            else if ((inlierRatioLeft + inlierRatioRight) >= 1.4)
            {
                if ((newPrimitive->getOBBDimensions()->x * newPrimitive->getOBBDimensions()->y) >
                    (intersectingPrimitive->getOBBDimensions()->x *
                     intersectingPrimitive->getOBBDimensions()->y))
                {
                    primitivesToRemove.insert(intersectingPrimitive->getId());
                }
                else
                {
                    addPrimitive = false;
                    break;
                }
            }
 
            if (currentLabel)
            {
            }
            else if (intersectingPrimitive->getLabel())
            {
                currentLabel = intersectingPrimitive->getLabel();
            }
 
            if (currentLabel == 0 && intersectingPrimitives.size())
            {
                currentLabel = ++lastUsedLabel;
            }
            newPrimitive->setLabel(currentLabel);
 
            for (EnvironmentalPrimitiveBasePtr& x : intersectingPrimitives)
            {
                x->setLabel(currentLabel);
                modifiedPrimitives.push_back(x);
            }
        }
        if (addPrimitive)
        {
            uniqueEntities.push_back(newPrimitive);
        }
    }
 
    if (robotStateComponent)
    {
 
        for (memoryx::EnvironmentalPrimitiveBasePtr& p : visiblePrimitives)
        {
 
            if (processedPrimitives.count(p->getId()) || primitivesToRemove.count(p->getId()))
            {
                continue;
            }
 
            float probability = p->getProbability();
            p->setProbability(std::max(0.0, p->getProbability() - 0.5));
            ARMARX_LOG << "reducing probability for primitive in field of view: " << p->getId()
                       << " from " << probability << " to " << p->getProbability();
 
            if (p->getProbability() <= 0.4)
            {
                ARMARX_LOG << "scheduling primitive " << p->getId() << " for removal";
                primitivesToRemove.insert(p->getId());
            }
            else
            {
                modifiedPrimitives.push_back(p);
            }
        }
    }
 
 
    ARMARX_INFO << "removing " << primitivesToRemove.size() << " primitives";
 
    for (const std::string& x : primitivesToRemove)
    {
        environmentalPrimitiveSegment->removeEntity(x);
    }
 
    int modified = 0;
    processedPrimitives.clear();
    for (EnvironmentalPrimitiveBasePtr& x : modifiedPrimitives)
    {
        if (!primitivesToRemove.count(x->getId()) && !processedPrimitives.count(x->getId()))
        {
            modified++;
            environmentalPrimitiveSegment->updateEntity(x->getId(), x);
            processedPrimitives.insert(x->getId());
        }
    }
 
    float unmodified = primitives.size() - modified - primitivesToRemove.size();
 
    ARMARX_INFO << "Writing segmentation (" << uniqueEntities.size() << " primitives) to memory";
 
    environmentalPrimitiveSegment->addEntityList(uniqueEntities);
 
    StringVariantBaseMap debugValues;
    debugValues["processingTime"] =
        new Variant((TimeUtil::GetTime().toMicroSeconds() - timestamp) / (1000.0 * 1000.0));
    debugValues["oldPrimitives"] = new Variant((float)primitives.size());
    debugValues["visiblePrimitives"] = new Variant((float)visiblePrimitives.size());
    debugValues["newPrimitives"] = new Variant((float)uniqueEntities.size());
    debugValues["modifiedPrimitives"] = new Variant(modified);
    debugValues["unmodifiedPrimitives"] = new Variant(unmodified);
    debugValues["removedPrimitives"] = new Variant((float)primitivesToRemove.size());
    numViews++;
    debugValues["numViews"] = new Variant((int)numViews);
    debugObserver->setDebugChannel(getName(), debugValues);
 
    pcl::PointCloud<pcl::PointXYZL>::Ptr inliers(new pcl::PointCloud<pcl::PointXYZL>());
    for (const EnvironmentalPrimitiveBasePtr& x : primitives)
    {
        if (primitivesToRemove.count(x->getId()))
        {
            continue;
        }
 
        for (const armarx::Vector3BasePtr& v : x->getInliers())
        {
            pcl::PointXYZL p;
            p.label = x->getLabel();
            p.x = v->x;
            p.y = v->y;
            p.z = v->z;
            inliers->points.push_back(p);
        }
    }
 
    for (const memoryx::EntityBasePtr& e : uniqueEntities)
    {
        EnvironmentalPrimitivePtr x = EnvironmentalPrimitivePtr::dynamicCast(e);
 
        for (const armarx::Vector3BasePtr& v : x->getInliers())
        {
            pcl::PointXYZL p;
            p.label = x->getLabel();
            p.x = v->x;
            p.y = v->y;
            p.z = v->z;
            inliers->points.push_back(p);
        }
    }
 
 
    inliers->height = 1;
    inliers->width = inliers->points.size();
 
 
    provideResultPointClouds(inliers, "Inliers");
 
 
    //primitiveExtractorPrx->reportNewPrimitiveSet(newPrimitives);
}
 
void
PrimitiveExtractor::pushPrimitivesToMemory(IceUtil::Int64 originalTimestamp)
{
    std::vector<memoryx::EntityBasePtr> newPrimitives;
 
    const std::vector<pcl::ModelCoefficients::Ptr>& planeCoefficients =
        primitiveExtractor->getPlaneCoefficients();
    const std::vector<pcl::ModelCoefficients::Ptr>& cylinderCoefficients =
        primitiveExtractor->getCylinderCoefficients();
    const std::vector<pcl::ModelCoefficients::Ptr>& sphereCoefficients =
        primitiveExtractor->getSphereCoefficients();
    const std::vector<float>& circularityProbabilities =
        primitiveExtractor->getCircularityProbabilities();
 
    const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& planeInliers =
        primitiveExtractor->getPlaneInliers();
    const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& cylinderInliers =
        primitiveExtractor->getCylinderInliers();
    const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& sphereInliers =
        primitiveExtractor->getSphereInliers();
 
    const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& cylinderGraspPoints =
        primitiveExtractor->getCylinderGraspPoints();
    const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& sphereGraspPoints =
        primitiveExtractor->getSphereGraspPoints();
 
    int label = 0;
    // for each detected plane primitive
    //#pragma omp parallel for
    for (size_t i = 0; i < planeCoefficients.size(); i++)
    {
        // coefficients
        pcl::ModelCoefficients::Ptr coefficients = planeCoefficients[i];
        pcl::PointCloud<pcl::PointXYZL>::Ptr inliers = planeInliers[i];
        //        pcl::PointCloud<pcl::PointXYZL>::Ptr graspPoints = planeGraspPoints[i];
        pcl::PointCloud<pcl::PointXYZL>::Ptr graspPoints(new pcl::PointCloud<pcl::PointXYZL>());
 
        pcl::PointCloud<pcl::PointXYZL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZL>());
        pcl::ProjectInliers<PointL> proj;
        proj.setModelType(pcl::SACMODEL_PLANE);
        proj.setInputCloud(inliers);
        proj.setModelCoefficients(coefficients);
        proj.filter(*temp);
 
        ARMARX_LOG << __LINE__ << "computing hull";
 
        pcl::ConcaveHull<PointL> concaveHull;
        concaveHull.setInputCloud(temp);
        concaveHull.setDimension(2);
        concaveHull.setAlpha(69.0);
        //        concaveHull.setKeepInformation(true);
        concaveHull.reconstruct(*graspPoints);
 
 
        ARMARX_LOG << __LINE__ << " done computing hull";
 
        // bounding boxes
        pcl::MomentOfInertiaEstimation<pcl::PointXYZL> bb;
        pcl::PointXYZL min_point_OBB, max_point_OBB, position_OBB;
        Eigen::Matrix3f rotational_matrix_OBB;
 
        bb.setInputCloud(graspPoints);
        bb.compute();
        bb.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
 
        Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
        Vector3Ptr dimension =
            new Vector3(max_point_OBB.x * 2, max_point_OBB.y * 2, max_point_OBB.z * 2);
        armarx::FramedPosePtr pose =
            new FramedPose(rotational_matrix_OBB, position, armarx::GlobalFrame, "");
 
        // computing the length
        pcl::PointXYZL pointMin, pointMax;
        pcl::getMinMax3D<pcl::PointXYZL>(*inliers, pointMin, pointMax);
        float length = (pointMax.getVector3fMap() - pointMin.getVector3fMap()).norm();
 
        // setting primitives
        EnvironmentalPrimitiveBasePtr primitive;
        primitive = new PlanePrimitive(coefficients->values);
        primitive->setOBBDimensions(dimension);
        primitive->setPose(pose);
        primitive->setProbability(0.75); // dummy value
        primitive->setLabel(0);
        primitive->setTime(new TimestampVariant(originalTimestamp));
        primitive->setLength(length);
        primitive->setCircularityProbability(circularityProbabilities[i]);
        primitive->setSampling(new armarx::MatrixFloat(4, 0));
        primitive->setName("environmentalPrimitive_" + boost::lexical_cast<std::string>(label++));
 
        // convert pcl grasp points into PointList
        PointList graspPointList(graspPoints->points.size());
        for (size_t p = 0; p < graspPoints->points.size(); p++)
        {
            Vector3Ptr currPoint = new Vector3(
                graspPoints->points[p].x, graspPoints->points[p].y, graspPoints->points[p].z);
            graspPointList[p] = currPoint;
        }
        primitive->setGraspPoints(graspPointList);
 
        PointList inlierList(inliers->points.size());
        for (size_t p = 0; p < inliers->points.size(); p++)
        {
            const Eigen::Vector3f& f = inliers->points[p].getVector3fMap();
            inlierList[p] = new Vector3(f);
        }
        primitive->setInliers(inlierList);
 
 
        //#pragma omp critical
 
        // Generate sampling
        std::vector<Eigen::Vector3f> convexHull(graspPointList.size());
        for (unsigned int i = 0; i < graspPointList.size(); i++)
        {
            convexHull[i] = armarx::Vector3Ptr::dynamicCast(graspPointList[i])->toEigen();
        }
        AffordanceKit::PlanePtr plane(new AffordanceKit::Plane(convexHull));
        plane->sample(spatialSamplingDistance, numOrientationalSamplings);
        armarx::MatrixFloatBasePtr s(new armarx::MatrixFloat(plane->getSampling()));
        primitive->setSampling(s);
 
        //#pragma omp critical
        {
            newPrimitives.push_back(primitive);
        }
    }
 
    // for each detected cylinder primitive
    //#pragma omp parallel for
    for (size_t i = 0; i < cylinderCoefficients.size(); i++)
    {
        // coefficients
        pcl::ModelCoefficients::Ptr coefficients = cylinderCoefficients[i];
        pcl::PointCloud<pcl::PointXYZL>::Ptr inliers = cylinderInliers[i];
        pcl::PointCloud<pcl::PointXYZL>::Ptr graspPoints = cylinderGraspPoints[i];
 
        pcl::PointCloud<pcl::PointXYZL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZL>());
        pcl::ProjectInliers<PointL> proj;
        proj.setModelType(pcl::SACMODEL_PLANE);
        proj.setInputCloud(inliers);
        proj.setModelCoefficients(coefficients);
        proj.filter(*temp);
 
        pcl::ConcaveHull<PointL> concaveHull;
        concaveHull.setInputCloud(temp);
        concaveHull.setDimension(2);
        concaveHull.setAlpha(69.0);
        concaveHull.setKeepInformation(true);
        concaveHull.reconstruct(*graspPoints);
 
        // bounding boxes
        pcl::MomentOfInertiaEstimation<pcl::PointXYZL> bb;
        pcl::PointXYZL min_point_OBB, max_point_OBB, position_OBB;
        Eigen::Matrix3f rotational_matrix_OBB;
 
        bb.setInputCloud(inliers);
        bb.compute();
        bb.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
 
        Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
        Vector3Ptr dimension =
            new Vector3(max_point_OBB.x * 2, max_point_OBB.y * 2, max_point_OBB.z * 2);
        armarx::FramedPosePtr pose =
            new FramedPose(rotational_matrix_OBB, position, armarx::GlobalFrame, "");
 
        // Projected inliers to determine cylinder base point and length
        // (These are not properly encoded in the RANSAC coefficients)
        float min_t = std::numeric_limits<float>::infinity();
        float max_t = -std::numeric_limits<float>::infinity();
 
        Eigen::Vector3f b(
            coefficients->values[0], coefficients->values[1], coefficients->values[2]);
        Eigen::Vector3f d(
            coefficients->values[3], coefficients->values[4], coefficients->values[5]);
        d.normalize();
 
        for (unsigned int i = 0; i < inliers->points.size(); i++)
        {
            Eigen::Vector3f v(inliers->points[i].x, inliers->points[i].y, inliers->points[i].z);
 
            float t = (v - b).dot(d) / d.dot(d);
 
            min_t = std::min(min_t, t);
            max_t = std::max(max_t, t);
        }
 
        float length = max_t - min_t;
        Eigen::Vector3f base = b + min_t * d + 0.5 * length * d;
        Eigen::Vector3f direction = d;
        float radius = coefficients->values[6];
 
        // Create MemoryX cylinder primitive instance
        CylinderPrimitiveBasePtr primitive;
        primitive = new CylinderPrimitive;
        primitive->setCylinderPoint(new Vector3(base));
        primitive->setCylinderAxisDirection(new Vector3(direction));
        primitive->setCylinderRadius(radius);
        primitive->setLength(length);
        primitive->setOBBDimensions(dimension);
        primitive->setPose(pose);
        primitive->setProbability(0.75); // dummy value
        primitive->setLabel(0);
        primitive->setTime(new TimestampVariant(originalTimestamp));
        primitive->setCircularityProbability(0.0); // it is always zero for non-planar surfaces
        primitive->setSampling(new armarx::MatrixFloat(4, 0));
 
        // convert pcl grasp points into PointList
        PointList graspPointList(graspPoints->points.size());
 
        for (size_t p = 0; p < graspPoints->points.size(); p++)
        {
            Vector3Ptr currPoint = new Vector3(
                graspPoints->points[p].x, graspPoints->points[p].y, graspPoints->points[p].z);
            graspPointList[p] = currPoint;
        }
 
        primitive->setName("environmentalPrimitive_" +
                           boost::lexical_cast<std::string>(planeCoefficients.size() + i));
        primitive->setGraspPoints(graspPointList);
 
        // Generate sampling
        AffordanceKit::CylinderPtr cylinder(
            new AffordanceKit::Cylinder(base, direction, length, radius));
        cylinder->sample(spatialSamplingDistance, numOrientationalSamplings);
        armarx::MatrixFloatBasePtr s(new armarx::MatrixFloat(cylinder->getSampling()));
        primitive->setSampling(s);
 
        //#pragma omp critical
        {
            newPrimitives.push_back(primitive);
        }
    }
 
    // for each detected sphere primitive
    //#pragma omp parallel for
    for (size_t i = 0; i < sphereCoefficients.size(); i++)
    {
        // coefficients
        pcl::ModelCoefficients::Ptr coefficients = sphereCoefficients[i];
        pcl::PointCloud<pcl::PointXYZL>::Ptr inliers = sphereInliers[i];
        pcl::PointCloud<pcl::PointXYZL>::Ptr graspPoints = sphereGraspPoints[i];
 
        // apply transformation to each inlier and grasp point
        //pcl::transformPointCloud(*inliers, *inliers, transform);
        //pcl::transformPointCloud(*graspPoints, *graspPoints, transform);
 
        // bounding boxes
        pcl::MomentOfInertiaEstimation<pcl::PointXYZL> bb;
        pcl::PointXYZL min_point_OBB, max_point_OBB, position_OBB;
        Eigen::Matrix3f rotational_matrix_OBB;
 
        bb.setInputCloud(inliers);
        bb.compute();
        bb.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
 
        Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
        Vector3Ptr dimension =
            new Vector3(max_point_OBB.x * 2, max_point_OBB.y * 2, max_point_OBB.z * 2);
        armarx::FramedPosePtr pose =
            new FramedPose(rotational_matrix_OBB, position, armarx::GlobalFrame, "");
 
 
        //computing the length
        pcl::PointXYZL pointMin, pointMax;
        pcl::getMinMax3D<pcl::PointXYZL>(*inliers, pointMin, pointMax);
        float length = (pointMax.getVector3fMap() - pointMin.getVector3fMap()).norm();
 
        // setting primitives
        EnvironmentalPrimitiveBasePtr primitive = {};
        primitive = new SpherePrimitive(coefficients->values);
        primitive->setOBBDimensions(dimension);
        primitive->setPose(pose);
        primitive->setLabel(0);
        primitive->setProbability(0.75); // dummy value
        primitive->setTime(new TimestampVariant(originalTimestamp));
        primitive->setLength(length);
        primitive->setCircularityProbability(0.0); // it is always zero for non-planar surfaces
        primitive->setSampling(new armarx::MatrixFloat(4, 0));
 
        // convert pcl grasp points into PointList
        PointList graspPointList(graspPoints->points.size());
 
        for (size_t p = 0; p < graspPoints->points.size(); p++)
        {
            Vector3Ptr currPoint = new Vector3(
                graspPoints->points[p].x, graspPoints->points[p].y, graspPoints->points[p].z);
            graspPointList[p] = currPoint;
        }
 
        primitive->setName("environmentalPrimitive_" +
                           boost::lexical_cast<std::string>(cylinderCoefficients.size() +
                                                            planeCoefficients.size() + i));
        primitive->setGraspPoints(graspPointList);
 
        // Generate sampling
        Eigen::Vector3f center(
            coefficients->values[0], coefficients->values[1], coefficients->values[2]);
        float radius = coefficients->values[3];
        AffordanceKit::SpherePtr sphere(new AffordanceKit::Sphere(center, radius));
        sphere->sample(spatialSamplingDistance, numOrientationalSamplings);
        armarx::MatrixFloatBasePtr s(new armarx::MatrixFloat(sphere->getSampling()));
        primitive->setSampling(s);
 
        //#pragma omp critical
        {
            newPrimitives.push_back(primitive);
        }
    }
 
    ARMARX_LOG << __LINE__ << " writing primitives to memory.";
 
    if (enablePrimitiveFusion)
    {
        fusePrimitives(newPrimitives, originalTimestamp);
    }
    else
    {
        ARMARX_LOG << __LINE__ << " removing previous primitives from memory.";
        environmentalPrimitiveSegment->removeAllEntities();
        ARMARX_LOG << __LINE__ << " adding " << VAROUT(newPrimitives.size());
        environmentalPrimitiveSegment->addEntityList(newPrimitives);
    }
 
 
    ARMARX_LOG << __LINE__ << "done writing primitives to memory.";
 
    if (getProperty<bool>("EnableBoxPrimitives").getValue())
    {
        newPrimitives.clear();
 
        std::vector<EnvironmentalPrimitiveBasePtr> primitives =
            environmentalPrimitiveSegment->getEnvironmentalPrimitives();
 
 
        unsigned int sz = newPrimitives.size();
        fusionStrategy.findBoxPrimitives(primitives, newPrimitives, environmentalPrimitiveSegment);
 
        // Generate sampling
        for (auto& primitive : newPrimitives)
        {
            memoryx::BoxPrimitivePtr box = memoryx::BoxPrimitivePtr::dynamicCast(primitive);
            if (!box)
            {
                continue;
            }
 
            Eigen::Matrix4f pose = PosePtr::dynamicCast(box->getPose())->toEigen();
            Eigen::Vector3f dimensions =
                Vector3Ptr::dynamicCast(box->getOBBDimensions())->toEigen();
            AffordanceKit::BoxPtr b(new AffordanceKit::Box(pose, dimensions));
            b->sample(getProperty<float>("SpatialSamplingDistance").getValue(),
                      getProperty<int>("NumOrientationalSamplings").getValue());
            armarx::MatrixFloatBasePtr s(new armarx::MatrixFloat(b->getSampling()));
            box->setSampling(s);
        }
 
        ARMARX_INFO << "Found " << (newPrimitives.size() - sz) << " Box primitives";
 
        environmentalPrimitiveSegment->addEntityList(newPrimitives);
    }
}
 
void
PrimitiveExtractor::pushPrimitivesToMemoryAffordanceKit(IceUtil::Int64 originalTimestamp)
{
    AffordanceKit::PrimitiveSetPtr ps = primitiveExtractor->getPrimitiveSet(
        getProperty<float>("SpatialSamplingDistance").getValue(),
        getProperty<int>("NumOrientationalSamplings").getValue(),
        originalTimestamp);
    AffordanceKitArmarX::PrimitiveSetArmarX primitiveSet(ps);
    primitiveSet.writeToMemory(environmentalPrimitiveSegment);
}
 
PropertyDefinitionsPtr
PrimitiveExtractor::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new PrimitiveExtractorPropertyDefinitions(getConfigIdentifier()));
}
 
bool
armarx::PrimitiveExtractor::isPipelineStepEnabled(const Ice::Current& c)
{
    std::unique_lock lock(enableMutex);
    return mappingEnabled;
}
 
armarx::TimestampBasePtr
armarx::PrimitiveExtractor::getLastProcessedTimestamp(const Ice::Current& c)
{
    std::unique_lock lock(timestampMutex);
    return new TimestampVariant(lastProcessedTimestamp);
}
 
void
armarx::PrimitiveExtractor::setParameters(const visionx::PrimitiveExtractorParameters& parameters,
                                          const Ice::Current& c)
{
    ARMARX_INFO << "Updating parameter setup";
    primitiveExtractor->updateParameters(verbose,
                                         parameters.minSegmentSize,
                                         parameters.maxSegmentSize,
                                         parameters.planeMaxIterations,
                                         parameters.planeDistanceThreshold,
                                         parameters.planeNormalDistance,
                                         parameters.cylinderMaxIterations,
                                         parameters.cylinderDistanceThreshold,
                                         parameters.cylinderRadiusLimit,
                                         parameters.sphereMaxIterations,
                                         parameters.sphereDistanceThreshold,
                                         parameters.sphereNormalDistance,
                                         parameters.euclideanClusteringTolerance,
                                         parameters.outlierThreshold,
                                         parameters.circularDistanceThreshold);
}
 
void
PrimitiveExtractor::loadParametersFromFile(const std::string& filename, const Ice::Current& c)
{
    std::string absolute_filename;
    ArmarXDataPath::getAbsolutePath(filename, absolute_filename);
 
    ARMARX_INFO << "Loading parameter setup from " << absolute_filename;
    QSettings config(QString::fromStdString(absolute_filename), QSettings::IniFormat);
 
    config.beginGroup("PrimitiveExtraction");
 
    primitiveExtractor->updateParameters(
        verbose,
        config.value("MinPrimitiveSize", getProperty<double>("minimumSegmentSize").getValue())
            .toDouble(),
        config.value("MaxPrimitiveSize", getProperty<double>("maximumSegmentSize").getValue())
            .toDouble(),
        config.value("PlaneMaxIterations", getProperty<double>("planeMaximumIteration").getValue())
            .toDouble(),
        config
            .value("PlaneDistanceThreshold",
                   getProperty<float>("planeDistanceThreshold").getValue())
            .toFloat(),
        config.value("PlaneNormalDistance", getProperty<float>("planeNormalDistance").getValue())
            .toFloat(),
        config
            .value("CylinderMaxIterations",
                   getProperty<double>("cylinderMaximumIteration").getValue())
            .toDouble(),
        config
            .value("CylinderDistanceThreshold",
                   getProperty<float>("cylinderDistanceThreshold").getValue())
            .toFloat(),
        config.value("CylinderRadiusLimit", getProperty<float>("cylinderRadiousLimit").getValue())
            .toFloat(),
        config
            .value("SphereMaxIterations", getProperty<double>("sphereMaximumIteration").getValue())
            .toDouble(),
        config
            .value("SphereDistanceThreshold",
                   getProperty<float>("sphereDistanceThreshold").getValue())
            .toFloat(),
        config.value("SphereNormalDistance", getProperty<float>("sphereNormalDistance").getValue())
            .toFloat(),
        config
            .value("EuclideanClusteringTolerance",
                   getProperty<float>("euclideanClusteringTolerance").getValue())
            .toFloat(),
        config.value("OutlierDistanceThreshold", getProperty<float>("outlierThreshold").getValue())
            .toFloat(),
        config
            .value("PlaneCircularDistanceThreshold",
                   getProperty<float>("circularDistanceThreshold").getValue())
            .toFloat());
 
    config.endGroup();
}
 
visionx::PrimitiveExtractorParameters
armarx::PrimitiveExtractor::getParameters(const Ice::Current&)
{
    visionx::PrimitiveExtractorParameters parameters;
    parameters.minSegmentSize = primitiveExtractor->minSegmentSize;
    parameters.maxSegmentSize = primitiveExtractor->maxSegmentSize;
    parameters.planeMaxIterations = primitiveExtractor->planeMaxIteration;
    parameters.planeDistanceThreshold = primitiveExtractor->planeMaxIteration;
    parameters.planeNormalDistance = primitiveExtractor->planeNormalDistanceWeight;
    parameters.cylinderMaxIterations = primitiveExtractor->cylinderMaxIteration;
    parameters.cylinderDistanceThreshold = primitiveExtractor->cylinderDistanceThreshold;
    parameters.cylinderRadiusLimit = primitiveExtractor->cylinderRadiusLimit;
 
    parameters.sphereMaxIterations = primitiveExtractor->sphereMaxIteration;
    parameters.sphereDistanceThreshold = primitiveExtractor->sphereDistanceThreshold;
    parameters.sphereNormalDistance = primitiveExtractor->sphereNormalDistanceWeight;
    parameters.euclideanClusteringTolerance = primitiveExtractor->euclideanClusteringTolerance;
    parameters.outlierThreshold = primitiveExtractor->outlierThreshold;
    parameters.circularDistanceThreshold = primitiveExtractor->circleDistanceThreshold;
 
    return parameters;
}