BimanualAffordanceArmarX.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 "BimanualAffordanceArmarX.h"
 
#include <AffordanceKit/ColorMap.h>
#include <Inventor/nodes/SoDrawStyle.h>
 
namespace AffordanceKitArmarX
{
    BimanualAffordanceArmarX::BimanualAffordanceArmarX(
        const AffordanceKit::BimanualAffordancePtr& bimanualAffordance) :
        visualizationNode(nullptr)
    {
        affordance = bimanualAffordance;
    }
 
    BimanualAffordanceArmarX::~BimanualAffordanceArmarX()
    {
        if (visualizationNode)
        {
            visualizationNode->unref();
        }
    }
 
    void
    BimanualAffordanceArmarX::reset()
    {
        if (visualizationNode)
        {
            visualizationNode->removeAllChildren();
        }
    }
 
    void
    BimanualAffordanceArmarX::visualize(const armarx::DebugDrawerInterfacePrx& debugDrawer,
                                        const std::string& layerName,
                                        const std::string& id,
                                        float minExpectedProbability,
                                        const AffordanceKit::PrimitivePtr& primitive)
    {
        if (!primitive)
        {
            return;
        }
 
        for (auto& entry : *affordance->getTheta())
        {
            if (primitive->getId() != entry.first.first->getId())
            {
                continue;
            }
 
            AffordanceKit::PrimitivePair pp = entry.first;
 
            unsigned int size = primitive->getSamplingSize();
            if (size == 0 || affordance->getThetaSize(pp) != size * size)
            {
                continue;
            }
 
            armarx::DebugDrawer24BitColoredPointCloud pointCloud;
            pointCloud.pointSize = 7;
            pointCloud.transparency = 0;
 
            // Attention: This implicitly assumes four orientational samplings
            pointCloud.points.reserve((size / 4) * (size / 4));
 
            std::map<std::pair<unsigned int, unsigned int>, AffordanceKit::Belief> maxBeliefMap;
 
            const Eigen::MatrixXf& T = affordance->getTheta(pp);
            for (unsigned int i = 0; i < T.cols(); i++)
            {
                unsigned int index1 = i / size;
                unsigned int index2 = i % size;
 
                // Attention: This implicitly assumes four orientational samplings
                index1 = index1 - (index1 % 4);
                index2 = index2 - (index2 % 4);
 
                // For visualization the index order does not matter
                std::pair<unsigned int, unsigned int> indices =
                    (index1 <= index2) ? std::pair<unsigned int, unsigned int>(index1, index2)
                                       : std::pair<unsigned int, unsigned int>(index2, index1);
 
                AffordanceKit::Belief belief(T.col(i));
                if ((maxBeliefMap.find(indices) == maxBeliefMap.end()) ||
                    (belief.expectedProbability() > maxBeliefMap.at(indices).expectedProbability()))
                {
                    maxBeliefMap[indices] = belief;
                }
            }
 
            std::cout << "maxBeliefMap size: " << maxBeliefMap.size() << std::endl;
 
            for (auto& entry : maxBeliefMap)
            {
                if (entry.second.expectedProbability() < minExpectedProbability)
                {
                    continue;
                }
 
                const Eigen::Matrix4f& x1 = primitive->getSampling(entry.first.first);
                const Eigen::Matrix4f& x2 = primitive->getSampling(entry.first.second);
                Eigen::Vector3i c = AffordanceKit::ColorMap::GetVisualizationColor(entry.second);
 
                armarx::DebugDrawer24BitColoredPointCloudElement e1;
                e1.x = x1(0, 3);
                e1.y = x1(1, 3);
                e1.z = x1(2, 3);
                e1.color = armarx::DrawColor24Bit{
                    (unsigned char)c(0), (unsigned char)c(1), (unsigned char)c(2)};
                pointCloud.points.push_back(e1);
 
                armarx::DebugDrawer24BitColoredPointCloudElement e2;
                e2.x = x2(0, 3);
                e2.y = x2(1, 3);
                e2.z = x2(2, 3);
                e2.color = armarx::DrawColor24Bit{
                    (unsigned char)c(0), (unsigned char)c(1), (unsigned char)c(2)};
                pointCloud.points.push_back(e2);
            }
 
            std::cout << "pointCloud size: " << pointCloud.points.size() << std::endl;
 
            debugDrawer->set24BitColoredPointCloudVisu(layerName, id + "_sampling", pointCloud);
        }
    }
} // namespace AffordanceKitArmarX