VisualContactDetection.h

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::Component
 * @author     David Schiebener (schiebener at kit dot edu)
 * @date       2011
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#pragma once
 
#include <map>
#include <set>
#include <string>
 
// VisionX
#include <VisionX/core/ImageProcessor.h>
#include <VisionX/interface/components/HandLocalization.h>
#include <VisionX/interface/components/VisualContactDetection.h>
 
 
// Core
#include <ArmarXCore/observers/Observer.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
 
#include <RobotAPI/libraries/core/LinkedPose.h>
 
// MemoryX
#include <MemoryX/interface/observers/ObjectMemoryObserverInterface.h>
 
// IVT
#include <Image/IplImageAdaptor.h>
#include <Math/Math2d.h>
#include <Math/Math3d.h>
 
// OpenCV
//#include <highgui.h>
#include <opencv2/highgui/highgui.hpp>
//#include <OpenCVLegacy.h>
 
class CStereoCalibration;
class CByteImage;
 
namespace visionx
{
    // forward declarations
    class CHandLocalisation;
    class CHandModelVisualizer;
 
    class VisualContactDetectionPropertyDefinitions :
        public visionx::ImageProcessorPropertyDefinitions
    {
    public:
        VisualContactDetectionPropertyDefinitions(std::string prefix) :
            visionx::ImageProcessorPropertyDefinitions(prefix)
        {
            defineOptionalProperty<std::string>("ImageProviderAdapterName",
                                                "Armar3ImageProvider",
                                                "Ice Adapter name of the image provider");
            defineOptionalProperty<std::string>("RobotStateProxyName",
                                                "RobotStateComponent",
                                                "Ice Adapter name of the robot state proxy");
            defineOptionalProperty<std::string>(
                "HandFrameName",
                "TCP R",
                "Name of the robot state frame of the hand that will be localized");
            defineOptionalProperty<std::string>(
                "CameraFrameName",
                "EyeLeftCamera",
                "Name of the robot state frame of the primary camera");
            defineOptionalProperty<int>(
                "NumberOfParticles",
                1500,
                "Number of particles for the particle filter for hand localization");
            defineOptionalProperty<std::string>("ObjectMemoryObserverName",
                                                "ObjectMemoryObserver",
                                                "Name of the object memory observer proxy");
            defineOptionalProperty<std::string>(
                "HandNameInMemoryX", "handright3b", "Name of the hand in the object memory");
            defineOptionalProperty<int>("MinNumPixelsInClusterForCollisionDetection",
                                        3000,
                                        "Minimal number of pixels that a motion cluster must "
                                        "contain to be considered in collision detection");
            defineOptionalProperty<bool>(
                "UseHandLocalization",
                true,
                "Switch on or off whether the hand should be localized with the particle filter. "
                "If it is switched off, the hand pose from MemoryX is used (if available, "
                "otherwise just from the kinematic model).");
            defineOptionalProperty<int>(
                "MinWaitingTimeBetweenTwoFrames",
                300,
                "Minimal time to wait between two frames so that enough motion happens (in ms)");
        }
    };
 
    /**
     * VisualContactDetection determines if the robot hand is colliding with another object, causing it to move.
     * To this end, it localizes the robot hand using the marker ball and the finger tips, calculates the optical
     * flow in the image, clusters it, and looks for a cluster that exist solely in front of the hand.
     *
     * \componentproperties
     * \prop VisionX.VisualContactDetection.ImageProviderAdapterName: Name of the
     *       image provider that delivers the camera images.
     * \prop VisionX.VisualContactDetection.RobotStateProxyName: Name of the robot state
     *       proxy used to obtain the current robot state.
     * \prop VisionX.VisualContactDetection.HandFrameName: Name of the frame in the robot
     *       model that corresponds to the localized hand.
     * \prop VisionX.VisualContactDetection.CameraFrameName: Name of the robot state frame of the primary camera
     */
    class VisualContactDetection :
        virtual public visionx::ImageProcessor,
        virtual public visionx::VisualContactDetectionInterface
    {
    public:
        /**
        * @see Component::getDefaultName()
        */
        std::string
        getDefaultName() const override
        {
            return "VisualContactDetection";
        }
 
        /**
        * Returns the hand pose.
        */
        armarx::FramedPoseBasePtr getHandPose(const Ice::Current& c = Ice::emptyCurrent) override;
 
        /**
        * Returns the positions of the fingertips in this order: thumb, index, middle, ring, pinky.
        */
        visionx::FramedPositionBaseList
        getFingertipPositions(const Ice::Current& c = Ice::emptyCurrent)
            override; // ordering: thumb, index, middle, ring, pinky
 
        void activate(const Ice::Current& c = Ice::emptyCurrent) override;
        void deactivate(const Ice::Current& c = Ice::emptyCurrent) override;
 
 
    protected:
        // inherited from VisionComponent
        void onInitImageProcessor() override;
        void onConnectImageProcessor() override;
        void onExitImageProcessor() override;
 
        void process() override;
 
        /**
         * @see PropertyUser::createPropertyDefinitions()
         */
        armarx::PropertyDefinitionsPtr
        createPropertyDefinitions() override
        {
            return armarx::PropertyDefinitionsPtr(
                new VisualContactDetectionPropertyDefinitions(getConfigIdentifier()));
        }
 
    private:
        std::string providerName;
        ImageProviderInterfacePrx imageProviderPrx;
        std::string robotStateProxyName;
        armarx::RobotStateComponentInterfacePrx robotStateProxy;
        std::string handFrameName, cameraFrameName;
        memoryx::ObjectMemoryObserverInterfacePrx objectMemoryObserver;
        std::string handNameInMemoryX;
        armarx::ChannelRefPtr handMemoryChannel;
        bool useHandLocalization;
        bool active;
        std::mutex activationStateMutex;
        IceUtil::Time timeOfLastExecution;
 
        CStereoCalibration* stereoCalibration;
        CByteImage** cameraImages;
        CByteImage** resultImages;
 
        void localizeHand();
        CHandLocalisation* handLocalization;
        CHandModelVisualizer *handModelVisualizer, *handModelVisualizer1, *handModelVisualizer2,
            *handModelVisualizer3;
        static const bool drawComplexHandModelInResultImage = false;
 
        void calculateOpticalFlowClusters();
        IplImage *pCamLeftIpl, *pCamLeftOldIpl;
        CByteImage *camImgLeftGrey, *camImgLeftGreySmall, *camImgLeftGreyOld,
            *camImgLeftGreyOldSmall, *tempImageRGB, *pSegmentedImage, *pOpticalFlowClusterImage;
        bool firstRun;
        Vec3d oldHandPosSensor;
        std::vector<std::vector<std::vector<float>>> opticalFlowClusters;
        float* pVisOptFlowRaw;
        int* colors;
        static const int imageResizeFactorForOpticalFlowCalculation = 4;
        static const int maxNumOptFlowClusters = 5;
        int minNumPixelsInClusterForCollisionDetection;
        static const int clusteringSampleStep = 1;
        static constexpr float pushDetectionBoxForwardOffsetToTCP = 150.0f;
        float oldCollisionProbability;
 
        bool detectCollision();
        Vec2d handPos2D, pushTarget2D, pushTargetBoxLeftUpperCorner, pushTargetBoxRightLowerCorner;
 
        void drawVisualization(bool collisionDetected);
        CByteImage *tempImageRGB1, *tempImageRGB2, *tempImageRGB3, *tempImageRGB4;
 
        static const bool recordImages = true;
        int visualizationImageNumber;
        std::vector<CByteImage*> cameraImagesForSaving, localizationResultImages, opticalFlowImages,
            opticalFlowClusterImages;
        std::vector<long> timesOfImageCapture;
 
        static void extractAnglesFromRotationMatrix(const Mat3d& mat, Vec3d& angles);
        static IplImage* convertToIplImage(CByteImage* pByteImageRGB);
        static void clusterXMeans(const std::vector<std::vector<float>>& aPoints,
                                  const int nMinNumClusters,
                                  const int nMaxNumClusters,
                                  const float fBICFactor,
                                  std::vector<std::vector<std::vector<float>>>& aaPointClusters,
                                  std::vector<std::vector<int>>& aaOldIndices);
 
        void SetNumberInFileName(std::string& sFileName, int nNumber, int nNumDigits = 4);
 
        VisualContactDetectionListenerPrx listener;
 
        enum EResultImageIndices
        {
            eEverything,
            //eCameraImage,
            //ePoseFromKinematik,
            //ePoseFromLocalization,
            //ePoseFromLocalizationOI,
            //eSegmentedImage,
            //eOpticalFlow,
            //eOpticalFlowClusters,
            eNumberOfResultImages
        };
    };
 
} // namespace visionx