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 <string>
#include <set>
 
// VisionX
#include <VisionX/core/ImageProcessor.h>
#include <VisionX/interface/components/VisualContactDetection.h>
#include <VisionX/interface/components/HandLocalization.h>
 
 
// Core
#include <RobotAPI/libraries/core/LinkedPose.h>
#include <ArmarXCore/observers/Observer.h>
#include <ArmarXCore/observers/variant/ChannelRef.h>
 
// MemoryX
#include <MemoryX/interface/observers/ObjectMemoryObserverInterface.h>
 
// IVT
#include <Math/Math3d.h>
#include <Math/Math2d.h>
#include <Image/IplImageAdaptor.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
        };
 
    };
 
}