ValveAttention.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 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::ArmarXObjects::ValveAttention
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2016
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ValveAttention.h"
 
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
 
#include <pcl/common/point_tests.h>
 
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <Image/ImageProcessor.h>
 
namespace armarx
{
 
 
    armarx::PropertyDefinitionsPtr
    ValveAttention::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(
            new ValveAttentionPropertyDefinitions(getConfigIdentifier()));
    }
 
    void
    armarx::ValveAttention::onInitPointCloudAndImageProcessor()
    {
        usingProxy(getProperty<std::string>("ViewSelectionName").getValue());
        usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
 
        providerName = getProperty<std::string>("providerName").getValue();
        usingImageProvider(providerName);
        usingPointCloudProvider(providerName);
 
        std::string graphFileName = "RobotAPI/ViewSelection/graph40k.gra";
 
        armarx::CMakePackageFinder finder("RobotAPI");
        ArmarXDataPath::addDataPaths(finder.getDataDir());
 
        if (ArmarXDataPath::getAbsolutePath(graphFileName, graphFileName))
        {
            saliencyEgosphereGraph = new CIntensityGraph(graphFileName);
            ARMARX_VERBOSE << "Created egosphere graph with "
                           << saliencyEgosphereGraph->getNodes()->size() << "nodes";
            graphLookupTable = new CGraphPyramidLookupTable(9, 18);
            graphLookupTable->buildLookupTable(saliencyEgosphereGraph);
        }
        else
        {
            ARMARX_ERROR << "Could not find required graph file";
            handleExceptions();
        }
 
        headFrameName = getProperty<std::string>("HeadFrameName").getValue();
        hog = new HoG();
        hog->loadTrainingData(getProperty<std::string>("TrainingData").getValue());
        hog->setParameters(true, true);
    }
 
    void
    armarx::ValveAttention::onConnectPointCloudAndImageProcessor()
    {
 
        visionx::ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
 
        visionx::ImageProviderInfo imageProviderInfo =
            getImageProvider(providerName, imageDisplayType);
 
        numImages = imageProviderInfo.numberImages;
 
        images = new CByteImage*[numImages];
        for (int i = 0; i < numImages; i++)
        {
            images[i] = visionx::tools::createByteImage(imageProviderInfo);
        }
 
        numResultImages = 3;
        result = new CByteImage*[numResultImages];
        result[0] = images[0];
        result[1] = visionx::tools::createByteImage(imageProviderInfo);
        result[2] = visionx::tools::createByteImage(imageProviderInfo);
 
 
        visionx::ImageDimension dimension(images[0]->width, images[0]->height);
        enableResultImages(numResultImages, dimension, visionx::tools::typeNameToImageType("rgb"));
 
        robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
            getProperty<std::string>("RobotStateComponentName").getValue());
 
        robot = RemoteRobot::createLocalCloneFromFile(
            robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eFull);
 
        viewSelection = getProxy<ViewSelectionInterfacePrx>(
            getProperty<std::string>("ViewSelectionName").getValue());
    }
 
    void
    armarx::ValveAttention::onDisconnectPointCloudAndImageProcessor()
    {
    }
 
    void
    armarx::ValveAttention::onExitPointCloudAndImageProcessor()
    {
        for (int i = 0; i < numImages; i++)
        {
            delete images[i];
        }
        delete[] images;
 
        for (int i = 0; i < numResultImages; i++)
        {
            delete result[i];
        }
        delete[] result;
 
        delete graphLookupTable;
        delete saliencyEgosphereGraph;
        delete hog;
    }
 
    void
    armarx::ValveAttention::process()
    {
        ARMARX_IMPORTANT << "process";
 
        std::lock_guard<std::mutex> lock(mutex);
 
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr currentPointCloud(
            new pcl::PointCloud<pcl::PointXYZRGBA>());
 
 
        ARMARX_VERBOSE << "waiting for images";
        if (!waitForImages(providerName))
        {
            ARMARX_WARNING << "Timeout while waiting for images";
            return;
        }
 
        ARMARX_VERBOSE << "waiting for pointcloud";
        if (!waitForPointClouds(providerName))
        {
            ARMARX_WARNING << "Timeout while waiting for pointcloud";
            return;
        }
 
        ARMARX_VERBOSE << "getting images";
        if (getImages(images) != numImages)
        {
            ARMARX_WARNING << "Unable to transfer or read images";
            return;
        }
 
        ARMARX_VERBOSE << "getting pointcloud";
        if (!getPointClouds(currentPointCloud))
        {
            ARMARX_WARNING << "Unable to transfer or read point cloud";
            return;
        }
 
 
        RemoteRobot::synchronizeLocalClone(robot, robotStateComponent);
 
        saliencyEgosphereGraph->set(0.0);
 
        const int scaleFactor = 4; //images[0]->width / 255;
 
        int width = images[0]->width / scaleFactor;
        int height = images[0]->height / scaleFactor;
        CByteImage* image = new CByteImage(width, height, CByteImage::eRGB24);
        ::ImageProcessor::Resize(images[0], image);
 
        ARMARX_VERBOSE << "downscaling image to: " << width << "x" << height;
 
        //CByteImage* saliencyImage = result[0]; //new CByteImage(width, height, CByteImage::eRGB24);
        CFloatImage* saliencyImage = new CFloatImage(width, height, 1);
 
 
        ARMARX_VERBOSE << "computing saliency map";
        IceUtil::Time startTime = TimeUtil::GetTime();
 
        hog->findSalientRegions(image, saliencyImage);
        //*************************test for Cluster**************************/
        std::vector<cv::Point2f> points;
        cv::Mat outputLable;
        cv::Mat centers;
        std::map<int, int> saliencyHist;
        float priorityProcent = 0.9;
        int clusterNum = 3;
        int intensityThreshold = 255;
        int saliencySum = 0;
        int saliencyImageWidth = saliencyImage->width;
 
        for (int i = 0; i < saliencyImage->height; i++)
        {
            for (int j = 0; j < saliencyImageWidth; j++)
            {
                saliencyHist[(int)(saliencyImage->pixels[i * saliencyImageWidth + j] * 255.0)]++;
            }
        }
 
        // determine  90% number of non-zero salient values
        int usefulSampleAmount =
            priorityProcent * (saliencyImage->width * saliencyImage->height - saliencyHist[0]);
        for (int i = 1; i <= 255; i++)
        {
            if (!saliencyHist.count(i))
            {
                continue;
            }
            saliencySum += saliencyHist[i];
            if (saliencySum > usefulSampleAmount)
            {
                intensityThreshold = i;
                break;
            }
        }
 
        for (int i = 0; i < saliencyImage->height; i++)
        {
            for (int j = 0; j < saliencyImageWidth; j++)
            {
                if (saliencyImage->pixels[i * saliencyImageWidth + j] * 255.0 > intensityThreshold)
                {
                    points.push_back(cv::Point2f(j, i));
                }
            }
        }
 
        cv::Mat samples(points.size(), 2, CV_32F);
        for (size_t i = 0; i < points.size(); i++)
        {
            samples.at<float>(i, 0) = points[i].x;
            samples.at<float>(i, 1) = points[i].y;
        }
 
        if (points.size())
        {
            cv::kmeans(samples,
                       clusterNum,
                       outputLable,
                       cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 20, 0.1),
                       10,
                       cv::KMEANS_PP_CENTERS,
                       centers);
        }
 
        // calculate the number of elements in each cluster and get first two biggest cluster
        std::map<int, int> numberOfEachCluster;
        for (int i = 0; i < samples.rows; i++)
        {
            numberOfEachCluster[outputLable.at<int>(i, 0)]++;
        }
        size_t maxCluster = 0;
        int secondCluster;
        for (size_t i = 0; i < numberOfEachCluster.size(); i++)
        {
            if (numberOfEachCluster[i] > numberOfEachCluster[maxCluster])
            {
                maxCluster = i;
            }
        }
        if (maxCluster != 0)
        {
            secondCluster = 0;
        }
        else
        {
            secondCluster = 1;
        }
        for (size_t i = 0; i < numberOfEachCluster.size(); i++)
        {
            if ((i != maxCluster) && numberOfEachCluster[i] > numberOfEachCluster[secondCluster])
            {
                secondCluster = i;
            }
        }
 
        //*************************test for Cluster END**************************/
 
        ARMARX_VERBOSE << "done computing saliency map. took "
                       << (TimeUtil::GetTime() - startTime).toMilliSeconds() << "ms";
 
        delete image;
 
#pragma omp parallel for
        for (int i = 0; i < images[0]->height; i++)
        {
            for (int j = 0; j < images[0]->width; j++)
            {
                int idx = i * images[0]->width + j;
 
                float saliency =
                    saliencyImage->pixels[(i / scaleFactor * width) + (j / scaleFactor)];
 
                result[1]->pixels[3 * idx + 0] = 255 * saliency;
                result[1]->pixels[3 * idx + 1] = 255 * saliency;
                result[1]->pixels[3 * idx + 2] = 255 * saliency;
 
                result[2]->pixels[3 * idx + 0] =
                    (0.2 + 0.8 * saliency) * result[0]->pixels[3 * idx + 0];
                result[2]->pixels[3 * idx + 1] =
                    (0.2 + 0.8 * saliency) * result[0]->pixels[3 * idx + 1];
                result[2]->pixels[3 * idx + 2] =
                    (0.2 + 0.8 * saliency) * result[0]->pixels[3 * idx + 2];
            }
        }
 
 
        /*********transform cluster daten (the points near cluster centers) from pointcloud to sphere **********/
        /*
            for (int i = 0; i < samples.rows; i++)
            {
                int downScaleIdx = ((int)samples.at<float>(i, 1) * width) + ((int)samples.at<float>(i, 0));
                int idx = (int)samples.at<float>(i, 1) * scaleFactor * images[0]->width + (int)samples.at<float>(i, 0) * scaleFactor;
                float saliency = saliencyImage->pixels[downScaleIdx];
                int clusterLabel = outputLable.at<int>(i, 0);
                float distanceToClusterCenter = sqrt(pow((centers.at<float>(clusterLabel, 0) - samples.at<float>(i, 0)), 2) + pow((centers.at<float>(clusterLabel, 1) - samples.at<float>(i, 1)), 2));
 
                if (saliency > 0 && (distanceToClusterCenter < (width / 5)))
                {
                    if (idx >= (int)currentPointCloud->points.size())
                    {
                        continue;
                    }
 
                    Eigen::Vector3f vec = currentPointCloud->points[idx].getVector3fMap();
 
                    if (!pcl::isFinite(currentPointCloud->points[idx]))
                    {
                        continue;
                    }
                    TSphereCoord positionInSphereCoordinates;
                    //armarx::GlobalFrame
                    FramedPositionPtr currentViewTarget = new FramedPosition(vec, "DepthCamera", robotStateComponent->getSynchronizedRobot()->getName());
                    currentViewTarget->changeFrame(robot, headFrameName);
 
                    MathTools::convert(currentViewTarget->toEigen(), positionInSphereCoordinates);
                    int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
                    CIntensityNode* currentNode = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(closestNodeIndex));
 
                    float currentSaliency = currentNode->getIntensity();
 
                    // give priority to the cluster which has the most elements, because this cluster always near valve
                    if (clusterLabel == maxCluster)
                    {
                        //consider about history? & predict  --> consider about observation & predict
                        currentSaliency = std::min(1.0f, currentSaliency + saliency * (float)1.3);
 
                        //consider only about predict
                        //currentSaliency = std::min(1.0f, saliency * (float)1.3);
                        //std::cout << "currentSaliency!! " << currentSaliency << std::endl;
                    }
                    else
                    {
                        if (outputLable.at<int>(i, 0) == secondCluster)
                        {
                            currentSaliency = std::min(1.0f, currentSaliency + saliency * (float)1.02);
                            //currentSaliency = std::min(1.0f, saliency * (float)1.02);
                        }
                    }
                    currentNode->setIntensity(currentSaliency);
                }
            }
 
        */
        /*********transform cluster daten from pointcloud to sphere ***** END*****/
 
        /*********transform cluster centers from pointcloud to sphere ***** *****/
        for (int i = 0; i < centers.rows; i++)
        {
            int downScaleIdx =
                ((int)centers.at<float>(i, 1) * width) + ((int)centers.at<float>(i, 0));
            int idx = (int)centers.at<float>(i, 1) * scaleFactor * images[0]->width +
                      (int)centers.at<float>(i, 0) * scaleFactor;
            //attention: center's saliency may shouldn't directly get from saliency map, because it's saliency can be low
            float saliency = saliencyImage->pixels[downScaleIdx];
 
            if (idx >= (int)currentPointCloud->points.size())
            {
                continue;
            }
 
            Eigen::Vector3f vec = currentPointCloud->points[idx].getVector3fMap();
 
            if (!pcl::isFinite(currentPointCloud->points[idx]))
            {
                continue;
            }
            TSphereCoord positionInSphereCoordinates;
            //armarx::GlobalFrame
            FramedPositionPtr currentViewTarget = new FramedPosition(
                vec, "DepthCamera", robotStateComponent->getSynchronizedRobot()->getName());
            currentViewTarget->changeFrame(robot, headFrameName);
 
            MathTools::convert(currentViewTarget->toEigen(), positionInSphereCoordinates);
            int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
 
            float halfCameraOpeningAngle = 12.0 * M_PI / 180.0;
            float modifiedHalfCameraOpeningAngle = halfCameraOpeningAngle;
            float distance = currentViewTarget->toEigen().norm();
 
 
            if (distance > 6000)
            {
                ARMARX_WARNING << "position is too far away." << distance;
                return;
            }
 
            const float distanceThreshold = 1500;
            if (distance < distanceThreshold)
            {
                modifiedHalfCameraOpeningAngle = (distance - 0.1f * distanceThreshold) /
                                                 (0.9f * distanceThreshold) *
                                                 halfCameraOpeningAngle;
                modifiedHalfCameraOpeningAngle = std::max(0.0f, modifiedHalfCameraOpeningAngle);
            }
 
 
            std::vector<bool> visitedNodes(saliencyEgosphereGraph->getNodes()->size(), false);
            addSaliencyRecursive(closestNodeIndex,
                                 visitedNodes,
                                 saliency,
                                 positionInSphereCoordinates,
                                 modifiedHalfCameraOpeningAngle);
        }
        /*********transform cluster centers from pointcloud to sphere ***** END*****/
 
        //*************************test for Cluster**************************/
        int index;
        for (std::size_t row = 0; centers.rows > 0 && row < static_cast<std::size_t>(centers.rows);
             row++)
        {
            for (std::size_t i = 0; i < 8; i++)
            {
                for (std::size_t j = 0; j < 8; j++)
                {
                    index =
                        3 * (((int)centers.at<float>(row, 1) * scaleFactor + i) * images[0]->width +
                             ((int)centers.at<float>(row, 0) * scaleFactor + j));
                    result[2]->pixels[index] = 255;
                    result[2]->pixels[index + 1] = 255 * (row == maxCluster);
                    result[2]->pixels[index + 2] =
                        255 * (row == static_cast<std::size_t>(secondCluster));
                }
            }
        }
 
        //*************************test for Cluster END**************************/
 
        ARMARX_VERBOSE << "mapping took !!!!! "
                       << (TimeUtil::GetTime() - startTime).toMilliSeconds() << "ms";
 
        provideResultImages(result);
 
        SaliencyMapBasePtr primitiveSaliency = new SaliencyMapBase();
        primitiveSaliency->name = "ValveAttention";
        saliencyEgosphereGraph->graphToVec(primitiveSaliency->map);
        viewSelection->updateSaliencyMap(primitiveSaliency);
 
        ARMARX_IMPORTANT << "process finished";
    }
 
    void
    ValveAttention::addSaliencyRecursive(const int currentNodeIndex,
                                         std::vector<bool>& visitedNodes,
                                         const float saliency,
                                         const TSphereCoord objectSphereCoord,
                                         const float maxDistanceOnArc)
    {
 
        // distance on arc between object projection center and node,
        // normalized by the maximal viewing angle of the camera (=> in [0,1])
        float normalizedDistance =
            MathTools::getDistanceOnArc(
                objectSphereCoord,
                saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) /
            maxDistanceOnArc;
 
        // increase value of node
        float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))
                             ->getIntensity() +
                         (1.0f - 0.5f * normalizedDistance * normalizedDistance) * saliency;
 
        if (newValue > 1.0)
        {
            ARMARX_INFO << "saliency is greater than 1.0: " << newValue;
            newValue = std::min(1.0f, newValue);
        }
 
        ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))
            ->setIntensity(newValue);
 
        // mark node as visited for this object
        visitedNodes.at(currentNodeIndex) = true;
 
        // recurse on neighbours if they were not yet visited and close enough to the object projection center
        int neighbourIndex;
 
        for (size_t i = 0; i < saliencyEgosphereGraph->getNodeAdjacency(currentNodeIndex)->size();
             i++)
        {
            neighbourIndex = saliencyEgosphereGraph->getNodeAdjacency(currentNodeIndex)->at(i);
 
            if (!visitedNodes.at(neighbourIndex))
            {
                if (MathTools::getDistanceOnArc(
                        objectSphereCoord,
                        saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <=
                    maxDistanceOnArc)
                {
                    addSaliencyRecursive(neighbourIndex,
                                         visitedNodes,
                                         saliency,
                                         objectSphereCoord,
                                         maxDistanceOnArc);
                }
                else
                {
                    visitedNodes.at(neighbourIndex) = true;
                }
            }
        }
    }
    ARMARX_REGISTER_COMPONENT_EXECUTABLE(::armarx::ValveAttention, ::armarx::ValveAttention::GetDefaultName());
 
} // namespace armarx