PrimitiveSetArmarX.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    AffordanceKitArmarX
 * @author     Peter Kaiser ( peter dot kaiser at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl.txt
 *             GNU General Public License
 */
 
#include "PrimitiveSetArmarX.h"
#include <AffordanceKit/primitives/Plane.h>
#include <AffordanceKit/primitives/Cylinder.h>
#include <AffordanceKit/primitives/Sphere.h>
#include <AffordanceKit/primitives/Box.h>
 
#include <ArmarXCore/observers/variant/TimestampVariant.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
 
#include <MemoryX/libraries/memorytypes/entity/EnvironmentalPrimitive.h>
#include <MemoryX/core/memory/SegmentUtilImplementations.h>
 
namespace AffordanceKitArmarX
{
    PrimitiveSetArmarX::PrimitiveSetArmarX() :
        AffordanceKit::PrimitiveSet()
    {
    }
 
    PrimitiveSetArmarX::PrimitiveSetArmarX(const AffordanceKit::PrimitiveSetPtr& primitiveSet)
    {
        for (auto& primitive : *primitiveSet)
        {
            push_back(primitive);
        }
    }
 
    PrimitiveSetArmarX::PrimitiveSetArmarX(const memoryx::EnvironmentalPrimitiveSegmentBasePrx& segment, long timestamp)
    {
        memoryx::PlanePrimitiveBaseList planes;
        memoryx::CylinderPrimitiveBaseList cylinders;
        memoryx::SpherePrimitiveBaseList spheres;
        memoryx::BoxPrimitiveBaseList boxes;
 
 
        if (timestamp == 0)
        {
            planes = segment->getMostRecentPlanes();
            cylinders = segment->getMostRecentCylinders();
            spheres = segment->getMostRecentSpheres();
            boxes = segment->getMostRecentBoxes();
        }
        else
        {
            planes = segment->getPlanesByTimestamp(new armarx::TimestampVariant(timestamp));
            cylinders = segment->getCylindersByTimestamp(new armarx::TimestampVariant(timestamp));
            spheres = segment->getSpheresByTimestamp(new armarx::TimestampVariant(timestamp));
            boxes = segment->getBoxesByTimestamp(new armarx::TimestampVariant(timestamp));
        }
 
        // Planar primitives
        for (auto& primitive : planes)
        {
            memoryx::PointList originalPoints = primitive->getGraspPoints();
 
            std::vector<Eigen::Vector3f> convexHull(originalPoints.size());
            for (unsigned int i = 0; i < originalPoints.size(); i++)
            {
                convexHull[i] = armarx::Vector3Ptr::dynamicCast(originalPoints[i])->toEigen();
            }
 
            AffordanceKit::PrimitivePtr plane(new AffordanceKit::Plane(convexHull));
            plane->setId(primitive->getId());
            plane->setSampling(armarx::MatrixFloatPtr::dynamicCast(primitive->getSampling())->toEigen());
            plane->setTimestamp(primitive->getTime()->timestamp);
            plane->setLabel(primitive->getLabel());
            push_back(plane);
        }
 
        // Cylindrical primitives
        for (auto& primitive : cylinders)
        {
            Eigen::Vector3f basePoint = armarx::Vector3Ptr::dynamicCast(primitive->getCylinderPoint())->toEigen();
            Eigen::Vector3f direction = armarx::Vector3Ptr::dynamicCast(primitive->getCylinderAxisDirection())->toEigen();
 
            AffordanceKit::PrimitivePtr cylinder(new AffordanceKit::Cylinder(basePoint, direction, primitive->getLength(), primitive->getCylinderRadius()));
            cylinder->setId(primitive->getId());
            cylinder->setSampling(armarx::MatrixFloatPtr::dynamicCast(primitive->getSampling())->toEigen());
            cylinder->setTimestamp(primitive->getTime()->timestamp);
            cylinder->setLabel(primitive->getLabel());
            push_back(cylinder);
        }
 
        // Spherical primitives
        for (auto& primitive : spheres)
        {
            Eigen::Vector3f center = armarx::Vector3Ptr::dynamicCast(primitive->getSphereCenter())->toEigen();
 
            AffordanceKit::PrimitivePtr sphere(new AffordanceKit::Sphere(center, primitive->getSphereRadius()));
            sphere->setId(primitive->getId());
            sphere->setTimestamp(primitive->getTime()->timestamp);
            sphere->setSampling(armarx::MatrixFloatPtr::dynamicCast(primitive->getSampling())->toEigen());
            push_back(sphere);
        }
 
        // Box primitives
        for (auto& primitive : boxes)
        {
            Eigen::Matrix4f pose = armarx::PosePtr::dynamicCast(primitive->getPose())->toEigen();
            Eigen::Vector3f dimensions = armarx::Vector3Ptr::dynamicCast(primitive->getOBBDimensions())->toEigen();
 
            AffordanceKit::PrimitivePtr box(new AffordanceKit::Box(pose, dimensions));
            box->setId(primitive->getId());
            box->setTimestamp(primitive->getTime()->timestamp);
            box->setSampling(armarx::MatrixFloatPtr::dynamicCast(primitive->getSampling())->toEigen());
            push_back(box);
        }
    }
 
    void PrimitiveSetArmarX::writeToMemory(const memoryx::EnvironmentalPrimitiveSegmentBasePrx& segment) const
    {
        if (size() == 0)
        {
            // No primitives to export
            return;
        }
 
        std::vector<memoryx::EntityBasePtr> newPrimitives;
        for (auto& primitive : *this)
        {
            memoryx::EnvironmentalPrimitiveBasePtr p;
 
            if (AffordanceKit::Plane* pl = dynamic_cast<AffordanceKit::Plane*>(primitive.get()))
            {
                const std::vector<Eigen::Vector3f>& ch = pl->getConvexHull();
 
                memoryx::PointList convexHull;
                convexHull.reserve(ch.size());
                for (auto& p : ch)
                {
                    convexHull.push_back(new armarx::Vector3(p));
                }
 
                p = new memoryx::PlanePrimitive();
                p->setGraspPoints(convexHull);
            }
            else if (AffordanceKit::Cylinder* c = dynamic_cast<AffordanceKit::Cylinder*>(primitive.get()))
            {
                memoryx::CylinderPrimitivePtr p2(new memoryx::CylinderPrimitive());
                p2->setCylinderRadius(c->getRadius());
                p2->setCylinderPoint(new armarx::Vector3(c->getBasePoint()));
                p2->setCylinderAxisDirection(new armarx::Vector3(c->getDirection()));
                p2->setLength(c->getLength());
 
                p = p2;
            }
            else if (AffordanceKit::Sphere* s = dynamic_cast<AffordanceKit::Sphere*>(primitive.get()))
            {
                memoryx::SpherePrimitivePtr p2(new memoryx::SpherePrimitive());
                p2->setSphereCenter(new armarx::Vector3(s->getCenter()));
                p2->setSphereRadius(s->getRadius());
 
                p = p2;
            }
            else if (AffordanceKit::Box* b = dynamic_cast<AffordanceKit::Box*>(primitive.get()))
            {
                memoryx::BoxPrimitivePtr p2(new memoryx::BoxPrimitive());
                p2->setPose(new armarx::FramedPose(b->getPose(), "", ""));
                p2->setOBBDimensions(new armarx::Vector3(b->getDimensions()));
 
                p = p2;
            }
 
            p->setName("environmentalPrimitive_" + primitive->getId());
            p->setId(primitive->getId());
 
            ARMARX_INFO << "Writing primitive to memory: ID=" << p->getId() << ", name=" << p->getName();
            p->setCircularityProbability(primitive->isCircular());
            p->setTime(new armarx::TimestampVariant(primitive->getTimestamp()));
            p->setLabel(primitive->getLabel());
 
            armarx::MatrixFloatBasePtr s(new armarx::MatrixFloat(primitive->getSampling()));
            p->setSampling(s);
 
 
            newPrimitives.push_back(p);
        }
 
        segment->upsertEntityList(newPrimitives);
    }
}