PrimitiveFilter.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 "PrimitiveFilter.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
 
using namespace armarx;
 
PrimitiveFilter::PrimitiveFilter()
{
}
 
PrimitiveFilter::~PrimitiveFilter()
{
}
 
void
PrimitiveFilter::getPrimitivesInFieldOfView(
    std::vector<memoryx::EnvironmentalPrimitiveBasePtr> allPrimitives,
    std::vector<memoryx::EnvironmentalPrimitiveBasePtr>& visibilePrimitives,
    std::vector<memoryx::EnvironmentalPrimitiveBasePtr>& otherPrimitives)
{
    for (memoryx::EnvironmentalPrimitiveBasePtr& p : allPrimitives)
    {
        if (!p->getPose())
        {
            ARMARX_WARNING << "no pose for primitive " << p->getId();
            continue;
        }
 
        Eigen::Matrix4f pose = FramedPosePtr::dynamicCast(p->getPose())->toEigen();
 
        Eigen::Vector3f OBBDimensions = Vector3Ptr::dynamicCast(p->getOBBDimensions())->toEigen();
 
        if (intersectsFrustum(pose, OBBDimensions))
        {
            visibilePrimitives.push_back(p);
        }
        else
        {
            otherPrimitives.push_back(p);
        }
    }
}
 
void
PrimitiveFilter::visualizeFrustum(DebugDrawerInterfacePrx debugDrawerTopicPrx)
{
 
    Eigen::Vector3f nearTopRight = frustum.col(0);
    Eigen::Vector3f nearTopLeft = frustum.col(1);
    Eigen::Vector3f farTopLeft = frustum.col(2);
    Eigen::Vector3f nearBottomLeft = frustum.col(3);
    Eigen::Vector3f nearBottomRight = frustum.col(4);
    Eigen::Vector3f farBottomRight = frustum.col(5);
    Eigen::Vector3f farTopRight = frustum.col(15);
    Eigen::Vector3f farBottomLeft = frustum.col(17);
 
    Vector3BasePtr start = new Vector3(nearTopLeft);
    Vector3BasePtr stop = new Vector3(nearBottomLeft);
    DrawColor color;
    debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumNearLeft", start, stop, 1.0, color);
 
    start = new Vector3(farTopLeft);
    stop = new Vector3(farBottomLeft);
    debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumFarLeft", start, stop, 1.0, color);
 
    start = new Vector3(nearTopRight);
    stop = new Vector3(nearBottomRight);
    debugDrawerTopicPrx->setLineVisu(
        "primitiveMapper", "frustumNearRight", start, stop, 1.0, color);
 
    start = new Vector3(farTopRight);
    stop = new Vector3(farBottomRight);
    debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumFarRight", start, stop, 1.0, color);
 
    start = new Vector3(nearBottomLeft);
    stop = new Vector3(nearBottomRight);
    debugDrawerTopicPrx->setLineVisu(
        "primitiveMapper", "frustumNearBottom", start, stop, 1.0, color);
 
    start = new Vector3(farBottomLeft);
    stop = new Vector3(farBottomRight);
    debugDrawerTopicPrx->setLineVisu(
        "primitiveMapper", "frustumFarBottom", start, stop, 1.0, color);
 
    start = new Vector3(nearBottomRight);
    stop = new Vector3(farBottomRight);
    debugDrawerTopicPrx->setLineVisu(
        "primitiveMapper", "frustumRightBottom", start, stop, 1.0, color);
 
    start = new Vector3(nearBottomLeft);
    stop = new Vector3(farBottomLeft);
    debugDrawerTopicPrx->setLineVisu(
        "primitiveMapper", "frustumLeftBottom", start, stop, 1.0, color);
 
    start = new Vector3(nearTopRight);
    stop = new Vector3(farTopRight);
    debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumRightTop", start, stop, 1.0, color);
 
    start = new Vector3(nearTopLeft);
    stop = new Vector3(farTopLeft);
    debugDrawerTopicPrx->setLineVisu("primitiveMapper", "frustumLeftTop", start, stop, 1.0, color);
}
 
/**
 * adapted from
 * https://github.com/vanderlin/FrustumCulling/
 */
Eigen::Matrix3Xf
PrimitiveFilter::updateCameraPosition(Eigen::Matrix4f cameraToWorld)
{
    float imageSizeRatio = 640.0 / 480.0;
 
    float farClipDistance = 3000.0;
    float nearClipDistance = 400.0;
 
    Eigen::Vector3f farCenter(0, 0, farClipDistance);
    Eigen::Vector3f nearCenter(0, 0, nearClipDistance);
 
    float a = -1.0 * std::tan(0.3926990925);
 
    Eigen::Vector3f farVertical(0, a * farClipDistance, 0);
    Eigen::Vector3f farHorizontal(a * farClipDistance * imageSizeRatio, 0, 0);
 
    Eigen::Vector3f farTopLeft = farCenter - farVertical - farHorizontal;
    Eigen::Vector3f farTopRight = farCenter - farVertical + farHorizontal;
 
    Eigen::Vector3f farBottomLeft = farCenter + farVertical - farHorizontal;
    Eigen::Vector3f farBottomRight = farCenter + farVertical + farHorizontal;
 
    Eigen::Vector3f nearVertical(0, a * nearClipDistance, 0);
    Eigen::Vector3f nearHorizontal(a * nearClipDistance * imageSizeRatio, 0, 0);
 
    Eigen::Vector3f nearTopLeft = nearCenter - nearVertical - nearHorizontal;
    Eigen::Vector3f nearTopRight = nearCenter - nearVertical + nearHorizontal;
 
    Eigen::Vector3f nearBottomLeft = nearCenter + nearVertical - nearHorizontal;
    Eigen::Vector3f nearBottomRight = nearCenter + nearVertical + nearHorizontal;
 
 
    frustum.col(0) = nearTopRight;
    frustum.col(1) = nearTopLeft;
    frustum.col(2) = farTopLeft;
 
    frustum.col(3) = nearBottomLeft;
    frustum.col(4) = nearBottomRight;
    frustum.col(5) = farBottomRight;
 
    frustum.col(6) = nearTopLeft;
    frustum.col(7) = nearBottomLeft;
    frustum.col(8) = farBottomLeft;
 
    frustum.col(9) = nearBottomRight;
    frustum.col(10) = nearTopRight;
    frustum.col(11) = farBottomRight;
 
    frustum.col(12) = nearTopLeft;
    frustum.col(13) = nearTopRight;
    frustum.col(14) = nearBottomRight;
 
    frustum.col(15) = farTopRight;
    frustum.col(16) = farTopLeft;
    frustum.col(17) = farBottomLeft;
 
 
    Eigen::Affine3f transform = Eigen::Affine3f::Identity();
    transform *= Eigen::AngleAxisf(1.0 * M_PI, Eigen::Vector3f::UnitZ());
 
 
    for (int i = 0; i < 18; i++)
    {
        Eigen::Vector4f temp = Eigen::Vector4f::Zero();
 
        temp.head<3>() = frustum.col(i);
 
        frustum.col(i) =
            cameraToWorld.block<3, 3>(0, 0) * transform.matrix().block<3, 3>(0, 0) * frustum.col(i);
        frustum.col(i) += cameraToWorld.col(3).head<3>();
    }
 
    return frustum;
}
 
bool
PrimitiveFilter::intersectsFrustum(Eigen::Matrix4f pose, Eigen::Vector3f extent)
{
    Eigen::Vector3f a = pose.block<3, 3>(0, 0) * extent;
 
    Eigen::Matrix3Xf points(3, 8);
    points.col(0) = Eigen::Vector3f(a(0), a(1), a(2));
    points.col(1) = Eigen::Vector3f(a(0), a(1), -a(2));
    points.col(2) = Eigen::Vector3f(a(0), -a(1), a(2));
    points.col(3) = Eigen::Vector3f(a(0), -a(1), -a(2));
    points.col(4) = Eigen::Vector3f(-a(0), a(1), a(2));
    points.col(5) = Eigen::Vector3f(-a(0), a(1), -a(2));
    points.col(6) = Eigen::Vector3f(-a(0), -a(1), a(2));
    points.col(7) = Eigen::Vector3f(-a(0), -a(1), -a(2));
 
    for (int i = 0; i < 8; i++)
    {
        Eigen::Vector3f point = pose.col(3).head<3>() + points.col(i);
        if (isPointInFrustum(point))
        {
            return true;
        }
    }
 
    return false;
}
 
bool
PrimitiveFilter::isPointInFrustum(Eigen::Vector3f point)
{
    for (int i = 0; i < 18; i += 3)
    {
        Eigen::Vector3f u = frustum.col(i) - frustum.col(i + 1);
        Eigen::Vector3f v = frustum.col(i) - frustum.col(i + 2);
        Eigen::Vector3f normal = u.cross(v);
        float dist = -1 * frustum.col(i).dot(normal);
        if (normal.dot(point) + dist < 0)
        {
            return false;
        }
    }
    return true;
}