RobotHandLocalizationWithFingertips.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::Component
* @author     Kai Welke <kai dot welke at kit dot edu>
* @copyright  2011 Humanoids Group, HIS, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "RobotHandLocalizationWithFingertips.h"
 
#include "../../../vision_algorithms/HandLocalizationWithFingertips/HandLocalisation.h"
#include "../../../vision_algorithms/HandLocalizationWithFingertips/Visualization/HandModelVisualizer.h"
#include "Helpers/helpers.h"
 
// Eigen
#include <Eigen/Core>
 
// IVT
#include <Calibration/StereoCalibration.h>
#include <Image/ImageProcessor.h>
#include <Math/Math3d.h>
 
// Core
#include <ArmarXCore/core/exceptions/Exception.h>
 
// VisionX
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
 
using namespace armarx;
 
namespace visionx
{
    void
    RobotHandLocalizationWithFingertips::onInitImageProcessor()
    {
        // set desired image provider
        providerName = getProperty<std::string>("ImageProviderAdapterName").getValue();
        usingImageProvider(providerName);
 
        robotStateProxyName = getProperty<std::string>("RobotStateProxyName").getValue();
        usingProxy(robotStateProxyName);
 
        handFrameName = getProperty<std::string>("HandFrameName").getValue();
        cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
    }
 
    void
    RobotHandLocalizationWithFingertips::onConnectImageProcessor()
    {
        // connect to image provider
        ARMARX_INFO << getName() << " connecting to " << providerName << armarx::flush;
        visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName);
        imageProviderPrx = getProxy<ImageProviderInterfacePrx>(providerName);
        imageFormat = imageProviderInfo.imageFormat;
 
        cameraImages = new CByteImage*[2];
        cameraImages[0] = tools::createByteImage(imageProviderInfo);
        cameraImages[1] = tools::createByteImage(imageProviderInfo);
 
        resultImages = new CByteImage*[numResultImages];
 
        for (int i = 0; i < numResultImages; i++)
        {
            resultImages[i] = tools::createByteImage(imageProviderInfo);
        }
 
        // retrieve stereo information
        StereoCalibrationProviderInterfacePrx calibrationProviderPrx =
            StereoCalibrationProviderInterfacePrx::checkedCast(imageProviderPrx);
 
        if (!calibrationProviderPrx)
        {
            ARMARX_ERROR << "Image provider with name " << providerName
                         << " is not a StereoCalibrationProvider" << std::endl;
            return;
        }
 
        stereoCalibration = visionx::tools::convert(calibrationProviderPrx->getStereoCalibration());
 
 
        // connect to robot state proxy
        ARMARX_INFO << getName() << " connecting to " << robotStateProxyName << armarx::flush;
        robotStateProxy = getProxy<RobotStateComponentInterfacePrx>(robotStateProxyName);
 
        this->enableResultImages(numResultImages, imageFormat.dimension, imageFormat.type);
 
        // construct hand localizer
        handLocalization =
            new CHandLocalisation(600, 2, 2, stereoCalibration, DSHT_HAND_MODEL_PATH); //6000, 2, 2
        handModelVisualizer = new CHandModelVisualizer(stereoCalibration);
    }
 
    void
    RobotHandLocalizationWithFingertips::process()
    {
        if (!waitForImages(8000))
        {
            ARMARX_WARNING << "Timeout or error in wait for images" << armarx::flush;
        }
        else
        {
            // get images
            int nNumberImages = getImages(cameraImages);
            ARMARX_VERBOSE << getName() << " got " << nNumberImages << " images";
 
            // get hand pose from robot state
            armarx::PosePtr handNodePosePtr =
                armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
                                                 ->getRobotNode(handFrameName)
                                                 ->getPoseInRootFrame());
            Eigen::Matrix4f handNodePose = handNodePosePtr->toEigen();
            armarx::PosePtr cameraNodePosePtr =
                armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
                                                 ->getRobotNode(cameraFrameName)
                                                 ->getPoseInRootFrame());
            Eigen::Matrix4f cameraNodePose = cameraNodePosePtr->toEigen();
            Eigen::Matrix4f handPoseInCameraFrame = cameraNodePose.inverse() * handNodePose;
            Vec3d handNodePosition = {handPoseInCameraFrame(0, 3),
                                      handPoseInCameraFrame(1, 3),
                                      handPoseInCameraFrame(2, 3)};
            Mat3d handNodeOrientation = {handPoseInCameraFrame(0, 0),
                                         handPoseInCameraFrame(0, 1),
                                         handPoseInCameraFrame(0, 2),
                                         handPoseInCameraFrame(1, 0),
                                         handPoseInCameraFrame(1, 1),
                                         handPoseInCameraFrame(1, 2),
                                         handPoseInCameraFrame(2, 0),
                                         handPoseInCameraFrame(2, 1),
                                         handPoseInCameraFrame(2, 2)};
 
 
            // TODO: get finger config
            double* fingerConfig = new double[6];
            fingerConfig[0] = 90 * M_PI / 180; //100 // palm
            fingerConfig[1] = 25 * M_PI / 180; // thumb1
            fingerConfig[2] = 15 * M_PI / 180; // thumb2
            fingerConfig[3] = 10 * M_PI / 180; // index
            fingerConfig[4] = 5 * M_PI / 180; // middle
            fingerConfig[5] = 5 * M_PI / 180; // ring+pinky
 
 
            // localize hand
            double* estimatedConfig = new double[12];
            double confidenceRating;
            handLocalization->LocaliseHand(cameraImages[0],
                                           cameraImages[1],
                                           handNodePosition,
                                           handNodeOrientation,
                                           fingerConfig,
                                           estimatedConfig,
                                           confidenceRating);
 
            // draw result images
            ::ImageProcessor::CopyImage(cameraImages[0], resultImages[0]);
            ::ImageProcessor::CopyImage(cameraImages[0], resultImages[1]);
            ::ImageProcessor::CopyImage(cameraImages[0], resultImages[2]);
            ::ImageProcessor::Zero(resultImages[3]);
 
            double* localizationResult = handLocalization->GetResultConfig();
            handModelVisualizer->UpdateHandModel(localizationResult,
                                                 drawComplexHandModelInResultImage);
            delete[] localizationResult;
            handModelVisualizer->DrawHandModelV2(resultImages[1]);
            handModelVisualizer->DrawHand(resultImages[2]);
            handModelVisualizer->DrawSegmentedImage(resultImages[3]);
            provideResultImages(resultImages);
        }
    }
 
    void
    RobotHandLocalizationWithFingertips::onExitImageProcessor()
    {
        delete[] cameraImages;
    }
 
    armarx::FramedPoseBasePtr
    RobotHandLocalizationWithFingertips::getHandPose(const Ice::Current& c)
    {
        Eigen::Matrix4f handPose = handLocalization->GetHandPose();
        armarx::FramedPosePtr ret = new armarx::FramedPose(
            handPose, cameraFrameName, robotStateProxy->getSynchronizedRobot()->getName());
        return ret;
    }
 
    visionx::FramedPositionBaseList
    RobotHandLocalizationWithFingertips::getFingertipPositions(const Ice::Current& c)
    {
        visionx::FramedPositionBaseList ret;
        std::vector<Vec3d> fingertipPositions = handLocalization->GetFingertipPositions();
 
        for (size_t i = 0; i < fingertipPositions.size(); i++)
        {
            Eigen::Vector3f position;
            position << fingertipPositions.at(i).x, fingertipPositions.at(i).y,
                fingertipPositions.at(i).z;
            armarx::FramedPositionPtr pos = new armarx::FramedPosition(
                position, cameraFrameName, robotStateProxy->getSynchronizedRobot()->getName());
            ret.push_back(pos);
        }
 
        return ret;
    }
 
} // namespace visionx