PrimitiveVisualization.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    VisionX::ArmarXObjects::PrimitiveVisualization
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PrimitiveVisualization.h"
 
#include <pcl/common/colors.h>
#include <pcl/point_types.h>
 
#include <VirtualRobot/MathTools.h>
 
#include <MemoryX/core/MemoryXCoreObjectFactories.h>
#include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
#include <MemoryX/libraries/memorytypes/entity/EnvironmentalPrimitive.h>
 
 
using namespace armarx;
 
void
PrimitiveVisualization::onInitComponent()
{
    usingTopic("SegmentedPointCloud");
 
    usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
 
    primitiveColorFrame = viz::Color::blue(150, 200);
}
 
void
PrimitiveVisualization::onConnectComponent()
{
    workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(
        getProperty<std::string>("WorkingMemoryName").getValue());
    environmentalPrimitiveSegment = workingMemory->getEnvironmentalPrimitiveSegment();
}
 
void
PrimitiveVisualization::onDisconnectComponent()
{
}
 
void
PrimitiveVisualization::onExitComponent()
{
}
 
void
PrimitiveVisualization::reportNewPointCloudSegmentation(const Ice::Current& c)
{
    std::unique_lock<std::mutex> lock(mutex, std::try_to_lock);
 
    if (!lock.owns_lock())
    {
        ARMARX_INFO << deactivateSpam(3)
                    << "unable to process new geometric primitives. already processing previous "
                       "primitive set.";
        return;
    }
 
 
    ;
    layer.clear();
 
    std::vector<memoryx::EnvironmentalPrimitiveBasePtr> primitives =
        environmentalPrimitiveSegment->getEnvironmentalPrimitives();
    for (memoryx::EnvironmentalPrimitiveBasePtr& primitive : primitives)
    {
        viz::Color color = getPrimitiveColor(primitive);
        visualizePrimitive(primitive, color);
    }
 
    arviz.commit({layer});
}
 
viz::Color
PrimitiveVisualization::getPrimitiveColor(const memoryx::EnvironmentalPrimitiveBasePtr& primitive)
{
    if (primitive->getLabel())
    {
        viz::Color color;
        pcl::RGB c = pcl::GlasbeyLUT::at(primitive->getLabel() % pcl::GlasbeyLUT::size());
        color.r = c.r;
        color.g = c.g;
        color.b = c.b;
        color.a = 200; //primitive->getProbability()
        return color;
    }
    else
    {
        return viz::Color::blue(128, 128);
    }
}
 
void
PrimitiveVisualization::visualizePrimitive(const memoryx::EnvironmentalPrimitiveBasePtr& primitive,
                                           viz::Color color)
{
    if (primitive->ice_isA(memoryx::PlanePrimitiveBase::ice_staticId()))
    {
        visualizePlane(memoryx::PlanePrimitiveBasePtr::dynamicCast(primitive), color);
    }
    else if (primitive->ice_isA(memoryx::SpherePrimitiveBase::ice_staticId()))
    {
        visualizeSphere(memoryx::SpherePrimitiveBasePtr::dynamicCast(primitive), color);
    }
    else if (primitive->ice_isA(memoryx::CylinderPrimitiveBase::ice_staticId()))
    {
        visualizeCylinder(memoryx::CylinderPrimitiveBasePtr::dynamicCast(primitive), color);
    }
    else
    {
        ARMARX_WARNING << "Cannot visualize unknown primitive type: " << primitive->getName();
        return;
    }
}
 
void
PrimitiveVisualization::visualizePlane(const memoryx::PlanePrimitiveBasePtr& plane,
                                       viz::Color color)
{
    memoryx::PlanePrimitivePtr p = memoryx::PlanePrimitivePtr::dynamicCast(plane);
 
 
    Eigen::Matrix4f pose = FramedPosePtr::dynamicCast(p->getPose())->toEigen();
    Eigen::Vector3f dimensions = Vector3Ptr::dynamicCast(p->getOBBDimensions())->toEigen();
 
 
    if (getProperty<PLANE_VISUALIZATION_METHOD>("PlaneVisualizationMethod").getValue() == Obb)
    {
        // Old behavior: Visualize planes as boxes, while the depth of the box indicates the deviation of the
        // point cloud from the plane model
 
        viz::Box box(p->getId());
        box.size(dimensions);
        box.color(color);
        box.pose(pose);
 
        layer.add(box);
    }
    else if (getProperty<PLANE_VISUALIZATION_METHOD>("PlaneVisualizationMethod").getValue() ==
             Rectangle)
    {
        viz::Polygon rectangle(p->getId());
 
        rectangle.color(color);
        rectangle.lineColor(primitiveColorFrame);
 
 
        rectangle.addPoint(Eigen::Vector3f(
            (pose * Eigen::Vector4f(dimensions.x() / 2, dimensions.y() / 2, 0, 1)).head(3)));
        rectangle.addPoint(Eigen::Vector3f(
            (pose * Eigen::Vector4f(dimensions.x() / 2, -dimensions.y() / 2, 0, 1)).head(3)));
        rectangle.addPoint(Eigen::Vector3f(
            (pose * Eigen::Vector4f(-dimensions.x() / 2, -dimensions.y() / 2, 0, 1)).head(3)));
        rectangle.addPoint(Eigen::Vector3f(
            (pose * Eigen::Vector4f(-dimensions.x() / 2, dimensions.y() / 2, 0, 1)).head(3)));
 
 
        layer.add(rectangle);
    }
    else if (getProperty<PLANE_VISUALIZATION_METHOD>("PlaneVisualizationMethod").getValue() == Hull)
    {
 
        viz::Polygon polygon(p->getId());
        polygon.color(color);
        polygon.lineColor(primitiveColorFrame);
 
        for (const Vector3BasePtr& v : p->getGraspPoints())
        {
            polygon.addPoint(Vector3Ptr::dynamicCast(v)->toEigen());
        }
 
        layer.add(polygon);
    }
    else if (getProperty<PLANE_VISUALIZATION_METHOD>("PlaneVisualizationMethod").getValue() ==
             Inlier)
    {
        viz::PointCloud cloud(p->getId());
 
        for (const armarx::Vector3BasePtr& v : p->getInliers())
        {
            // ..todo:: add color
            cloud.addPoint(v->x, v->y, v->z);
        }
 
        layer.add(cloud);
    }
}
 
void
PrimitiveVisualization::visualizeSphere(const memoryx::SpherePrimitiveBasePtr& sphere,
                                        viz::Color color)
{
    memoryx::SpherePrimitivePtr p = memoryx::SpherePrimitivePtr::dynamicCast(sphere);
 
    Eigen::Vector3f center = Vector3Ptr::dynamicCast(p->getSphereCenter())->toEigen();
    float radius = p->getSphereRadius();
 
    viz::Sphere s(p->getId());
    s.position(center);
    s.radius(radius);
    s.color(color);
 
    layer.add(s);
}
 
void
PrimitiveVisualization::visualizeCylinder(const memoryx::CylinderPrimitiveBasePtr& cylinder,
                                          viz::Color color)
{
    memoryx::CylinderPrimitivePtr p = memoryx::CylinderPrimitivePtr::dynamicCast(cylinder);
    Eigen::Vector3f center = Vector3Ptr::dynamicCast(p->getCylinderPoint())->toEigen();
    Eigen::Vector3f direction = Vector3Ptr::dynamicCast(p->getCylinderAxisDirection())->toEigen();
 
    float radius = p->getCylinderRadius();
    float length = p->getLength();
 
    viz::Cylinder c(p->getId());
    c.position(center);
    c.height(length);
    c.direction(direction);
    c.radius(radius);
    layer.add(c);
}
 
armarx::PropertyDefinitionsPtr
PrimitiveVisualization::createPropertyDefinitions()
{
    return armarx::PropertyDefinitionsPtr(
        new PrimitiveVisualizationPropertyDefinitions(getConfigIdentifier()));
}