PrimitiveFusion.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     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PrimitiveFusion.h"
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
 
using namespace armarx;
 
PrimitiveFusion::PrimitiveFusion()
{
}
 
PrimitiveFusion::~PrimitiveFusion()
{
}
 
// todo rename
bool
PrimitiveFusion::testBoxIntersection(Eigen::Matrix4f leftPose,
                                     Eigen::Matrix4f rightPose,
                                     Eigen::Vector3f leftExtent,
                                     Eigen::Vector3f rightExtent)
{
    Eigen::Matrix3Xf axes(3, 15);
 
    axes.block<3, 3>(0, 0) = leftPose.block<3, 3>(0, 0);
    axes.block<3, 3>(0, 3) = rightPose.block<3, 3>(0, 0);
 
 
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            Eigen::Vector3f a = leftPose.col(i).head<3>();
            Eigen::Vector3f b = rightPose.col(j).head<3>();
            int idx = 6 + (i * 3 + j);
            Eigen::Vector3f c = a.cross(b);
            if (a.norm() == 0)
            {
                axes.col(idx) = Eigen::Vector3f::Zero();
            }
            else
            {
                axes.col(idx) = c / c.norm();
            }
        }
    }
 
    for (int i = 0; i < 15; i++)
    {
        float ra = 0.0;
        float rb = 0.0;
 
        Eigen::Vector3f axis = axes.col(i);
 
        if (axis.norm() == 0)
        {
            continue;
        }
 
        for (int j = 0; j < 3; j++)
        {
            Eigen::Vector3f a = leftPose.col(j).head<3>();
            Eigen::Vector3f b = rightPose.col(j).head<3>();
 
            ra += fabs(axis.dot(a) * leftExtent(j));
            rb += fabs(axis.dot(b) * rightExtent(j));
        }
 
        Eigen::Vector3f t = projectVector(axis, leftPose.col(3).head<3>());
        t = t - projectVector(axis, rightPose.col(3).head<3>());
 
        if (t.norm() > (ra + rb))
        {
            return false;
        }
    }
 
    return true;
}
 
Eigen::Vector3f
PrimitiveFusion::projectVector(Eigen::Vector3f axis, Eigen::Vector3f u)
{
    float v = u.dot(axis) / axis.squaredNorm();
    return axis * v;
}
 
void
PrimitiveFusion::getIntersectingPrimitives(
    memoryx::EnvironmentalPrimitiveBaseList& primitives,
    memoryx::EnvironmentalPrimitiveBasePtr primitive,
    memoryx::EnvironmentalPrimitiveBaseList& intersectingPrimitives,
    float eps)
{
    Eigen::Matrix4f leftPose = FramedPosePtr::dynamicCast(primitive->getPose())->toEigen();
    Eigen::Vector3f leftExtent =
        Vector3Ptr::dynamicCast(primitive->getOBBDimensions())->toEigen() / 2.0 +
        eps * Eigen::Vector3f::Ones();
 
    for (memoryx::EnvironmentalPrimitiveBasePtr& p : primitives)
    {
        Eigen::Matrix4f rightPose = FramedPosePtr::dynamicCast(p->getPose())->toEigen();
        Eigen::Vector3f rightExtent =
            Vector3Ptr::dynamicCast(p->getOBBDimensions())->toEigen() / 2.0 +
            eps * Eigen::Vector3f::Ones();
 
        if (primitive->ice_staticId() != p->ice_staticId())
        {
            continue;
        }
        if (primitive->getId() == p->getId())
        {
            continue;
        }
        if (testBoxIntersection(leftPose, rightPose, leftExtent, rightExtent))
        {
            intersectingPrimitives.push_back(p);
        }
    }
}
 
void
PrimitiveFusion::getIntersectingSimilarPrimitives(
    memoryx::EnvironmentalPrimitiveBaseList& primitives,
    memoryx::EnvironmentalPrimitiveBasePtr primitive,
    memoryx::EnvironmentalPrimitiveBaseList& intersectingPrimitives,
    float eps)
{
    memoryx::EnvironmentalPrimitiveBaseList candidates;
 
    getIntersectingPrimitives(primitives, primitive, candidates, eps);
 
    for (memoryx::EnvironmentalPrimitiveBasePtr p : candidates)
    {
        if (testPlane(p, primitive))
        {
            intersectingPrimitives.push_back(p);
        }
 
        else if (testCylinder(p, primitive))
        {
            intersectingPrimitives.push_back(p);
        }
    }
}
 
bool
PrimitiveFusion::testCylinder(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive,
                              memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive)
{
    memoryx::CylinderPrimitivePtr leftCylinder =
        memoryx::CylinderPrimitivePtr::dynamicCast(leftPrimitive);
    memoryx::CylinderPrimitivePtr rightCylinder =
        memoryx::CylinderPrimitivePtr::dynamicCast(rightPrimitive);
 
    if (!leftCylinder || !rightCylinder)
    {
        return false;
    }
 
    Eigen::Vector3f leftAxisDirection =
        Vector3Ptr::dynamicCast(leftCylinder->getCylinderAxisDirection())->toEigen();
    Eigen::Vector3f rightAxisDirection =
        Vector3Ptr::dynamicCast(rightCylinder->getCylinderAxisDirection())->toEigen();
 
    float r = leftAxisDirection.dot(rightAxisDirection);
 
    if (std::acos(r) > pcl::deg2rad(15.0))
    {
        return false;
    }
 
 
    /*
    // radius should be almost the same
    if (std::fabs(leftCylinder->getCylinderRadius(), rightCylinder->getCylinderRadius()) < 50.0)
    {
        return false;
    }
    */
 
    Eigen::Matrix4f leftPose = FramedPosePtr::dynamicCast(leftCylinder->getPose())->toEigen();
    Eigen::Vector3f leftExtent = Eigen::Vector3f(leftCylinder->getLength() / 2.0,
                                                 leftCylinder->getCylinderRadius(),
                                                 leftCylinder->getCylinderRadius());
 
 
    Eigen::Matrix4f rightPose = FramedPosePtr::dynamicCast(rightCylinder->getPose())->toEigen();
    Eigen::Vector3f rightExtent = Eigen::Vector3f(rightCylinder->getLength() / 2.0,
                                                  rightCylinder->getCylinderRadius(),
                                                  rightCylinder->getCylinderRadius());
 
 
    if (!testBoxIntersection(leftPose, rightPose, leftExtent, rightExtent))
    {
        return false;
    }
 
    return true;
}
 
bool
PrimitiveFusion::testPlane(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive,
                           memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive)
{
    memoryx::PlanePrimitivePtr leftPlane = memoryx::PlanePrimitivePtr::dynamicCast(leftPrimitive);
    memoryx::PlanePrimitivePtr rightPlane = memoryx::PlanePrimitivePtr::dynamicCast(rightPrimitive);
 
    if (!leftPlane || !rightPlane)
    {
        return false;
    }
 
    Eigen::Vector3f leftPlaneNormal =
        Vector3Ptr::dynamicCast(leftPlane->getPlaneNormal())->toEigen();
    Eigen::Vector3f rightPlaneNormal =
        Vector3Ptr::dynamicCast(rightPlane->getPlaneNormal())->toEigen();
 
    if ((leftPlaneNormal - rightPlaneNormal).norm() < 0.001)
    {
        return true;
    }
 
    float alpha = std::acos(leftPlaneNormal.dot(rightPlaneNormal));
 
    if (alpha > pcl::deg2rad(10.0))
    {
        return false;
    }
 
    return true;
}
 
bool
PrimitiveFusion::isSimilar(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive,
                           memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive)
{
    if (leftPrimitive->ice_staticId() != rightPrimitive->ice_staticId())
    {
        return false;
    }
 
    Eigen::Vector3f leftOBB = Vector3Ptr::dynamicCast(leftPrimitive->getOBBDimensions())->toEigen();
    Eigen::Vector3f rightOBB =
        Vector3Ptr::dynamicCast(rightPrimitive->getOBBDimensions())->toEigen();
 
    if ((leftOBB - rightOBB).norm() > 10.0)
    {
        return false;
    }
 
    Eigen::Vector3f leftPosition =
        Vector3Ptr::dynamicCast(leftPrimitive->getPose()->position)->toEigen();
    Eigen::Vector3f rightPosition =
        Vector3Ptr::dynamicCast(rightPrimitive->getPose()->position)->toEigen();
 
    if ((leftPosition - rightPosition).norm() > 5.0)
    {
        return false;
    }
 
    Eigen::Matrix3f leftRotation =
        QuaternionPtr::dynamicCast(leftPrimitive->getPose()->orientation)->toEigen();
    Eigen::Matrix3f rightRotation =
        QuaternionPtr::dynamicCast(rightPrimitive->getPose()->orientation)->toEigen();
 
    Eigen::Vector3f ea = (leftRotation.transpose() * rightRotation).eulerAngles(0, 1, 2);
 
 
    float delta = pcl::deg2rad(15.0);
    for (int i = 0; i < 3; i++)
    {
        ea[i] = std::fabs(ea[i]);
        ea[i] = std::fmod(ea[i], M_PI);
        if (ea[i] > delta && ea[i] < (M_PI - delta))
        {
            return false;
        }
    }
 
    return true;
}
 
void
PrimitiveFusion::findBoxPrimitives(
    memoryx::EnvironmentalPrimitiveBaseList& primitives,
    std::vector<memoryx::EntityBasePtr>& boxes,
    memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment)
{
    for (memoryx::EnvironmentalPrimitiveBasePtr& currentPrimitive : primitives)
    {
        memoryx::PlanePrimitivePtr plane =
            memoryx::PlanePrimitivePtr::dynamicCast(currentPrimitive);
        if (!plane)
        {
            continue;
        }
 
        Eigen::Vector3f planeNormal = Vector3Ptr::dynamicCast(plane->getPlaneNormal())->toEigen();
        float alpha = std::acos(planeNormal.dot(Eigen::Vector3f::UnitZ()));
        if (alpha < pcl::deg2rad(15.0))
        {
            std::vector<memoryx::PlanePrimitivePtr> boxSides;
            boxSides.push_back(plane);
 
            std::vector<memoryx::EnvironmentalPrimitiveBasePtr> intersectingPrimitives;
            getIntersectingPrimitives(primitives, currentPrimitive, intersectingPrimitives, 50.0);
 
            for (memoryx::EnvironmentalPrimitiveBasePtr& i : intersectingPrimitives)
            {
                memoryx::PlanePrimitivePtr sideCandidate =
                    memoryx::PlanePrimitivePtr::dynamicCast(i);
                if (!sideCandidate)
                {
                    continue;
                }
                else if (plane->getPose()->position->z < sideCandidate->getPose()->position->z)
                {
                    continue;
                }
 
                bool isPerpendicular = true;
                for (memoryx::PlanePrimitivePtr& side : boxSides)
                {
                    Eigen::Vector3f sideNormal =
                        Vector3Ptr::dynamicCast(sideCandidate->getPlaneNormal())->toEigen();
                    Eigen::Vector3f otherNormal =
                        Vector3Ptr::dynamicCast(side->getPlaneNormal())->toEigen();
 
                    float beta = std::acos(sideNormal.dot(otherNormal));
 
                    if (std::abs(beta) < 0.01)
                    {
                        ARMARX_LOG_S << "found a parallel side. doing nothing at the moment";
                        // todo check if this is a possible opposite side;
                        isPerpendicular = false;
                        break;
                    }
                    else if (std::fabs(beta - M_PI_2) > pcl::deg2rad(5.0))
                    {
                        isPerpendicular = false;
                        break;
                    }
                }
                if (isPerpendicular)
                {
                    boxSides.push_back(sideCandidate);
                }
            }
 
            if (boxSides.size() == 3)
            {
                memoryx::EntityRefList entityRefList;
                memoryx::PointList graspPoints;
                std::vector<Eigen::Vector3f> normals;
                std::vector<Eigen::Vector3f> positions;
                for (size_t i = 0; i < 3; i++)
                {
                    memoryx::PlanePrimitivePtr side = boxSides[i];
                    normals.push_back(Vector3Ptr::dynamicCast(side->getPlaneNormal())->toEigen());
                    positions.push_back(
                        Vector3Ptr::dynamicCast(side->getPose()->position)->toEigen());
 
                    memoryx::EntityRefBasePtr entityRef =
                        environmentalPrimitiveSegment->getEntityRefById(side->getId());
                    entityRefList.push_back(entityRef);
 
                    for (armarx::Vector3BasePtr& g : side->getGraspPoints())
                    {
                        graspPoints.push_back(g);
                    }
                }
 
 
                Eigen::Matrix3f rot = Eigen::Matrix3f::Zero();
                rot.col(0) = normals[0];
                rot.col(1) = normals[1];
                rot.col(2) = normals[0].cross(normals[1]);
 
                Eigen::Vector3f position = positions[0];
                position(2) = positions[1](2);
 
                FramedPosePtr pose = new FramedPose(rot, position, armarx::GlobalFrame, "");
 
                Eigen::Vector3f dimE;
                dimE(0) = normals[0].dot(positions[0] - position);
                dimE(1) = normals[1].dot(positions[1] - position);
                dimE(2) = normals[2].dot(positions[2] - position);
                dimE = 2.0 * dimE.cwiseAbs();
 
                memoryx::BoxPrimitivePtr boxPrimitive = new memoryx::BoxPrimitive();
                boxPrimitive->setBoxSides(entityRefList);
                boxPrimitive->setGraspPoints(graspPoints);
                boxPrimitive->setLabel(0);
                boxPrimitive->setOBBDimensions(new Vector3(dimE));
                boxPrimitive->setPose(pose);
 
                boxPrimitive->setProbability(1.0);
                // for (memoryx::PlanePrimitivePtr& side : boxSides)
                // {
                //boxPrimitive->setProbability(boxPrimitive->getProbability(), side->getProbability());
                // }
 
                boxPrimitive->setTime(boxSides[0]->getTime());
                boxPrimitive->setName("box");
 
                boxes.push_back(boxPrimitive);
 
                bool mirrorSides = true;
                if (mirrorSides)
                {
                    for (size_t i = 1; i < 3; i++)
                    {
                        memoryx::PlanePrimitivePtr side = boxSides[i];
                        Eigen::Matrix4f p = FramedPosePtr::dynamicCast(side->getPose())->toEigen();
                        Eigen::Vector3f m = p.block<3, 3>(0, 0).transpose() * normals[i];
 
                        Eigen::Vector3f mPosition = position - normals[i] * dimE(2);
                        FramedPosePtr mPose =
                            new FramedPose(p.block<3, 3>(0, 0), mPosition, armarx::GlobalFrame, "");
 
                        memoryx::PointList graspPoints;
                        for (armarx::Vector3BasePtr& g : side->getGraspPoints())
                        {
                            Eigen::Vector4f point = Eigen::Vector4f::Identity();
                            point.head<3>() = Vector3Ptr::dynamicCast(g)->toEigen();
 
                            point = p.inverse() * point;
                            point.head<3>() = point.head<3>() - m * dimE(i);
                            point = p * point;
 
                            Eigen::Vector3f temp = point.head<3>();
                            graspPoints.push_back(new Vector3(temp));
                        }
 
 
                        memoryx::PlanePrimitivePtr mSide = new memoryx::PlanePrimitive();
                        mSide->setOBBDimensions(side->getOBBDimensions());
                        mSide->setGraspPoints(graspPoints);
                        mSide->setPose(mPose);
                        // todo rotate normal
                        mSide->setPlaneNormal(side->getPlaneNormal());
                        mSide->setLabel(0);
                        mSide->setTime(side->getTime());
                        mSide->setProbability(0.0);
 
                        boxes.push_back(mSide);
                    }
                }
            }
        }
    }
}