AffordancePipelineVisualization.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 Lesser General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * 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 Lesser 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     Peter Kaiser (peter dot kaiser at kit dot edu)
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#include "AffordancePipelineVisualization.h"
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
#include <MemoryX/libraries/memorytypes/entity/Affordance.h>
#include <MemoryX/libraries/memorytypes/entity/EnvironmentalPrimitive.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
 
#include <VisionX/libraries/AffordanceKitArmarX/UnimanualAffordanceArmarX.h>
#include <VisionX/libraries/AffordanceKitArmarX/BimanualAffordanceArmarX.h>
 
#include <VirtualRobot/MathTools.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <pcl/point_types.h>
#include <pcl/common/colors.h>
 
#include <memory>
 
#include <fstream>
#include <cmath>
 
// Alternate layer names in order to have safe clears,
// even in case of overtaking TCP calls
#define PRIMITIVE_LAYER_NAME(n)                    (std::string("primitivesLayer") + std::string(((n)%2 == 0)? "" : "_2"))
#define PRIMITIVE_LABELS_LAYER_NAME(n)             (std::string("primitiveLabelsLayer") + std::string(((n)%2 == 0)? "" : "_2"))
#define PRIMITIVE_FRAMES_LAYER_NAME(n)             (std::string("primitiveFramesLayer") + std::string(((n)%2 == 0)? "" : "_2"))
#define PRIMITIVE_GRASP_POINTS_LAYER_NAME(n)       (std::string("graspPointLayer") + std::string(((n)%2 == 0)? "" : "_2"))
#define AFFORDANCES_LAYER_NAME(n)                  (std::string("affordancesLayer") + std::string(((n)%2 == 0)? "" : "_2"))
 
int alternatingPrimitiveVisualizationIndex = 0;
int alternatingAffordanceVisualizationIndex = 0;
int alternatingDebugInfoVisualizationIndex = 0;
 
using namespace armarx;
 
AffordancePipelineVisualization::AffordancePipelineVisualization() :
    affordanceLabelSize(18),
    primitiveVisualizationEnabled(true),
    affordanceVisualizationEnabled(true),
    debugVisualizationEnabled(false),
    visualizationStyle(AffordanceVisualizationStyle::eAffordanceLabels)
{
}
 
void AffordancePipelineVisualization::onInitComponent()
{
    offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
    offeringTopic(getProperty<std::string>("VisualizationUpdateTopicName").getValue());
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
    usingTopic("ExtractedAffordances");
    usingTopic("SegmentedPointCloud");
 
    primitiveColor = {0.4, 0.6, 1.0, 0.7};
    primitiveColorFrame = {0.2, 0.4, 0.8, 0.7};
    affordanceLabelColor = {0.96, 0.82, 0.3, 1};
 
    primitiveVisualizationEnabled = getProperty<bool>("EnablePrimitiveVisualization").getValue();
    affordanceVisualizationEnabled = getProperty<bool>("EnableAffordanceVisualization").getValue();
    debugVisualizationEnabled = getProperty<bool>("EnableDebugVisualization").getValue();
 
    std::string beliefFunction = getProperty<std::string>("VisualizeAffordanceBeliefFunction").getValue();
 
    if (beliefFunction != "")
    {
        std::vector<std::string> primitives;
 
        std::vector<std::string> affordances;
        affordances.push_back(beliefFunction);
 
        configureAffordanceVisualization(eAffordanceBeliefFunctions, primitives, affordances, getProperty<float>("MinExpectedProbability").getValue());
    }
}
 
void AffordancePipelineVisualization::onConnectComponent()
{
 
 
    debugDrawerTopicPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
    if (!debugDrawerTopicPrx)
    {
        ARMARX_ERROR << "Failed to obtain debug drawer proxy";
        return;
    }
 
    affordanceVisualizationTopicPrx = getTopic<AffordancePipelineVisualizationListenerPrx>(getProperty<std::string>("VisualizationUpdateTopicName").getValue());
    if (!affordanceVisualizationTopicPrx)
    {
        ARMARX_ERROR << "Failed to obtain affordance visualization topic proxy";
        return;
    }
 
    usingTopic(getProperty<std::string>("DebugDrawerSelectionsTopicName").getValue());
    offeringTopic(getProperty<std::string>("VisualizationUpdateTopicName").getValue());
 
    std::string fileName = getProperty<std::string>("FileName").getValue();
    if(!fileName.empty())
    {
        ARMARX_INFO << "Visualizing affordances from file " << VAROUT(fileName);
        scene.reset(new AffordanceKitArmarX::SceneArmarX());
        scene->load(fileName);
        visualize();
        ARMARX_INFO << "Done visualizing affordances.";
        return;   
    }
 
    workingMemoryPrx = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
    if (!workingMemoryPrx)
    {
        ARMARX_ERROR << "Failed to obtain working memory proxy";
        return;
    }
 
    affordanceSegment = workingMemoryPrx->getAffordanceSegment();
    if (!affordanceSegment)
    {
        ARMARX_ERROR << "Failed to obtain affordance segment pointer";
        return;
    }
 
    environmentalPrimitiveSegment = workingMemoryPrx->getEnvironmentalPrimitiveSegment();
    if (!environmentalPrimitiveSegment)
    {
        ARMARX_ERROR << "Failed to obtain environmental primitive segment pointer";
        return;
    }
}
 
void AffordancePipelineVisualization::onDisconnectComponent()
{
}
 
void AffordancePipelineVisualization::onExitComponent()
{
}
 
PropertyDefinitionsPtr AffordancePipelineVisualization::createPropertyDefinitions()
{
    return PropertyDefinitionsPtr(new AffordancePipelineVisualizationPropertyDefinitions(getConfigIdentifier()));
}
 
void AffordancePipelineVisualization::reportNewPointCloudSegmentation(const Ice::Current& c)
{
    ARMARX_INFO << "Receiving new primitives";
 
    if (getProperty<bool>("NoAffordanceExtractionConfigured").getValue())
    {
        sceneMutex.lock();
        scene.reset(new AffordanceKitArmarX::SceneArmarX(environmentalPrimitiveSegment, affordanceSegment));
        sceneMutex.unlock();
 
        visualize();
    }
}
 
void AffordancePipelineVisualization::reportNewAffordances(const Ice::Current& c)
{
    ARMARX_INFO << "Receiving new affordances";
 
    if (!getProperty<bool>("NoAffordanceExtractionConfigured").getValue())
    {
        sceneMutex.lock();
        scene.reset(new AffordanceKitArmarX::SceneArmarX(environmentalPrimitiveSegment, affordanceSegment));
        sceneMutex.unlock();
 
        visualize();
    }
}
 
void AffordancePipelineVisualization::selectPrimitive(const std::string& id, const Ice::Current& c)
{
    selections.insert(id);
 
    if (getProperty<bool>("EnableDebugDrawerSelections").getValue())
    {
        debugDrawerTopicPrx->select(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), id);
    }
}
 
void AffordancePipelineVisualization::deselectPrimitive(const std::string& id, const Ice::Current& c)
{
    selections.erase(id);
 
    if (getProperty<bool>("EnableDebugDrawerSelections").getValue())
    {
        debugDrawerTopicPrx->deselect(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), id);
    }
}
 
void AffordancePipelineVisualization::deselectAllPrimitives(const Ice::Current& c)
{
    selections.clear();
 
    if (getProperty<bool>("EnableDebugDrawerSelections").getValue())
    {
        debugDrawerTopicPrx->clearSelections(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex));
    }
}
 
void AffordancePipelineVisualization::enableVisualization(bool enablePrimitives, bool enableAffordances, bool enableDebugInformation, const Ice::Current& c)
{
    ARMARX_INFO << "Visualization configuration changed: primitives=" << (enablePrimitives ? "enabled" : "disabled")
                << ", affordances="  << (enableAffordances ? "enabled" : "disabled")
                << ", debug=" << (enableDebugInformation ? "enabled" : "disabled");
 
    bool updatePrimitives = (primitiveVisualizationEnabled != enablePrimitives);
    bool updateAffordances = (affordanceVisualizationEnabled != enableAffordances);
    bool updateDebugInfo = (debugVisualizationEnabled != enableDebugInformation);
 
    primitiveVisualizationEnabled = enablePrimitives;
    affordanceVisualizationEnabled = enableAffordances;
    debugVisualizationEnabled = enableDebugInformation;
 
    visualize(updatePrimitives, updateAffordances, updateDebugInfo);
}
 
void AffordancePipelineVisualization::configureAffordanceVisualization(AffordanceVisualizationStyle style, const std::vector<std::string>& primitives, const std::vector<std::string>& affordances, float minExpectedProbability, const Ice::Current& c)
{
    visualizationStyle = style;
    visualizationPrimitiveSet = primitives;
    visualizationAffordanceSet = affordances;
    visualizationMinExpectedProbability = minExpectedProbability;
 
    visualize(true, true, false);
}
 
void AffordancePipelineVisualization::triggerVisualization(const Ice::Current& c)
{
    visualize();
}
 
void AffordancePipelineVisualization::visualizeScene(const AffordanceKit::ScenePtr& scene, bool updatePrimitives, bool updateAffordances, bool updateDebugInfo)
{
    if (primitiveVisualizationEnabled && updatePrimitives)
    {
        visualizePrimitives(scene->getPrimitives());
    }
 
    if (affordanceVisualizationEnabled && updateAffordances)
    {
        visualizeAffordances(scene);
    }
 
    if (debugVisualizationEnabled && updateDebugInfo)
    {
        visualizeDebugInformation(scene);
    }
}
 
void AffordancePipelineVisualization::visualizePrimitives(const AffordanceKit::PrimitiveSetPtr& primitives)
{
    for (auto& primitive : *primitives)
    {
        visualizePrimitive(primitive);
    }
}
 
void AffordancePipelineVisualization::visualizePrimitive(const AffordanceKit::PrimitivePtr& primitive)
{
    if (auto plane = std::dynamic_pointer_cast<AffordanceKit::Plane>(primitive))
    {
        visualizePlane(plane);
    }
    else if (auto cyl = std::dynamic_pointer_cast<AffordanceKit::Cylinder>(primitive))
    {
        visualizeCylinder(cyl);
    }
    else if (auto sphere = std::dynamic_pointer_cast<AffordanceKit::Sphere>(primitive))
    {
        visualizeSphere(sphere);
    }
    else if (auto box = std::dynamic_pointer_cast<AffordanceKit::Box>(primitive))
    {
        visualizeBox(box);
    }
    else
    {
        ARMARX_WARNING << "Attempt to visualize primitive of unknown type";
        return;
    }
}
 
void AffordancePipelineVisualization::visualizeAffordances(const AffordanceKit::ScenePtr& scene)
{
    AffordanceKit::PrimitiveSetPtr primitives = scene->getPrimitives();
    const std::vector<AffordanceKit::AffordancePtr>& affordances = scene->getAffordances();
 
    for (auto& primitive : *primitives)
    {
        if (visualizationPrimitiveSet.size() > 0 && std::find(visualizationPrimitiveSet.begin(), visualizationPrimitiveSet.end(), primitive->getId()) == visualizationPrimitiveSet.end())
        {
            // This primitive is not configured to be visualized
            continue;
        }
 
        std::vector<std::string> labels;
 
        int i = 0;
        for (auto& affordance : affordances)
        {
            if (visualizationAffordanceSet.size() > 0 && std::find(visualizationAffordanceSet.begin(), visualizationAffordanceSet.end(), affordance->getName()) == visualizationAffordanceSet.end())
            {
                // This affordance is not configured to be visualized
                continue;
            }
 
            if (visualizationStyle == AffordanceVisualizationStyle::eAffordanceLabels && affordance->existsForPrimitive(primitive))
            {
                labels.push_back(affordance->getName());
            }
            else if (visualizationStyle == AffordanceVisualizationStyle::eAffordanceBeliefFunctions)
            {
                visualizeAffordance(affordance, primitive, i++);
            }
        }
 
        if (visualizationStyle == AffordanceVisualizationStyle::eAffordanceLabels)
        {
            visualizeAffordanceLabels(primitive, labels);
        }
    }
}
 
void AffordancePipelineVisualization::visualizeAffordance(const AffordanceKit::AffordancePtr& affordance, const AffordanceKit::PrimitivePtr& primitive, unsigned int index)
{
    std::string id = "affordance_" + affordance->getName() + "_" + std::to_string(index) + "_" + primitive->getId();
 
    if (affordance->getTimestamp() != primitive->getTimestamp())
    {
        IceUtil::Time affordanceTimestamp = IceUtil::Time::microSeconds(affordance->getTimestamp());
        IceUtil::Time primitiveTimestamp = IceUtil::Time::microSeconds(primitive->getTimestamp());
 
        std::string t1 = affordanceTimestamp.toDateTime().substr(affordanceTimestamp.toDateTime().find(' ') + 1);
        std::string t2 = primitiveTimestamp.toDateTime().substr(primitiveTimestamp.toDateTime().find(' ') + 1);
 
        ARMARX_WARNING << "Timestamp of visualized affordance does not match the primitive timestamp (Affordance: " << t1 << ", Primitive: " << t2 << ")";
    }
 
    if (visualizationStyle == AffordanceVisualizationStyle::eAffordanceBeliefFunctions)
    {
        if (auto unimanualA = std::dynamic_pointer_cast<AffordanceKit::UnimanualAffordance>(affordance))
        {
            AffordanceKitArmarX::UnimanualAffordanceArmarX a(unimanualA);
            a.visualize(debugDrawerTopicPrx, AFFORDANCES_LAYER_NAME(alternatingAffordanceVisualizationIndex), id, visualizationMinExpectedProbability, primitive);
        }
        else if (auto bimanualA = std::dynamic_pointer_cast<AffordanceKit::BimanualAffordance>(affordance))
        {
            AffordanceKitArmarX::BimanualAffordanceArmarX a(bimanualA);
            a.visualize(debugDrawerTopicPrx, AFFORDANCES_LAYER_NAME(alternatingAffordanceVisualizationIndex), id, visualizationMinExpectedProbability, primitive);
        }
    }
    else
    {
        ARMARX_WARNING << "Unknown affordance visualization style";
    }
}
 
void AffordancePipelineVisualization::visualizeAffordanceLabels(const AffordanceKit::PrimitivePtr& primitive, const std::vector<std::string>& labels)
{
    std::string label;
 
    for (unsigned int i = 0; i < labels.size(); i++)
    {
        label += labels[i] + ((i == labels.size() - 1) ? "" : ", ");
    }
 
    Eigen::Vector3f c = primitive->getCenter();
    debugDrawerTopicPrx->setTextVisu(AFFORDANCES_LAYER_NAME(alternatingAffordanceVisualizationIndex), primitive->getId() + "_labels", label, new Vector3(c), affordanceLabelColor, affordanceLabelSize);
}
 
void AffordancePipelineVisualization::visualizeDebugInformation(const AffordanceKit::ScenePtr& scene)
{
}
 
void AffordancePipelineVisualization::visualizePlane(const AffordanceKit::PlanePtr& plane)
{
    const std::vector<Eigen::Vector3f>& hull = plane->getConvexHull();
 
    std::vector<Vector3BasePtr> polygon;
    for (auto x : hull)
    {
        polygon.push_back(Vector3Ptr(new Vector3(x)));
    }
 
    debugDrawerTopicPrx->setPolygonVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), plane->getId(), polygon, primitiveColor, primitiveColorFrame, 2);
}
 
void AffordancePipelineVisualization::visualizeSphere(const AffordanceKit::SpherePtr& sphere)
{
    Vector3Ptr center(new Vector3(sphere->getCenter()));
    debugDrawerTopicPrx->setSphereVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), sphere->getId(), center, primitiveColor, sphere->getRadius());
}
 
void AffordancePipelineVisualization::visualizeCylinder(const AffordanceKit::CylinderPtr& cylinder)
{
    float radius = cylinder->getRadius();
    float length = cylinder->getLength();
    Vector3Ptr point(new Vector3(cylinder->getBasePoint()));
    Vector3Ptr direction(new Vector3(cylinder->getDirection()));
 
    debugDrawerTopicPrx->setCylinderVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), cylinder->getId(), point, direction, length, radius, primitiveColor);
}
 
void AffordancePipelineVisualization::visualizeBox(const AffordanceKit::BoxPtr& box)
{
    //debugDrawerTopicPrx->setBoxVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId(), new Pose(box->getPose()), new Vector3(box->getDimensions()),  primitiveColor);
 
    Eigen::Matrix4f pose = box->getPose();
    Eigen::Vector3f dim = box->getDimensions();
 
    std::vector<std::vector<Eigen::Vector3f>> sides;
    sides.resize(6);
 
    sides[0].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[0].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[0].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[0].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
 
    sides[1].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[1].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[1].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[1].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
 
    sides[2].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[2].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[2].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[2].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
 
    sides[3].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[3].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[3].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[3].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
 
    sides[4].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[4].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[4].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[4].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) + (dim.z() / 2) * pose.block<3, 1>(0, 2));
 
    sides[5].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[5].push_back(pose.block<3, 1>(0, 3) + (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[5].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) - (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
    sides[5].push_back(pose.block<3, 1>(0, 3) - (dim.x() / 2) * pose.block<3, 1>(0, 0) + (dim.y() / 2) * pose.block<3, 1>(0, 1) - (dim.z() / 2) * pose.block<3, 1>(0, 2));
 
    for (unsigned int i = 0; i < sides.size(); i++)
    {
        debugDrawerTopicPrx->setLineVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId() + "_side" + std::to_string(i) + "_line_1", new Vector3(sides[i][0]), new Vector3(sides[i][1]), 3, primitiveColor);
        debugDrawerTopicPrx->setLineVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId() + "_side" + std::to_string(i) + "_line_2", new Vector3(sides[i][1]), new Vector3(sides[i][2]), 3, primitiveColor);
        debugDrawerTopicPrx->setLineVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId() + "_side" + std::to_string(i) + "_line_3", new Vector3(sides[i][2]), new Vector3(sides[i][3]), 3, primitiveColor);
        debugDrawerTopicPrx->setLineVisu(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), box->getId() + "_side" + std::to_string(i) + "_line_4", new Vector3(sides[i][3]), new Vector3(sides[i][0]), 3, primitiveColor);
    }
}
 
void AffordancePipelineVisualization::visualize(bool updatePrimitives, bool updateAffordances, bool updateDebugInfo)
{
    sceneMutex.lock();
 
    if (!scene)
    {
        sceneMutex.unlock();
        return;
    }
 
    ARMARX_INFO << "Current scene contains: " << scene->getPrimitives()->size() << " primitives";
    ARMARX_INFO << "Current scene contains: " << scene->getUnimanualAffordances().size() << " unimanual affordances";
    ARMARX_INFO << "Current scene contains: " << scene->getBimanualAffordances().size() << " bimanual affordances";
    ARMARX_INFO << "Current scene contains: " << scene->getAffordances().size() << " affordances in total";
 
    if (updatePrimitives)
    {
        ARMARX_INFO << "Visualizing primitives";
 
        alternatingPrimitiveVisualizationIndex++;
        debugDrawerTopicPrx->clearLayer(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex - 1));
    }
 
    if (updateAffordances)
    {
        ARMARX_INFO << "Visualizing affordances";
 
        alternatingAffordanceVisualizationIndex++;
        debugDrawerTopicPrx->clearLayer(AFFORDANCES_LAYER_NAME(alternatingAffordanceVisualizationIndex - 1));
    }
 
    if (updateDebugInfo)
    {
        ARMARX_INFO << "Visualizing debug information";
 
        alternatingDebugInfoVisualizationIndex++;
        debugDrawerTopicPrx->clearLayer(PRIMITIVE_LABELS_LAYER_NAME(alternatingDebugInfoVisualizationIndex - 1));
        debugDrawerTopicPrx->clearLayer(PRIMITIVE_FRAMES_LAYER_NAME(alternatingDebugInfoVisualizationIndex - 1));
        debugDrawerTopicPrx->clearLayer(PRIMITIVE_GRASP_POINTS_LAYER_NAME(alternatingDebugInfoVisualizationIndex - 1));
    }
 
    visualizeScene(scene, updatePrimitives, updateAffordances, updateDebugInfo);
 
    sceneMutex.unlock();
 
    // Update selections
    if (getProperty<bool>("EnableDebugDrawerSelections").getValue())
    {
        std::set<std::string> previousSelections = selections;
 
        debugDrawerTopicPrx->enableSelections(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex));
 
        debugDrawerTopicPrx->disableSelections(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex - 1));
        debugDrawerTopicPrx->clearSelections(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex - 1));
 
        for (auto& primitive : previousSelections)
        {
            ARMARX_INFO << "Selecting: " << primitive;
            debugDrawerTopicPrx->select(PRIMITIVE_LAYER_NAME(alternatingPrimitiveVisualizationIndex), primitive);
        }
    }
 
    affordanceVisualizationTopicPrx->reportAffordanceVisualizationUpdate();
}
 
void AffordancePipelineVisualization::exportVisualizationAsIV(const std::string& filename, const Ice::Current& c)
{
    // Currently there is no way to export multiple layers at once => exKport whole scene
    ARMARX_INFO << "Exporting scene to '" << filename << "'";
    debugDrawerTopicPrx->exportScene(filename);
}
 
void AffordancePipelineVisualization::exportVisualizationAsJSON(const std::string& filename, const Ice::Current& c)
{
    JSONObjectPtr serializer = new JSONObject(ElementType::eArray);
 
    memoryx::EnvironmentalPrimitiveBaseList primitives = environmentalPrimitiveSegment->getEnvironmentalPrimitives();
 
    for (auto& primitive : primitives)
    {
        JSONObjectPtr s = new JSONObject;
 
        if (primitive->ice_isA(memoryx::PlanePrimitiveBase::ice_staticId()))
        {
            memoryx::PlanePrimitiveBasePtr::dynamicCast(primitive)->serialize(s);
        }
        else if (primitive->ice_isA(memoryx::SpherePrimitiveBase::ice_staticId()))
        {
            memoryx::SpherePrimitiveBasePtr::dynamicCast(primitive)->serialize(s);
        }
        else if (primitive->ice_isA(memoryx::CylinderPrimitiveBase::ice_staticId()))
        {
            memoryx::CylinderPrimitiveBasePtr::dynamicCast(primitive)->serialize(s);
        }
        else
        {
            ARMARX_WARNING << "Could not serialize unknown primitive type";
        }
 
        serializer->append(s);
    }
 
    std::ofstream out;
    out.open(filename, std::ios_base::trunc);
 
    if (out.is_open())
    {
        out << serializer->toString();
        out.close();
        ARMARX_INFO << "Scene exported as JSON to " << filename;
    }
    else
    {
        ARMARX_WARNING << "Failed to export scene as JSON to " << filename;
    }
}
 
void AffordancePipelineVisualization::clearVisualization(const Ice::Current& c)
{
    ARMARX_INFO << "Clearing visualization";
 
    debugDrawerTopicPrx->clearLayer(PRIMITIVE_LAYER_NAME(0));
    debugDrawerTopicPrx->clearLayer(PRIMITIVE_LAYER_NAME(1));
 
    debugDrawerTopicPrx->clearLayer(PRIMITIVE_LABELS_LAYER_NAME(0));
    debugDrawerTopicPrx->clearLayer(PRIMITIVE_LABELS_LAYER_NAME(1));
 
    debugDrawerTopicPrx->clearLayer(PRIMITIVE_FRAMES_LAYER_NAME(0));
    debugDrawerTopicPrx->clearLayer(PRIMITIVE_FRAMES_LAYER_NAME(1));
 
    debugDrawerTopicPrx->clearLayer(PRIMITIVE_GRASP_POINTS_LAYER_NAME(0));
    debugDrawerTopicPrx->clearLayer(PRIMITIVE_GRASP_POINTS_LAYER_NAME(1));
 
    debugDrawerTopicPrx->clearLayer(AFFORDANCES_LAYER_NAME(0));
    debugDrawerTopicPrx->clearLayer(AFFORDANCES_LAYER_NAME(1));
}
 
void AffordancePipelineVisualization::reportSelectionChanged(const DebugDrawerSelectionList& selectedObjects, const Ice::Current&)
{
    std::vector<std::string> tmpSelections;
    selections.clear();
 
    ARMARX_INFO << "Selections cleared";
 
    for (auto& object : selectedObjects)
    {
        if (simox::alg::starts_with(object.layerName, "primitivesLayer"))
        {
            ARMARX_INFO << "Object " << object.elementName << " selected";
 
            selections.insert(object.elementName);
            tmpSelections.push_back(object.elementName);
        }
    }
 
    affordanceVisualizationTopicPrx->reportPrimitiveSelectionChanged(tmpSelections);
}