UnimanualAffordanceArmarX.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 "UnimanualAffordanceArmarX.h"
#include <AffordanceKit/ColorMap.h>
#include <AffordanceKit/primitives/Plane.h>
 
#include <Inventor/nodes/SoDrawStyle.h>
 
namespace AffordanceKitArmarX
{
    UnimanualAffordanceArmarX::UnimanualAffordanceArmarX(const AffordanceKit::UnimanualAffordancePtr& unimanualAffordance) :
        visualizationNode(nullptr)
    {
        affordance = unimanualAffordance;
    }
 
    UnimanualAffordanceArmarX::~UnimanualAffordanceArmarX()
    {
        if (visualizationNode)
        {
            visualizationNode->unref();
        }
    }
 
    void UnimanualAffordanceArmarX::reset()
    {
        if (visualizationNode)
        {
            visualizationNode->removeAllChildren();
        }
    }
 
    Eigen::Vector3f UnimanualAffordanceArmarX::computeSamplingPosition(const Eigen::Matrix4f& pose, float offset)
    {
        // Add a slight offset in negative z direction for visualization purposes
        return pose.block<3, 1>(0, 3) - offset * pose.block<3, 1>(0, 2);
    }
 
    float UnimanualAffordanceArmarX::computeSamplingDistance(const Eigen::Matrix4f& sampling1, const Eigen::Matrix4f& sampling2)
    {
        float pos = (sampling1.block<3, 1>(0, 3) - sampling2.block<3, 1>(0, 3)).norm();
 
        float rot = sampling1.block<3, 1>(0, 2).normalized().dot(sampling2.block<3, 1>(0, 2).normalized());
        CLAMP_TO_ACOS_DOMAIN(rot);
 
        return pos + 1000 * acos(rot);
    }
 
    void UnimanualAffordanceArmarX::visualize(const armarx::DebugDrawerInterfacePrx& debugDrawer, const std::string& layerName, const std::string& id, float minExpectedProbability, const AffordanceKit::PrimitivePtr& primitive)
    {
        const float pointCloudVisualizationOffset = 2;
 
        if (!primitive)
        {
            return;
        }
 
        for (auto& entry : *affordance->getTheta())
        {
            if (primitive->getId() != entry.first.first->getId())
            {
                continue;
            }
 
            unsigned int size = primitive->getSamplingSize();
            if (size == 0 || affordance->getThetaSize(entry.first) != size)
            {
                continue;
            }
 
#if 0
            // DEBUG: Visualize plane bounding boxes
            if (boost::dynamic_pointer_cast<AffordanceKit::Plane>(primitive))
            {
                AffordanceKit::PlanePtr p = boost::dynamic_pointer_cast<AffordanceKit::Plane>(primitive);
 
                Eigen::Vector3f min = p->getBoundingBoxMin();
                Eigen::Vector3f max = p->getBoundingBoxMax();
                Eigen::Matrix4f pose = p->getPose();
 
                armarx::PolygonPointList ppl;
                ppl.push_back(new armarx::Vector3(Eigen::Vector3f((pose * Eigen::Vector4f(min(0), min(1), 0, 1)).head(3))));
                ppl.push_back(new armarx::Vector3(Eigen::Vector3f((pose * Eigen::Vector4f(max(0), min(1), 0, 1)).head(3))));
                ppl.push_back(new armarx::Vector3(Eigen::Vector3f((pose * Eigen::Vector4f(max(0), max(1), 0, 1)).head(3))));
                ppl.push_back(new armarx::Vector3(Eigen::Vector3f((pose * Eigen::Vector4f(min(0), max(1), 0, 1)).head(3))));
 
                debugDrawer->setPolygonVisu(layerName, id + "_boundingBox", ppl, armarx::DrawColor {0, 0, 0, 0}, armarx::DrawColor {0, 0, 0, 1}, 3);
                debugDrawer->setPoseVisu(layerName, id + "_pose", new armarx::Pose(p->getPose()));
            }
#endif
 
            armarx::DebugDrawer24BitColoredPointCloud originalSamplingPointCloud;
            originalSamplingPointCloud.pointSize = 7;
            originalSamplingPointCloud.transparency = 0;
            originalSamplingPointCloud.points.reserve(size);
 
            Eigen::Matrix4f maxP = Eigen::Matrix4f::Identity();
            AffordanceKit::Belief maxBelief;
            float maxExpectedProbability = -1;
            int counter = 0;
 
            for (unsigned int i = 0; i < size; i++)
            {
                bool addCurrentPoint = false;
                Eigen::Matrix4f P;
                if (i < size - 1)
                {
                    P = primitive->getSampling(i);
                    addCurrentPoint = (i > 0 && computeSamplingDistance(maxP, P) > 1e-3);
                }
                if (i == size - 1)
                {
                    addCurrentPoint = true;
                }
 
                if (addCurrentPoint)
                {
                    if (maxExpectedProbability >= minExpectedProbability)
                    {
                        Eigen::Vector3f pos = computeSamplingPosition(maxP, pointCloudVisualizationOffset);
 
                        armarx::DebugDrawer24BitColoredPointCloudElement e;
                        e.x = pos.x();
                        e.y = pos.y();
                        e.z = pos.z();
 
                        Eigen::Vector3i c = AffordanceKit::ColorMap::GetVisualizationColor(maxBelief);
                        e.color = armarx::DrawColor24Bit {(unsigned char)c(0), (unsigned char)c(1), (unsigned char)c(2)};
 
                        originalSamplingPointCloud.points.push_back(e);
                    }
 
                    maxExpectedProbability = -1;
                    counter = 0;
                }
 
                AffordanceKit::Belief b = affordance->getTheta(AffordanceKit::PrimitivePair(primitive), i);
 
                counter++;
                float v = b.expectedProbability();
                if (maxExpectedProbability < v)
                {
                    maxP = P;
                    maxBelief = b;
                    maxExpectedProbability = v;
                }
            }
 
            debugDrawer->set24BitColoredPointCloudVisu(layerName, id + "_sampling", originalSamplingPointCloud);
        }
    }
}