VisualContactDetection.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 "VisualContactDetection.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/ByteImage.h>
#include <Image/ImageProcessor.h>
#include <Image/IplImageAdaptor.h>
#include <Threading/Threading.h>
 
// OpenMP
#include <omp.h>
 
// Core
#include <ArmarXCore/core/exceptions/Exception.h>
 
#include <RobotAPI/interface/components/ViewSelectionInterface.h>
 
// VisionX
#include <sys/time.h>
 
#include <opencv2/video/tracking.hpp>
 
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
namespace visionx
{
 
    void
    VisualContactDetection::onInitImageProcessor()
    {
        timeOfLastExecution = IceUtil::Time::now();
        active = false;
 
        // set desired image provider
        providerName = getProperty<std::string>("ImageProviderAdapterName").getValue();
        usingImageProvider(providerName);
 
        robotStateProxyName = getProperty<std::string>("RobotStateProxyName").getValue();
        usingProxy(robotStateProxyName);
 
        usingProxy(getProperty<std::string>("ObjectMemoryObserverName").getValue());
 
        handFrameName = getProperty<std::string>("HandFrameName").getValue();
        cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
 
        minNumPixelsInClusterForCollisionDetection =
            getProperty<int>("MinNumPixelsInClusterForCollisionDetection").getValue();
        minNumPixelsInClusterForCollisionDetection /= clusteringSampleStep * clusteringSampleStep *
                                                      imageResizeFactorForOpticalFlowCalculation *
                                                      imageResizeFactorForOpticalFlowCalculation;
 
        useHandLocalization = getProperty<bool>("UseHandLocalization").getValue();
 
        pVisOptFlowRaw = new float[3 * DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT];
 
        offeringTopic("VisualContactDetection");
    }
 
    void
    VisualContactDetection::onConnectImageProcessor()
    {
        // connect to image provider
        ARMARX_INFO << getName() << " connecting to " << providerName;
        visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName);
        imageProviderPrx = getProxy<ImageProviderInterfacePrx>(providerName);
 
        cameraImages = new CByteImage*[2];
        cameraImages[0] = tools::createByteImage(imageProviderInfo);
        cameraImages[1] = tools::createByteImage(imageProviderInfo);
 
        resultImages = new CByteImage*[eNumberOfResultImages];
 
        for (int i = 0; i < eNumberOfResultImages; i++)
        {
            resultImages[i] = tools::createByteImage(imageProviderInfo);
            //resultImages[i] = new CByteImage(3*cameraImages[0]->width, 2*cameraImages[0]->height, CByteImage::eRGB24);
        }
 
        delete resultImages[eEverything];
        resultImages[eEverything] = new CByteImage(
            3 * cameraImages[0]->width, 2 * cameraImages[0]->height, CByteImage::eRGB24);
 
        camImgLeftGrey =
            new CByteImage(cameraImages[0]->width, cameraImages[0]->height, CByteImage::eGrayScale);
        camImgLeftGreySmall =
            new CByteImage(cameraImages[0]->width / imageResizeFactorForOpticalFlowCalculation,
                           cameraImages[0]->height / imageResizeFactorForOpticalFlowCalculation,
                           CByteImage::eGrayScale);
        //camImgLeftGreyOld = new CByteImage(camImgLeftGrey);
        camImgLeftGreyOldSmall = new CByteImage(camImgLeftGreySmall);
        tempImageRGB = new CByteImage(cameraImages[0]);
        tempImageRGB1 = new CByteImage(cameraImages[0]);
        tempImageRGB2 = new CByteImage(cameraImages[0]);
        tempImageRGB3 = new CByteImage(cameraImages[0]);
        tempImageRGB4 = new CByteImage(cameraImages[0]);
        pSegmentedImage = new CByteImage(cameraImages[0]);
        pOpticalFlowClusterImage = new CByteImage(cameraImages[0]);
 
        // 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<armarx::RobotStateComponentInterfacePrx>(robotStateProxyName);
 
        visionx::ImageDimension dim;
        dim.width = 3 * cameraImages[0]->width;
        dim.height = 2 * cameraImages[0]->height;
        this->enableResultImages(1, dim, visionx::eRgb);
 
        // construct hand localizer
        handLocalization = new CHandLocalisation(getProperty<int>("NumberOfParticles").getValue(),
                                                 2,
                                                 2,
                                                 stereoCalibration,
                                                 DSHT_HAND_MODEL_PATH); //6000, 2, 2
        handLocalization->SetParticleVarianceFactor(0.3);
        handModelVisualizer = new CHandModelVisualizer(stereoCalibration);
        handModelVisualizer1 = new CHandModelVisualizer(stereoCalibration);
        handModelVisualizer2 = new CHandModelVisualizer(stereoCalibration);
        handModelVisualizer3 = new CHandModelVisualizer(stereoCalibration);
 
        // other member variables
        firstRun = true;
        oldCollisionProbability = 0;
 
        colors = new int[3 * (maxNumOptFlowClusters + 1)];
 
        for (int j = 0; j < 3 * (maxNumOptFlowClusters + 1); j++)
        {
            colors[j] = rand() % 220 + 30;
        }
 
        // check if MemoryX is running
        handNameInMemoryX = getProperty<std::string>("HandNameInMemoryX").getValue();
 
        if (this->getIceManager()->isObjectReachable(
                getProperty<std::string>("ObjectMemoryObserverName").getValue()))
        {
            ARMARX_INFO_S << "Connecting to ObjectMemoryObserver";
            objectMemoryObserver = getProxy<memoryx::ObjectMemoryObserverInterfacePrx>(
                getProperty<std::string>("ObjectMemoryObserverName").getValue());
            handMemoryChannel =
                armarx::ChannelRefPtr::dynamicCast(objectMemoryObserver->requestObjectClassRepeated(
                    handNameInMemoryX, 200, armarx::DEFAULT_VIEWTARGET_PRIORITY));
 
            if (!handMemoryChannel)
            {
                ARMARX_IMPORTANT_S << "Object " << handNameInMemoryX
                                   << " seems to be unknown to ObjectMemoryObserver";
            }
        }
        else
        {
            ARMARX_INFO_S << "ObjectMemoryObserver not available";
            objectMemoryObserver = NULL;
        }
 
        listener = getTopic<VisualContactDetectionListenerPrx>("VisualContactDetection");
    }
 
    void
    VisualContactDetection::process()
    {
        bool doSomething = false;
 
        {
            std::unique_lock lock(activationStateMutex);
 
            if (!waitForImages(8000))
            {
                ARMARX_IMPORTANT << "Timeout or error in wait for images";
            }
            else
            {
                // get images
                int nNumberImages = getImages(cameraImages);
                ARMARX_DEBUG << getName() << " got " << nNumberImages << " images";
 
                if (nNumberImages > 0 && active)
                {
                    if ((IceUtil::Time::now() - timeOfLastExecution).toMilliSeconds() >=
                        getProperty<int>("MinWaitingTimeBetweenTwoFrames").getValue())
                    {
                        doSomething = true;
                    }
                }
            }
        }
 
        if (doSomething)
        {
            IceUtil::Time startAll = IceUtil::Time::now();
            timeOfLastExecution = IceUtil::Time::now();
 
            ::ImageProcessor::Zero(resultImages[eEverything]);
 
            omp_set_nested(true);
 
#pragma omp parallel sections
            {
#pragma omp section
                {
                    IceUtil::Time start = IceUtil::Time::now();
 
                    // localize the hand
                    localizeHand();
 
                    ARMARX_VERBOSE << "localizeHand() took "
                                   << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
                }
 
#pragma omp section
                {
                    IceUtil::Time start = IceUtil::Time::now();
 
                    // calculate the optical flow and cluster it
                    calculateOpticalFlowClusters();
 
                    ARMARX_VERBOSE << "calculateOpticalFlowClusters() took "
                                   << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
                }
            }
 
            // check for a cluster solely in front of the hand
            IceUtil::Time start = IceUtil::Time::now();
            bool collisionDetected = detectCollision();
            listener->reportContactDetected(collisionDetected);
            ARMARX_VERBOSE << "detectCollision() took "
                           << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
 
            start = IceUtil::Time::now();
            drawVisualization(collisionDetected);
            provideResultImages(resultImages);
            ARMARX_VERBOSE << "drawVisualization() took "
                           << (IceUtil::Time::now() - start).toMilliSeconds() << " ms";
            firstRun = false;
 
            ARMARX_VERBOSE << "Complete calculation took "
                           << (IceUtil::Time::now() - startAll).toMilliSeconds() << " ms";
        }
    }
 
    void
    VisualContactDetection::localizeHand()
    {
        // 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;
 
        // offset from TCP to palm
        Eigen::Vector4f offsetTCPtoPalm = {0, 0, 0, 1};
        Eigen::Vector4f palmPosition = cameraNodePose.inverse() * handNodePose * offsetTCPtoPalm;
 
        //    // TODO: little hack for easier matching
        //    handPoseInCameraFrame(0,3) += 70;
        //    handPoseInCameraFrame(1,3) += 0;
        //    handPoseInCameraFrame(2,3) += -10;
 
        Vec3d tcpPosition = {
            handPoseInCameraFrame(0, 3), handPoseInCameraFrame(1, 3), handPoseInCameraFrame(2, 3)};
        Vec3d handNodePosition = {palmPosition(0), palmPosition(1), palmPosition(2)};
        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)};
 
        ARMARX_VERBOSE_S << "hand pose: " << handNodePose;
        ARMARX_VERBOSE_S << "camera pose: " << cameraNodePose;
        ARMARX_VERBOSE_S << "c.i * h: " << handPoseInCameraFrame;
        //ARMARX_VERBOSE_S << "c * h: " << cameraNodePose*handNodePose;
 
        // TODO: get finger config
        double* fingerConfig = new double[6];
 
        if (false)
        {
            fingerConfig[0] = 30 * M_PI / 180; // 95 // palm
            fingerConfig[1] = 5 * M_PI / 180; // 20 // thumb1
            fingerConfig[2] = 5 * M_PI / 180; // 10 // thumb2
            fingerConfig[3] = 5 * M_PI / 180; // 10 // index
            fingerConfig[4] = 5 * M_PI / 180; // 5 // middle
            fingerConfig[5] = 5 * M_PI / 180; // 5 // ring+pinky
        }
        else
        {
            armarx::SharedRobotInterfacePrx robot = robotStateProxy->getSynchronizedRobot();
            fingerConfig[0] = 1.57f + robot->getRobotNode("Hand Palm 2 R")->getJointValue(); // palm
            fingerConfig[1] = robot->getRobotNode("Thumb R J0")->getJointValue(); // 20 // thumb1
            fingerConfig[2] = robot->getRobotNode("Thumb R J1")->getJointValue(); // 10 // thumb2
            fingerConfig[3] =
                0.5f * (robot->getRobotNode("Index R J0")->getJointValue() +
                        robot->getRobotNode("Index R J1")->getJointValue()); // 10 // index
            fingerConfig[4] =
                0.5f * (robot->getRobotNode("Middle R J0")->getJointValue() +
                        robot->getRobotNode("Middle R J1")->getJointValue()); // 5 // middle
            fingerConfig[5] =
                0.25f * (robot->getRobotNode("Ring R J0")->getJointValue() +
                         robot->getRobotNode("Ring R J1")->getJointValue() +
                         robot->getRobotNode("Pinky R J0")->getJointValue() +
                         robot->getRobotNode("Pinky R J1")->getJointValue()); // 5 // ring+pinky
        }
 
 
        // if MemoryX is available, get hand pose from there
        if (objectMemoryObserver && handMemoryChannel)
        {
            memoryx::ChannelRefBaseSequence instances =
                objectMemoryObserver->getObjectInstances(handMemoryChannel);
 
            if (instances.size() != 0)
            {
                ARMARX_VERBOSE_S << "Using hand pose from MemoryX";
 
                armarx::FramedPositionPtr position =
                    armarx::ChannelRefPtr::dynamicCast(instances.front())
                        ->get<armarx::FramedPosition>("position");
                armarx::FramedOrientationPtr orientation =
                    armarx::ChannelRefPtr::dynamicCast(instances.front())
                        ->get<armarx::FramedOrientation>("orientation");
                armarx::FramedPose handPoseMemory(orientation->toEigen(),
                                                  position->toEigen(),
                                                  position->getFrame(),
                                                  position->agent);
 
                ARMARX_VERBOSE_S << "Pose from memory: " << handPoseMemory;
 
                // convert to camera frame
                if (position->getFrame().compare(cameraFrameName) != 0)
                {
                    auto robot = robotStateProxy->getSynchronizedRobot();
                    Eigen::Matrix4f memoryNodePose =
                        armarx::PosePtr::dynamicCast(
                            robot->getRobotNode(position->getFrame())->getPoseInRootFrame())
                            ->toEigen();
                    Eigen::Matrix4f m =
                        cameraNodePose.inverse() * memoryNodePose * handPoseMemory.toEigen();
                    handPoseMemory = armarx::FramedPose(m, cameraFrameName, robot->getName());
                }
 
                ARMARX_VERBOSE_S << "Pose from memory in camera coordinates: " << handPoseMemory;
 
                // use average of position from kinematic model and from MemoryX
                const float ratio = 0.999f;
                handNodePosition.x =
                    (1.0f - ratio) * handNodePosition.x + ratio * handPoseMemory.position->x;
                handNodePosition.y =
                    (1.0f - ratio) * handNodePosition.y + ratio * handPoseMemory.position->y;
                handNodePosition.z =
                    (1.0f - ratio) * handNodePosition.z + ratio * handPoseMemory.position->z;
            }
            else
            {
                ARMARX_VERBOSE_S << "Hand not yet localized by MemoryX";
            }
        }
        else
        {
            ARMARX_VERBOSE_S << "objectMemoryObserver: " << objectMemoryObserver
                             << "  handMemoryChannel: " << handMemoryChannel;
        }
 
 
        // debug test
        if (false)
        {
            Vec3d xDirection = {100, 0, 0};
            Vec3d yDirection = {0, 100, 0};
            Vec3d zDirection = {0, 0, 100};
            Math3d::MulMatVec(handNodeOrientation, xDirection, tcpPosition, xDirection);
            Math3d::MulMatVec(handNodeOrientation, yDirection, tcpPosition, yDirection);
            Math3d::MulMatVec(handNodeOrientation, zDirection, tcpPosition, zDirection);
            Vec2d tcp, xAxis, yAxis, zAxis;
            stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
                tcpPosition, tcp, false);
            stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
                xDirection, xAxis, false);
            stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
                yDirection, yAxis, false);
            stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
                zDirection, zAxis, false);
            handModelVisualizer->DrawLineIntoImage(
                cameraImages[0], tcp.x, tcp.y, xAxis.x, xAxis.y, 255, 0, 0);
            handModelVisualizer->DrawLineIntoImage(
                cameraImages[0], tcp.x, tcp.y, yAxis.x, yAxis.y, 0, 255, 0);
            handModelVisualizer->DrawLineIntoImage(
                cameraImages[0], tcp.x, tcp.y, zAxis.x, zAxis.y, 0, 0, 255);
        }
 
        // localize hand
        if (useHandLocalization)
        {
            double* estimatedConfig = new double[12];
            double confidenceRating;
            handLocalization->LocaliseHand(cameraImages[0],
                                           cameraImages[1],
                                           handNodePosition,
                                           handNodeOrientation,
                                           fingerConfig,
                                           estimatedConfig,
                                           confidenceRating);
            delete[] estimatedConfig;
        }
        else
        {
            handLocalization->SetSensorConfig(handNodePosition, handNodeOrientation, fingerConfig);
            handLocalization->SetResultConfig(handNodePosition, handNodeOrientation, fingerConfig);
        }
 
        // draw segmentation image
        double* localizationResult = handLocalization->GetResultConfig();
        handModelVisualizer->UpdateHandModel(localizationResult, drawComplexHandModelInResultImage);
        std::string output = "Localization result:";
 
        for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
        {
            output += " ";
            output += std::to_string(localizationResult[i]);
        }
 
        ARMARX_VERBOSE_S << output;
        delete[] localizationResult;
        ::ImageProcessor::Zero(pSegmentedImage);
        handModelVisualizer->DrawSegmentedImage(pSegmentedImage, drawComplexHandModelInResultImage);
    }
 
    void
    VisualContactDetection::calculateOpticalFlowClusters()
    {
        ::ImageProcessor::ConvertImage(cameraImages[0], camImgLeftGrey);
        ::ImageProcessor::Resize(camImgLeftGrey, camImgLeftGreySmall);
 
        if (firstRun)
        {
            //::ImageProcessor::CopyImage(camImgLeftGrey, camImgLeftGreyOld);
            ::ImageProcessor::CopyImage(camImgLeftGreySmall, camImgLeftGreyOldSmall);
        }
 
        pCamLeftIpl = convertToIplImage(camImgLeftGreySmall);
        pCamLeftOldIpl = convertToIplImage(camImgLeftGreyOldSmall);
        cv::Mat mCamLeftMat = cv::cvarrToMat(pCamLeftIpl);
        cv::Mat mCamLeftOldMat = cv::cvarrToMat(pCamLeftOldIpl);
        cv::Mat mOpticalFlowMat;
 
        // prevImg – First 8-bit single-channel input image.
        // nextImg – Second input image of the same size and the same type as prevImg .
        // flow – Computed flow image that has the same size as prevImg and type CV_32FC2 .
        // pyrScale – Parameter specifying the image scale (<1) to build pyramids for each image. pyrScale=0.5 means a classical pyramid, where each next layer is twice smaller than the previous one.
        // levels – Number of pyramid layers including the initial image. levels=1 means that no extra layers are created and only the original images are used.
        // winsize – Averaging window size. Larger values increase the algorithm robustness to image noise and give more chances for fast motion detection, but yield more blurred motion field.
        // iterations – Number of iterations the algorithm does at each pyramid level.
        // polyN – Size of the pixel neighborhood used to find polynomial expansion in each pixel. Larger values mean that the image will be approximated with smoother surfaces, yielding more robust algorithm and more blurred motion field. Typically, polyN =5 or 7.
        // polySigma – Standard deviation of the Gaussian that is used to smooth derivatives used as a basis for the polynomial expansion. For polyN=5 , you can set polySigma=1.1 . For polyN=7 , a good value would be polySigma=1.5 .
        // flags –
        //     Operation flags that can be a combination of the following:
        //     OPTFLOW_USE_INITIAL_FLOW Use the input flow as an initial flow approximation.
        //     OPTFLOW_FARNEBACK_GAUSSIAN Use the Gaussian \texttt{winsize}\times\texttt{winsize} filter instead of a box filter of the same size for optical flow estimation. Usually, this option gives z more accurate flow than with a box filter, at the cost of lower speed. Normally, winsize for a Gaussian window should be set to a larger value to achieve the same level of robustness.
 
        cv::calcOpticalFlowFarneback(mCamLeftMat,
                                     mCamLeftOldMat,
                                     mOpticalFlowMat,
                                     0.5,
                                     5,
                                     20,
                                     7,
                                     7,
                                     1.5,
                                     0); // 0.5, 5, 30, 10, 7, 1.5, 0
 
        cv::Mat mVisOptFlow(mOpticalFlowMat.size(), CV_32FC3);
 
 
#pragma omp parallel for schedule(static, 40)
        for (int j = 0; j < mOpticalFlowMat.rows; j++)
        {
            const int l = imageResizeFactorForOpticalFlowCalculation * j;
 
            for (int k = 0, m = 0; k < mOpticalFlowMat.cols;
                 k++, m += imageResizeFactorForOpticalFlowCalculation)
            {
                mVisOptFlow.at<cv::Vec3f>(j, k).val[0] =
                    0.4 * mOpticalFlowMat.at<cv::Vec2f>(j, k).val[0];
                mVisOptFlow.at<cv::Vec3f>(j, k).val[1] =
                    0.4 * mOpticalFlowMat.at<cv::Vec2f>(j, k).val[1];
                mVisOptFlow.at<cv::Vec3f>(j, k).val[2] = 0;
 
                for (int n = 0; n < imageResizeFactorForOpticalFlowCalculation; n++)
                {
                    for (int o = 0; o < imageResizeFactorForOpticalFlowCalculation; o++)
                    {
                        pVisOptFlowRaw[3 * DSHT_IMAGE_WIDTH * (l + n) + 3 * (m + o)] =
                            0.5f * (mVisOptFlow.at<cv::Vec3f>(j, k).val[2] + 1);
                        pVisOptFlowRaw[3 * DSHT_IMAGE_WIDTH * (l + n) + 3 * (m + o) + 1] =
                            0.5f * (mVisOptFlow.at<cv::Vec3f>(j, k).val[1] + 1);
                        pVisOptFlowRaw[3 * DSHT_IMAGE_WIDTH * (l + n) + 3 * (m + o) + 2] =
                            0.5f * (mVisOptFlow.at<cv::Vec3f>(j, k).val[0] + 1);
                    }
                }
            }
        }
 
 
        // clustering by position and direction
        std::vector<std::vector<float>> aPixelsAndFlowDirections;
        std::vector<float> aPixelPosAndFlowDirection;
        aPixelPosAndFlowDirection.resize(4);
 
        for (int j = 0; j < mOpticalFlowMat.rows; j += clusteringSampleStep)
        {
            for (int k = 0; k < mOpticalFlowMat.cols; k += clusteringSampleStep)
            {
                aPixelPosAndFlowDirection.at(0) = imageResizeFactorForOpticalFlowCalculation * j;
                aPixelPosAndFlowDirection.at(1) = imageResizeFactorForOpticalFlowCalculation * k;
                aPixelPosAndFlowDirection.at(2) = 4000 * mOpticalFlowMat.at<cv::Vec2f>(j, k).val[0];
                aPixelPosAndFlowDirection.at(3) = 4000 * mOpticalFlowMat.at<cv::Vec2f>(j, k).val[1];
 
                aPixelsAndFlowDirections.push_back(aPixelPosAndFlowDirection);
            }
        }
 
        // remove points with zero motion
        std::vector<std::vector<float>> aZeroMotionCluster;
 
        for (size_t j = 0; j < aPixelsAndFlowDirections.size(); j++)
        {
            float fAbsMotion = fabs(aPixelsAndFlowDirections.at(j).at(2)) +
                               fabs(aPixelsAndFlowDirections.at(j).at(3));
 
            if (fAbsMotion < 350) // 350
            {
                aZeroMotionCluster.push_back(aPixelsAndFlowDirections.at(j));
                aPixelsAndFlowDirections.at(j) =
                    aPixelsAndFlowDirections.at(aPixelsAndFlowDirections.size() - 1);
                aPixelsAndFlowDirections.pop_back();
                j--;
            }
        }
 
        std::vector<std::vector<int>> aOldIndices;
        const float fBIC = 0.08; // bigger = more clusters (default: 0.05)
        clusterXMeans(aPixelsAndFlowDirections,
                      1,
                      maxNumOptFlowClusters,
                      fBIC,
                      opticalFlowClusters,
                      aOldIndices);
 
        opticalFlowClusters.push_back(aZeroMotionCluster);
 
        ARMARX_VERBOSE_S << opticalFlowClusters.size() << " clusters" << armarx::flush;
 
        //::ImageProcessor::CopyImage(camImgLeftGrey, camImgLeftGreyOld);
        ::ImageProcessor::CopyImage(camImgLeftGreySmall, camImgLeftGreyOldSmall);
    }
 
    bool
    VisualContactDetection::detectCollision()
    {
        //************************************************************************************************************
        // Determine push direction and area in front of the hand that will be analyzed
        //************************************************************************************************************
 
 
        Eigen::Matrix4f handNodePose =
            armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
                                             ->getRobotNode(handFrameName)
                                             ->getPoseInRootFrame())
                ->toEigen();
        Vec3d handPosSensor = {handNodePose(0, 3), handNodePose(1, 3), handNodePose(2, 3)};
 
        if (firstRun)
        {
            Math3d::SetVec(oldHandPosSensor, handPosSensor);
        }
 
 
        Vec3d pushDirection, pushDirectionNormalized;
        //Math3d::SubtractVecVec(handPosPF, oldHandPosPF, pushDirection);
        Math3d::SubtractVecVec(handPosSensor, oldHandPosSensor, pushDirection);
        Math3d::SetVec(pushDirectionNormalized, pushDirection);
 
        ARMARX_INFO << "Push direction: " << pushDirection.x << " " << pushDirection.y << " "
                    << pushDirection.z;
 
        if (Math3d::Length(pushDirection) > 0)
        {
            Math3d::NormalizeVec(pushDirectionNormalized);
            Math3d::MulVecScalar(pushDirection,
                                 pushDetectionBoxForwardOffsetToTCP / Math3d::Length(pushDirection),
                                 pushDirection);
        }
 
        // check if hand is moving forward
        Eigen::Vector3f forwardAxis;
        forwardAxis << 0.0f, 1.0f, 1.0f; // -1, 0, 1
        forwardAxis.normalize();
        forwardAxis = handNodePose.block<3, 3>(0, 0) * forwardAxis;
        Vec3d forwardDirection = {forwardAxis(0), forwardAxis(1), forwardAxis(2)};
        ARMARX_INFO << "Angle to forward direction: "
                    << Math3d::ScalarProduct(forwardDirection, pushDirectionNormalized);
 
        //    // just for testing!!
        //    pushDirectionNormalized = forwardDirection;
        //    Math3d::MulVecScalar(forwardDirection, pushDetectionBoxForwardOffsetToTCP, pushDirection);
 
        if ((Math3d::ScalarProduct(forwardDirection, pushDirectionNormalized) < -0.6f) ||
            (Math3d::Length(pushDirection) == 0))
        {
            ARMARX_INFO << "Not checking for collision because arm is not moving forward";
            handPos2D.x = -1;
            handPos2D.y = -1;
            return false;
        }
 
        // create box
 
        Eigen::Vector3f pushDirectionForBox = {pushDirection.x, pushDirection.y, pushDirection.z};
        forwardAxis *= 1.0f * pushDetectionBoxForwardOffsetToTCP;
        pushDirectionForBox = 0.5f * (pushDirectionForBox + forwardAxis);
        Eigen::Matrix4f cameraNodePose =
            armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
                                             ->getRobotNode(cameraFrameName)
                                             ->getPoseInRootFrame())
                ->toEigen();
        Eigen::Vector3f pushDirectionInCameraCoords =
            cameraNodePose.block<3, 3>(0, 0) * pushDirectionForBox;
        Eigen::Matrix4f handPosePF = handLocalization->GetHandPose();
        Vec3d handPosPF = {handPosePF(0, 3), handPosePF(1, 3), handPosePF(2, 3)};
        Vec3d pushTarget = {handPosPF.x + pushDirectionInCameraCoords(0),
                            handPosPF.y + pushDirectionInCameraCoords(1),
                            handPosPF.z + pushDirectionInCameraCoords(2)};
 
        stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
            handPosPF, handPos2D, false);
        stereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(
            pushTarget, pushTarget2D, false);
 
        const float handCameraDistance =
            (handNodePose.block<3, 1>(0, 3) - cameraNodePose.block<3, 1>(0, 3)).norm();
        const float boxSize = 80.0f;
        Vec2d pushTargetBoxSize = {boxSize * 600 / handCameraDistance,
                                   boxSize * 600 / handCameraDistance};
        ARMARX_INFO << "pushTargetBoxSize: " << pushTargetBoxSize.x;
 
        Math2d::SetVec(pushTargetBoxLeftUpperCorner,
                       pushTarget2D.x - pushTargetBoxSize.x,
                       pushTarget2D.y - pushTargetBoxSize.y);
        Math2d::SetVec(pushTargetBoxRightLowerCorner,
                       pushTarget2D.x + pushTargetBoxSize.x,
                       pushTarget2D.y + pushTargetBoxSize.y);
 
        if (pushTargetBoxLeftUpperCorner.x < 0)
        {
            pushTargetBoxLeftUpperCorner.x = 0;
        }
 
        if (pushTargetBoxLeftUpperCorner.y < 0)
        {
            pushTargetBoxLeftUpperCorner.y = 0;
        }
 
        if (pushTargetBoxRightLowerCorner.x > DSHT_IMAGE_WIDTH - 1)
        {
            pushTargetBoxRightLowerCorner.x = DSHT_IMAGE_WIDTH - 1;
        }
 
        if (pushTargetBoxRightLowerCorner.y > DSHT_IMAGE_HEIGHT - 1)
        {
            pushTargetBoxRightLowerCorner.y = DSHT_IMAGE_HEIGHT - 1;
        }
 
        Math3d::SetVec(oldHandPosSensor, handPosSensor);
 
 
        //************************************************************************************************************
        // Visual contact detection
        // Check if one of the clusters of optical flow is within the rectangle in the area where the hand
        // pushes towards, but not in the rest of the image (except where the hand and arm are).
        //************************************************************************************************************
 
        bool bContact = false;
 
        std::vector<int> aNumClusterPointsInPushingBox, aNumClusterPointsInHandArea,
            aNumClusterPointsInRestOfImage;
 
        for (size_t j = 0; j < opticalFlowClusters.size(); j++)
        {
            aNumClusterPointsInPushingBox.push_back(0);
            aNumClusterPointsInHandArea.push_back(0);
            aNumClusterPointsInRestOfImage.push_back(0);
        }
 
        for (size_t j = 0; j < opticalFlowClusters.size(); j++)
        {
            for (size_t k = 0; k < opticalFlowClusters.at(j).size(); k++)
            {
                const float y = opticalFlowClusters.at(j).at(k).at(0);
                const float x = opticalFlowClusters.at(j).at(k).at(1);
                int nIndex = DSHT_IMAGE_WIDTH * y + x;
 
                if (0 <= nIndex && nIndex < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT)
                {
                    if (pSegmentedImage->pixels[3 * nIndex])
                    {
                        aNumClusterPointsInHandArea.at(j)++;
                    }
                    else if (pushTargetBoxLeftUpperCorner.x <= x &&
                             x <= pushTargetBoxRightLowerCorner.x &&
                             pushTargetBoxLeftUpperCorner.y <= y &&
                             y <= pushTargetBoxRightLowerCorner.y)
                    {
                        aNumClusterPointsInPushingBox.at(j)++;
                    }
                    else
                    {
                        aNumClusterPointsInRestOfImage.at(j)++;
                    }
                }
                else
                {
                    aNumClusterPointsInRestOfImage.at(j)++;
                }
            }
        }
 
        float fBestRatio = 0;
        int nBestIndex = -1;
 
        for (size_t j = 0; j < opticalFlowClusters.size(); j++)
        {
            if (aNumClusterPointsInPushingBox.at(j) > minNumPixelsInClusterForCollisionDetection)
            {
                float fRatio = (float)aNumClusterPointsInPushingBox.at(j) /
                               (float)(aNumClusterPointsInPushingBox.at(j) +
                                       aNumClusterPointsInRestOfImage.at(j));
 
                if (fRatio > fBestRatio)
                {
                    fBestRatio = fRatio;
                    nBestIndex = j;
                }
            }
        }
 
        ARMARX_LOG << "Contact probability: " << fBestRatio;
 
        bContact = (fBestRatio > 0.5f && oldCollisionProbability > 0.5);
        oldCollisionProbability = fBestRatio;
 
        if (bContact)
        {
 
            ARMARX_LOG << "Region size: "
                       << clusteringSampleStep * clusteringSampleStep *
                              imageResizeFactorForOpticalFlowCalculation *
                              imageResizeFactorForOpticalFlowCalculation *
                              aNumClusterPointsInPushingBox.at(nBestIndex);
        }
 
        return bContact;
    }
 
    void
    VisualContactDetection::drawVisualization(bool collisionDetected)
    {
        timesOfImageCapture.push_back(IceUtil::Time::now().toMilliSeconds());
 
#pragma omp parallel sections
        {
 
#pragma omp section
            {
                if (recordImages)
                {
                    CByteImage* pNew = new CByteImage(cameraImages[0]);
                    ::ImageProcessor::CopyImage(cameraImages[0], pNew);
                    cameraImagesForSaving.push_back(pNew);
                }
            }
 
 
            // hand localization
 
#pragma omp section
            {
                double* sensorConfig = handLocalization->GetSensorConfig();
                handModelVisualizer1->UpdateHandModel(sensorConfig,
                                                      drawComplexHandModelInResultImage);
                delete[] sensorConfig;
                ::ImageProcessor::CopyImage(cameraImages[0], tempImageRGB1);
                handModelVisualizer1->DrawHandModelV2(tempImageRGB1);
 
                for (int i = 0; i < cameraImages[0]->height; i++)
                {
                    for (int j = 0; j < cameraImages[0]->width; j++)
                    {
                        resultImages[eEverything]
                            ->pixels[3 * (i * 3 * cameraImages[0]->width + j)] =
                            tempImageRGB1->pixels[3 * (i * cameraImages[0]->width + j)];
                        resultImages[eEverything]
                            ->pixels[3 * (i * 3 * cameraImages[0]->width + j) + 1] =
                            tempImageRGB1->pixels[3 * (i * cameraImages[0]->width + j) + 1];
                        resultImages[eEverything]
                            ->pixels[3 * (i * 3 * cameraImages[0]->width + j) + 2] =
                            tempImageRGB1->pixels[3 * (i * cameraImages[0]->width + j) + 2];
                    }
                }
            }
 
 
#pragma omp section
            {
                double* localizationResult = handLocalization->GetResultConfig();
                handModelVisualizer2->UpdateHandModel(localizationResult,
                                                      drawComplexHandModelInResultImage);
                delete[] localizationResult;
                ::ImageProcessor::CopyImage(cameraImages[0], tempImageRGB2);
                handModelVisualizer2->DrawHandModelV2(tempImageRGB2, true);
 
                for (int i = 0; i < cameraImages[0]->height; i++)
                {
                    for (int j = 0, j2 = cameraImages[0]->width; j < cameraImages[0]->width;
                         j++, j2++)
                    {
                        resultImages[eEverything]
                            ->pixels[3 * (i * 3 * cameraImages[0]->width + j2)] =
                            tempImageRGB2->pixels[3 * (i * cameraImages[0]->width + j)];
                        resultImages[eEverything]
                            ->pixels[3 * (i * 3 * cameraImages[0]->width + j2) + 1] =
                            tempImageRGB2->pixels[3 * (i * cameraImages[0]->width + j) + 1];
                        resultImages[eEverything]
                            ->pixels[3 * (i * 3 * cameraImages[0]->width + j2) + 2] =
                            tempImageRGB2->pixels[3 * (i * cameraImages[0]->width + j) + 2];
                    }
                }
 
                if (recordImages)
                {
                    CByteImage* pNew = new CByteImage(tempImageRGB2);
                    ::ImageProcessor::CopyImage(tempImageRGB2, pNew);
                    localizationResultImages.push_back(pNew);
                }
            }
 
 
#pragma omp section
            {
                double* localizationResult = handLocalization->GetResultConfig();
                handModelVisualizer3->UpdateHandModel(localizationResult,
                                                      drawComplexHandModelInResultImage);
                delete[] localizationResult;
                ::ImageProcessor::CopyImage(cameraImages[1], tempImageRGB3);
                handModelVisualizer3->DrawHandModelV2(tempImageRGB3, false);
 
                for (int i = 0; i < cameraImages[0]->height; i++)
                {
                    for (int j = 0, j2 = 2 * cameraImages[0]->width; j < cameraImages[0]->width;
                         j++, j2++)
                    {
                        resultImages[eEverything]
                            ->pixels[3 * (i * 3 * cameraImages[0]->width + j2)] =
                            tempImageRGB3->pixels[3 * (i * cameraImages[0]->width + j)];
                        resultImages[eEverything]
                            ->pixels[3 * (i * 3 * cameraImages[0]->width + j2) + 1] =
                            tempImageRGB3->pixels[3 * (i * cameraImages[0]->width + j) + 1];
                        resultImages[eEverything]
                            ->pixels[3 * (i * 3 * cameraImages[0]->width + j2) + 2] =
                            tempImageRGB3->pixels[3 * (i * cameraImages[0]->width + j) + 2];
                    }
                }
            }
 
 
#pragma omp section
            {
                for (int i = 0, i2 = cameraImages[0]->height; i < cameraImages[0]->height;
                     i++, i2++)
                {
                    for (int j = 0; j < cameraImages[0]->width; j++)
                    {
                        resultImages[eEverything]
                            ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j)] =
                            pSegmentedImage->pixels[3 * (i * cameraImages[0]->width + j)];
                        resultImages[eEverything]
                            ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j) + 1] =
                            pSegmentedImage->pixels[3 * (i * cameraImages[0]->width + j) + 1];
                        resultImages[eEverything]
                            ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j) + 2] =
                            pSegmentedImage->pixels[3 * (i * cameraImages[0]->width + j) + 2];
                    }
                }
            }
 
 
            // optical flow
 
#pragma omp section
            {
                ::ImageProcessor::Zero(tempImageRGB4);
 
                //            const int step = 2*clusteringSampleStep*imageResizeFactorForOpticalFlowCalculation;
                //            for (int j=0; j<DSHT_IMAGE_HEIGHT; j+=step)
                //            {
                //                for (int i=0; i<DSHT_IMAGE_WIDTH; i+=step)
                //                {
                //                    const int flowX = 5*step*(pVisOptFlowRaw[3*(j*DSHT_IMAGE_WIDTH+i)+2]-0.5);
                //                    const int flowY = 5*step*(pVisOptFlowRaw[3*(j*DSHT_IMAGE_WIDTH+i)+1]-0.5);
                //                    handModelVisualizer->DrawLineIntoImage(tempImageRGB4, i, j, i+flowX, j+flowY, 255, 255, 255);
                //                }
                //            }
 
                for (int j = 0; j < 3 * DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT; j++)
                {
                    int nValue = 255 * pVisOptFlowRaw[j];
                    //resultImages[eOpticalFlow]->pixels[j] = (0 > nValue) ? 0 : ((255 < nValue) ? 255 : nValue);
                    tempImageRGB4->pixels[j] = (0 > nValue) ? 0 : ((255 < nValue) ? 255 : nValue);
                }
 
                for (int i = 0, i2 = cameraImages[0]->height; i < cameraImages[0]->height;
                     i++, i2++)
                {
                    for (int j = 0, j2 = cameraImages[0]->width; j < cameraImages[0]->width;
                         j++, j2++)
                    {
                        resultImages[eEverything]
                            ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2)] =
                            tempImageRGB4->pixels[3 * (i * cameraImages[0]->width + j)];
                        resultImages[eEverything]
                            ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2) + 1] =
                            tempImageRGB4->pixels[3 * (i * cameraImages[0]->width + j) + 1];
                        resultImages[eEverything]
                            ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2) + 2] =
                            tempImageRGB4->pixels[3 * (i * cameraImages[0]->width + j) + 2];
                    }
                }
 
                if (recordImages)
                {
                    CByteImage* pNew = new CByteImage(tempImageRGB4);
                    ::ImageProcessor::CopyImage(tempImageRGB4, pNew);
                    opticalFlowImages.push_back(pNew);
                }
            }
 
 
#pragma omp section
            {
                ::ImageProcessor::Zero(pOpticalFlowClusterImage);
 
                for (size_t j = 0; j < opticalFlowClusters.size(); j++)
                {
                    ARMARX_VERBOSE_S << "Cluster " << j << ": " << opticalFlowClusters.at(j).size()
                                     << " points" << armarx::flush;
 
                    for (size_t k = 0; k < opticalFlowClusters.at(j).size(); k++)
                    {
                        int nIndex = DSHT_IMAGE_WIDTH * opticalFlowClusters.at(j).at(k).at(0) +
                                     opticalFlowClusters.at(j).at(k).at(1);
 
                        if (0 <= nIndex && nIndex < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT)
                        {
                            for (int l = 0; l < imageResizeFactorForOpticalFlowCalculation *
                                                    clusteringSampleStep;
                                 l++)
                            {
                                for (int m = 0; m < imageResizeFactorForOpticalFlowCalculation *
                                                        clusteringSampleStep;
                                     m++)
                                {
                                    int nIndex2 = nIndex + l * DSHT_IMAGE_WIDTH + m;
 
                                    if (nIndex2 < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT)
                                    {
                                        pOpticalFlowClusterImage->pixels[3 * nIndex2] =
                                            colors[3 * j];
                                        pOpticalFlowClusterImage->pixels[3 * nIndex2 + 1] =
                                            colors[3 * j + 1];
                                        pOpticalFlowClusterImage->pixels[3 * nIndex2 + 2] =
                                            colors[3 * j + 2];
                                    }
                                }
                            }
                        }
                    }
                }
 
                // collision detection
 
                if (handPos2D.x != -1 || handPos2D.y != -1)
                {
                    for (int j = 0; j < 3 * DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT; j++)
                    {
                        if (pSegmentedImage->pixels[j])
                        {
                            pOpticalFlowClusterImage->pixels[j] /= 2;
                        }
                    }
 
                    handModelVisualizer->DrawCross(
                        pOpticalFlowClusterImage, handPos2D.x, handPos2D.y, 255, 255, 255);
                    int n = collisionDetected ? 8 : 2;
 
                    for (int i = 0; i < n; i++)
                    {
                        handModelVisualizer->DrawLineIntoImage(pOpticalFlowClusterImage,
                                                               pushTargetBoxLeftUpperCorner.x + i,
                                                               pushTargetBoxLeftUpperCorner.y + i,
                                                               pushTargetBoxRightLowerCorner.x - i,
                                                               pushTargetBoxLeftUpperCorner.y + i,
                                                               255,
                                                               255,
                                                               255);
                        handModelVisualizer->DrawLineIntoImage(pOpticalFlowClusterImage,
                                                               pushTargetBoxRightLowerCorner.x - i,
                                                               pushTargetBoxLeftUpperCorner.y + 1,
                                                               pushTargetBoxRightLowerCorner.x - i,
                                                               pushTargetBoxRightLowerCorner.y - i,
                                                               255,
                                                               255,
                                                               255);
                        handModelVisualizer->DrawLineIntoImage(pOpticalFlowClusterImage,
                                                               pushTargetBoxRightLowerCorner.x - i,
                                                               pushTargetBoxRightLowerCorner.y - i,
                                                               pushTargetBoxLeftUpperCorner.x + i,
                                                               pushTargetBoxRightLowerCorner.y - i,
                                                               255,
                                                               255,
                                                               255);
                        handModelVisualizer->DrawLineIntoImage(pOpticalFlowClusterImage,
                                                               pushTargetBoxLeftUpperCorner.x + i,
                                                               pushTargetBoxRightLowerCorner.y - i,
                                                               pushTargetBoxLeftUpperCorner.x + i,
                                                               pushTargetBoxLeftUpperCorner.y + i,
                                                               255,
                                                               255,
                                                               255);
                    }
 
                    for (int i = 0, i2 = cameraImages[0]->height; i < cameraImages[0]->height;
                         i++, i2++)
                    {
                        for (int j = 0, j2 = 2 * cameraImages[0]->width; j < cameraImages[0]->width;
                             j++, j2++)
                        {
                            resultImages[eEverything]
                                ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2)] =
                                pOpticalFlowClusterImage
                                    ->pixels[3 * (i * cameraImages[0]->width + j)];
                            resultImages[eEverything]
                                ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2) + 1] =
                                pOpticalFlowClusterImage
                                    ->pixels[3 * (i * cameraImages[0]->width + j) + 1];
                            resultImages[eEverything]
                                ->pixels[3 * (i2 * 3 * cameraImages[0]->width + j2) + 2] =
                                pOpticalFlowClusterImage
                                    ->pixels[3 * (i * cameraImages[0]->width + j) + 2];
                        }
                    }
                }
 
                if (recordImages)
                {
                    CByteImage* pNew = new CByteImage(pOpticalFlowClusterImage);
                    ::ImageProcessor::CopyImage(pOpticalFlowClusterImage, pNew);
                    opticalFlowClusterImages.push_back(pNew);
                }
            }
        }
    }
 
    void
    VisualContactDetection::onExitImageProcessor()
    {
        delete[] cameraImages;
 
        if (recordImages)
        {
            ARMARX_INFO << "Writing all visualization images to disk...";
            std::string path = "/localhome/ottenhau/VisColDet/";
            std::string fileNameCam = path + "cam0000.bmp";
            std::string fileNameLocRes = path + "locres0000.bmp";
            std::string fileNameOptFlow = path + "optflow0000.bmp";
            std::string fileNameOptFlowClus = path + "optflowclus0000.bmp";
            std::string fileNameTimes = path + "times.txt";
            FILE* pFile = fopen(fileNameTimes.c_str(), "wt");
            fprintf(pFile, "%ld \n", timesOfImageCapture.size() - 1);
 
            for (long i = 0; i < (long)timesOfImageCapture.size() - 1; i++)
            {
                fprintf(pFile, "%ld   %ld \n", i, timesOfImageCapture.at(i));
                SetNumberInFileName(fileNameCam, i, 4);
                SetNumberInFileName(fileNameLocRes, i, 4);
                SetNumberInFileName(fileNameOptFlow, i, 4);
                SetNumberInFileName(fileNameOptFlowClus, i, 4);
                cameraImagesForSaving.at(i)->SaveToFile(fileNameCam.c_str());
                localizationResultImages.at(i)->SaveToFile(fileNameLocRes.c_str());
                opticalFlowImages.at(i)->SaveToFile(fileNameOptFlow.c_str());
                opticalFlowClusterImages.at(i)->SaveToFile(fileNameOptFlowClus.c_str());
                delete cameraImagesForSaving.at(i);
                delete localizationResultImages.at(i);
                delete opticalFlowImages.at(i);
                delete opticalFlowClusterImages.at(i);
 
                if (i % 20 == 0)
                {
                    ARMARX_VERBOSE << "Image " << i << " of " << timesOfImageCapture.size();
                }
            }
 
            fclose(pFile);
            ARMARX_INFO << "Finished writing all visualization images to disk";
        }
    }
 
    void
    VisualContactDetection::extractAnglesFromRotationMatrix(const Mat3d& mat, Vec3d& angles)
    {
        angles.y = asin(mat.r3);
        angles.x = atan2((-mat.r6), (mat.r9));
        angles.z = atan2((-mat.r2), (mat.r1));
    }
 
    IplImage*
    VisualContactDetection::convertToIplImage(CByteImage* pByteImageRGB)
    {
        if (pByteImageRGB->type == CByteImage::eRGB24)
        {
            unsigned char cTemp;
 
            for (int j = 0; j < pByteImageRGB->width * pByteImageRGB->height; j++)
            {
                cTemp = pByteImageRGB->pixels[3 * j];
                pByteImageRGB->pixels[3 * j] = pByteImageRGB->pixels[3 * j + 2];
                pByteImageRGB->pixels[3 * j + 2] = cTemp;
            }
        }
 
        IplImage* pRet = IplImageAdaptor::Adapt(pByteImageRGB);
        return pRet;
    }
 
    void
    VisualContactDetection::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)
    {
        aaPointClusters.clear();
        aaOldIndices.clear();
        const int nNumberOfSamples = aPoints.size();
 
        if (nNumberOfSamples < nMaxNumClusters)
        {
            ARMARX_IMPORTANT_S << "Not enough points for clustering (only " << nNumberOfSamples
                               << " points)" << armarx::flush;
            return;
        }
 
        ARMARX_VERBOSE_S << "Number of points: " << nNumberOfSamples;
 
 
        cv::Mat mSamples;
        const int nNumberOfDifferentInitialisations = 4;
        cv::TermCriteria tTerminationCriteria(
            cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, 0.01);
 
        // copy the points
        const int nDimension = aPoints.at(0).size();
        mSamples.create(nNumberOfSamples, nDimension, CV_32FC1);
 
        for (int i = 0; i < nNumberOfSamples; i++)
        {
            for (int j = 0; j < nDimension; j++)
            {
                mSamples.at<float>(i, j) = aPoints.at(i).at(j);
            }
        }
 
        std::vector<std::vector<std::vector<float>>>* aaPointClustersForAllK =
            new std::vector<std::vector<std::vector<float>>>[nMaxNumClusters + 1];
        std::vector<std::vector<int>>* aaOldIndicesForAllK =
            new std::vector<std::vector<int>>[nMaxNumClusters + 1];
 
 
        // execute k-means for several values of k and find the value for k that minimizes the
        // Bayesian Information Criterion (BIC)
        double dMinBIC = 10000000;
        int nOptK = nMinNumClusters;
 
#pragma omp parallel for schedule(dynamic, 1)
        for (int k = nMaxNumClusters; k >= nMinNumClusters; k--)
        {
            double dKMeansCompactness, dLogVar, dBIC;
            cv::Mat mClusterLabelsLocal;
            mClusterLabelsLocal.create(nNumberOfSamples, 1, CV_32SC1);
            cv::Mat mClusterCentersLocal; // = NULL;
            dKMeansCompactness = cv::kmeans(mSamples,
                                            k,
                                            mClusterLabelsLocal,
                                            tTerminationCriteria,
                                            nNumberOfDifferentInitialisations,
                                            cv::KMEANS_RANDOM_CENTERS,
                                            mClusterCentersLocal);
 
            const int nNumberOfFreeParameters = (k - 1) + (nDimension * k) + k;
            dLogVar = log(dKMeansCompactness);
            dBIC = fBICFactor * 0.35 * dLogVar +
                   ((double)nNumberOfFreeParameters / (double)nNumberOfSamples) *
                       log((double)nNumberOfSamples);
 
#pragma omp critical
            if (dBIC < dMinBIC)
            {
                dMinBIC = dBIC;
                nOptK = k;
            }
 
            if (dBIC == dMinBIC)
            {
                std::vector<float> vPoint;
                vPoint.resize(nDimension);
 
                for (int i = 0; i < k; i++)
                {
                    std::vector<std::vector<float>> aNewCluster;
                    aaPointClustersForAllK[k].push_back(aNewCluster);
                    std::vector<int> aClusterIndices;
                    aaOldIndicesForAllK[k].push_back(aClusterIndices);
                }
 
                for (int i = 0; i < nNumberOfSamples; i++)
                {
                    const int nLabel = mClusterLabelsLocal.at<int>(i, 0);
 
                    if ((nLabel >= 0) && (nLabel < k))
                    {
                        for (int j = 0; j < nDimension; j++)
                        {
                            vPoint.at(j) = mSamples.at<float>(i, j);
                        }
 
                        aaPointClustersForAllK[k].at(nLabel).push_back(vPoint);
                        aaOldIndicesForAllK[k].at(nLabel).push_back(i);
                    }
 
                    //else
                    //{
                    //    ARMARX_WARNING_S << "Invalid cluster label: " << nLabel << "\n nOptK: " << nOptK << ", i: " << i << ", nNumberOfSamples: " << nNumberOfSamples << armarx::flush;
                    //    break;
                    //}
                }
            }
 
            //ARMARX_VERBOSE_S << "k-means with " << i << " clusters. log(var): " << dLogVar << " BIC: " << dBIC;
        }
 
        // return results with best k
        aaPointClusters = aaPointClustersForAllK[nOptK];
        aaOldIndices = aaOldIndicesForAllK[nOptK];
 
        delete[] aaPointClustersForAllK;
        delete[] aaOldIndicesForAllK;
    }
 
    armarx::FramedPoseBasePtr
    VisualContactDetection::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
    VisualContactDetection::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;
    }
 
    void
    VisualContactDetection::activate(const Ice::Current& c)
    {
        std::unique_lock lock(activationStateMutex);
 
        if (!waitForImages(1000))
        {
            ARMARX_WARNING << "Timeout or error in wait for images";
        }
        else
        {
            // get images
            int nNumberImages = getImages(cameraImages);
            ARMARX_VERBOSE << getName() << " got " << nNumberImages << " images";
            ::ImageProcessor::ConvertImage(cameraImages[0], camImgLeftGrey);
            ::ImageProcessor::Resize(camImgLeftGrey, camImgLeftGreyOldSmall);
 
            Eigen::Matrix4f handNodePose =
                armarx::PosePtr::dynamicCast(robotStateProxy->getSynchronizedRobot()
                                                 ->getRobotNode(handFrameName)
                                                 ->getPoseInRootFrame())
                    ->toEigen();
            Math3d::SetVec(
                oldHandPosSensor, handNodePose(0, 3), handNodePose(1, 3), handNodePose(2, 3));
        }
 
        timeOfLastExecution = IceUtil::Time::now();
        oldCollisionProbability = 0;
        active = true;
    }
 
    void
    VisualContactDetection::deactivate(const Ice::Current& c)
    {
        std::unique_lock lock(activationStateMutex);
        active = false;
    }
 
    void
    VisualContactDetection::SetNumberInFileName(std::string& sFileName, int nNumber, int nNumDigits)
    {
        for (int i = 0; i < nNumDigits; i++)
        {
            int nDecimalDivisor = 1;
 
            for (int j = 0; j < i; j++)
            {
                nDecimalDivisor *= 10;
            }
 
            sFileName.at(sFileName.length() - (5 + i)) = '0' + (nNumber / nDecimalDivisor) % 10;
        }
    }
} // namespace visionx