RGBDHandLocalizer.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::ArmarXObjects::RGBDHandLocalizer
 * @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 "RGBDHandLocalizer.h"
 
 
 
namespace visionx
{
 
    armarx::PropertyDefinitionsPtr RGBDHandLocalizer::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new RGBDHandLocalizerPropertyDefinitions(
                getConfigIdentifier()));
    }
 
    void RGBDHandLocalizer::onInitPointCloudAndImageProcessor()
    {
        usingProxy(getProperty<std::string>("RobotStateComponentProxyName").getValue());
 
        providerName = getProperty<std::string>("providerName").getValue();
 
 
 
        sensorFrameName = getProperty<std::string>("SensorFrameName").getValue();
        markerFrameName = getProperty<std::string>("MarkerName").getValue();
        handFrameName = getProperty<std::string>("HandNameInRobotModel").getValue();
 
 
        usingImageProvider(providerName);
        usingPointCloudProvider(providerName);
        offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
    }
 
    void RGBDHandLocalizer::onConnectPointCloudAndImageProcessor()
    {
        ImageType imageDisplayType = visionx::tools::typeNameToImageType("rgb");
 
        ImageProviderInfo imageProviderInfo = getImageProvider(providerName, imageDisplayType);
 
        this->numImages = imageProviderInfo.numberImages;
 
        if (numImages < 1 || numImages > 2)
        {
            ARMARX_FATAL << "invalid number of images. aborting";
            return;
        }
 
        images = new CByteImage*[numImages];
 
        for (int i = 0 ; i < numImages ; i++)
        {
            images[i] = visionx::tools::createByteImage(imageProviderInfo);
        }
 
        result = new CByteImage*[numImages];
 
        this->robotStatePrx = getProxy<armarx::RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentProxyName").getValue());
 
        ImageDimension dimension(images[0]->width, images[0]->height);
        enableResultImages(numImages, dimension, visionx::tools::typeNameToImageType("rgb"));
        enableResultPointClouds<pcl::PointXYZRGBA>("MarkerCloud");
        enableResultPointClouds<pcl::PointXYZRGBA>("Filtered_Cloud");
        this->debugDrawerTopic = getTopic<armarx::DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
    }
 
    void RGBDHandLocalizer::onDisconnectPointCloudAndImageProcessor()
    {
        this->debugDrawerTopic->clearLayer("RGBDHandLocalizer");
    }
 
    void RGBDHandLocalizer::onExitPointCloudAndImageProcessor()
    {
        for (int i = 0; i < numImages; i++)
        {
            delete images[i];
        }
        delete [] images;
 
        for (int i = 0; i < numImages; i++)
        {
            delete result[i];
        }
        delete [] result;
    }
 
    void RGBDHandLocalizer::process()
    {
 
        if (!waitForImages(2000))
        {
            ARMARX_WARNING << "Timeout or error while waiting for image data";
            return;
        }
        if (getImages(images) != numImages)
        {
            ARMARX_WARNING << "Unable to transfer or read images";
            return;
        }
        if (!waitForPointClouds(2000))
        {
            ARMARX_WARNING << "Can't read Pointcloud.";
            return;
        }
 
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr currentPointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr filteredPointCloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
        armarx::SharedRobotInterfacePrx currentRobot = this->robotStatePrx->getSynchronizedRobot();
        std::vector<Eigen::Vector3f> medianCoordinates;
 
 
        getPointClouds(currentPointCloud);
 
        if (!currentPointCloud->isOrganized())
        {
            ARMARX_WARNING << "Pointcloud is not organized.";
            return;
        }
 
 
        IplImage* iplImage = IplImageAdaptor::Adapt(images[0]);
        cv::Mat RGBImage = cv::cvarrToMat(iplImage);
 
        cv::Mat HSVImage;
        cv::cvtColor(RGBImage, HSVImage, cv::COLOR_RGB2HSV);
 
        cv::Mat mask;
        // todo move to onInit..-method
        cv::Scalar minColor(getProperty<int>("hueMin").getValue(),
                            getProperty<int>("satMin").getValue(),
                            getProperty<int>("valMin").getValue());
 
        cv::Scalar maxColor(getProperty<int>("hueMax").getValue(),
                            getProperty<int>("satMax").getValue(),
                            getProperty<int>("valMax").getValue());
        cv::inRange(HSVImage, minColor, maxColor, mask);
 
        //opening
        cv::erode(mask, mask, getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(6, 6)));
        cv::dilate(mask, mask, getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(6, 6)));
        //closing
        cv::dilate(mask, mask, getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3)));
        cv::erode(mask, mask, getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3)));
 
 
        //ARMARX_INFO << "Hand name in robot model:" << handFrameName;
 
 
 
 
        armarx::FramedPositionPtr kinematicMarkerPosition = new armarx::FramedPosition(Eigen::Vector3f(0, 0, 0), markerFrameName, currentRobot->getName());
        armarx::FramedPositionPtr copy = armarx::FramedPositionPtr::dynamicCast(kinematicMarkerPosition->clone());
        copy->changeFrame(currentRobot, sensorFrameName);
        Eigen::Vector3f guessMM = copy->toEigen();
        this->cropFilter(currentPointCloud, filteredPointCloud, getProperty<float>("uncertaintyMM").getValue(), guessMM);
 
        provideResultPointClouds(filteredPointCloud, "Filtered_Cloud");
 
 
 
        this->calcPositions(mask, filteredPointCloud, medianCoordinates);   //Here the calculation happens
 
        armarx::FramedPositionPtr kinematicHandPosition = new armarx::FramedPosition(Eigen::Vector3f(0, 0, 0), handFrameName, currentRobot->getName());
        kinematicHandPosition->changeFrame(currentRobot, markerFrameName);
        armarx::FramedDirectionPtr offsetToHand = new armarx::FramedDirection(kinematicHandPosition->toEigen(), markerFrameName, currentRobot->getName());
 
        VirtualRobot::RobotPtr sharedRobot(new armarx::RemoteRobot(currentRobot));
        offsetToHand->changeFrame(sharedRobot, sensorFrameName);
 
        //Choose the best blob
        if (medianCoordinates.size() > 0)
        {
            float distance = std::numeric_limits<float>::max();
            armarx::FramedPosition min;
            for (Eigen::Vector3f currentMedian : medianCoordinates)
            {
                armarx::FramedPosition currentPos(currentMedian, sensorFrameName, currentRobot->getName());
 
                float currentDist = (kinematicMarkerPosition->toEigen() - currentMedian).norm();
                if (currentDist < distance)
                {
                    min = currentPos;
                    distance = currentDist;
                }
            }
 
 
            armarx::FramedPositionPtr markerPosition = new armarx::FramedPosition(min);
            Eigen::Vector3f handPositionEigen = markerPosition->toEigen() + offsetToHand->toEigen();
 
 
            if (getProperty<bool>("PrimitiveCalibrationOnStart").getValue())
            {
                int numCalibrations = 20;
                if (calibrationCounter < numCalibrations)
                {
                    armarx::FramedPositionPtr kinCopy = armarx::FramedPositionPtr::dynamicCast(kinematicHandPosition->clone());
                    kinCopy->changeFrame(currentRobot, sensorFrameName);
                    Eigen::Vector3f diff = kinCopy->toEigen() - handPositionEigen;
                    this->calibrationSum = this->calibrationSum + diff;
                    calibrationCounter++;
                }
                else
                {
                    handPositionEigen = handPositionEigen + (calibrationSum / numCalibrations);
                }
            }
 
 
            armarx::FramedPositionPtr realHandPosition = new armarx::FramedPosition(handPositionEigen, sensorFrameName, currentRobot->getName());
 
 
 
            armarx::FramedPosePtr handRootPose = armarx::FramedPosePtr::dynamicCast(currentRobot->getRobotNode(handFrameName)->getPoseInRootFrame());
 
 
            armarx::FramedOrientationPtr handOrientation = handRootPose->getOrientation();
            handOrientation->changeFrame(currentRobot, sensorFrameName);
 
            std::scoped_lock lock(positionLock);
 
            this->realHandPose = new armarx::FramedPose(handOrientation->toEigen(), realHandPosition->toEigen(), sensorFrameName, currentRobot->getName());
 
            this->timestamp = armarx::TimestampVariant::nowPtr();
 
 
            this->debugDrawerTopic->setPoseVisu("RGBDHandLocalizer",
                                                "Real_Hand_Position",
                                                this->realHandPose->toGlobal(currentRobot));
            this->debugDrawerTopic->setPoseVisu("RGBDHandLocalizer",
                                                "Kinematic_Hand_Position",
                                                handRootPose->toGlobal(currentRobot));
            this->debugDrawerTopic->setPoseVisu("RGBDHandLocalizer",
                                                "DepthCamera",
                                                currentRobot->getRobotNode(sensorFrameName)->getGlobalPose());
            this->debugDrawerTopic->setSphereVisu("RGBDHandLocalizer",
                                                  "MarkerPos_Sensor",
                                                  new armarx::Vector3(markerPosition->toGlobalEigen(currentRobot)),
                                                  armarx::DrawColor {1, 1, 0, 0.5},
                                                  20.0f);
            this->debugDrawerTopic->setSphereVisu("RGBDHandLocalizer",
                                                  "MarkerPos_Kinematic",
                                                  new armarx::Vector3(kinematicMarkerPosition->toGlobalEigen(currentRobot)),
                                                  armarx::DrawColor {1, 0, 1, 0.5},
                                                  20.0f);
 
        }
        else
        {
            ARMARX_WARNING << "No blob found";
        }
 
 
        //Provide the segmented Image as Result
 
 
        IplImage* img = cvCreateImage(cvSize(640, 480), IPL_DEPTH_8U, 3);
        cv::Mat outMat = cv::cvarrToMat(img);
        cv::cvtColor(mask, outMat, cv::COLOR_GRAY2RGB);
 
        result[1] = images[0];
        result[0] = IplImageAdaptor::Adapt(img);
        provideResultImages(result);
        cvReleaseImage(&img);
        outMat.release();
        //ARMARX_VERBOSE << "process took:  " << (IceUtil::Time::now() - start).toMilliSecondsDouble() << " milliseconds.";
    }
 
 
    void RGBDHandLocalizer::calcPositions(cv::Mat& binaryImage, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pointcloud, std::vector<Eigen::Vector3f>& medianCoordinates)
    {
        medianCoordinates.clear();
        BlobLabeler labeler;
        cv::Mat labeledImage;
        std::vector<std::vector<cv::Point>> blobList;
        labeler.labelBlobs(binaryImage, labeledImage, blobList);
 
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGBA>());
 
        for (std::vector<cv::Point> blob : blobList)
        {
            Eigen::Vector3f sum = Eigen::Vector3f::Zero();
 
            int numberOfPixels = 0;
            for (cv::Point picturePoint : blob)
            {
                pcl::PointXYZRGBA point3D = pointcloud->at(picturePoint.x, picturePoint.y);
                bool point3Dvalid = std::isfinite(point3D.x) && std::isfinite(point3D.y) && std::isfinite(point3D.z);
 
                if (point3Dvalid)
                {
                    outputCloud->push_back(point3D);
 
                    Eigen::Vector3f vector;
                    vector << point3D.x, point3D.y, point3D.z;
                    sum = sum + vector;
                    numberOfPixels ++;
                }
            }
 
            if (numberOfPixels != 0)
            {
                Eigen::Vector3f median = sum / numberOfPixels;
                float markerRadius = getProperty<float>("MarkerRadiusMM").getValue();
                median = median + markerRadius * median.normalized();
                medianCoordinates.push_back(median);
            }
        }
        labeledImage.release();
        visionx::tools::colorizeSegment(outputCloud);
        provideResultPointClouds(outputCloud, "MarkerCloud");
    }
 
    void RGBDHandLocalizer::cropFilter(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr input, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr output, float uncertaintyMM, Eigen::Vector3f guessMM)
    {
 
        //Guesses come from the armarx Framework and so are measured in mm.
 
 
 
 
        //Crop the pointcloud
        this->cropper.setInputCloud(input);
        this->cropper.setKeepOrganized(true);
        this->cropper.setMax(Eigen::Vector4f(uncertaintyMM, uncertaintyMM, uncertaintyMM, 1));
        this->cropper.setMin(Eigen::Vector4f(-uncertaintyMM, -uncertaintyMM, -uncertaintyMM, 1));
        this->cropper.setTranslation(guessMM);
        this->cropper.setRotation(Eigen::Vector3f::Zero());
        this->cropper.filter(*output);
    }
 
    memoryx::ObjectLocalizationResultList RGBDHandLocalizer::localizeObjectClasses(const memoryx::ObjectClassNameList& classNames, const Ice::Current&)
    {
 
        memoryx::ObjectLocalizationResultList resultList;
 
 
        if (classNames.size() == 1)
        {
            if (classNames.at(0) == getProperty<std::string>("HandNameInMemory").getValue())
            {
                if (this->realHandPose)
                {
 
                    std::scoped_lock lock(positionLock);
 
                    memoryx::ObjectLocalizationResult localizationResult;
 
                    localizationResult.objectClassName = getProperty<std::string>("HandNameInMemory").getValue();
                    localizationResult.timeStamp = this->timestamp;
                    localizationResult.orientation = this->realHandPose->getOrientation();
                    localizationResult.position = this->realHandPose->getPosition();
                    localizationResult.recognitionCertainty = 1.0;
                    Eigen::Vector3f mean = Eigen::Vector3f::Zero();
                    Eigen::Matrix3f covar = Eigen::Matrix3f::Identity() * 10000;
                    memoryx::MultivariateNormalDistributionBasePtr dummy = new memoryx::MultivariateNormalDistribution(mean, covar);
                    localizationResult.positionNoise = dummy;
 
 
                    resultList.push_back(localizationResult);
                    ARMARX_INFO << "Hand found at: " << localizationResult.position->output();
                }
                else
                {
                    ARMARX_WARNING << "Hand not found";
                }
 
            }
            else
            {
                ARMARX_WARNING << "Wrong classname requested";
            }
        }
        else
        {
            ARMARX_WARNING << "More than one classname requested";
        }
 
        return resultList;
    }
}