ObjectLearningByPushing.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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "ObjectLearningByPushing.h"
 
#include "FeatureCalculation.h"
#include "GaussBackground.h"
#include "HypothesisGeneration.h"
#include "HypothesisValidationRGBD.h"
#include "HypothesisVisualization.h"
#include "OLPTools.h"
#include "ObjectRecognition.h"
#include "PointCloudRegistration.h"
#include "SaliencyCalculation.h"
#include "SegmentedPointCloudFusion.h"
 
 
// IVT
#include <Calibration/Calibration.h>
#include <Calibration/StereoCalibration.h>
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
#include <Math/FloatMatrix.h>
#include <Math/LinearAlgebra.h>
 
 
// OpenMP
#include <omp.h>
 
 
// VisionX
#include <VisionX/interface/components/Calibration.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/TypeMapping.h>
 
 
// PCL
#include <sys/time.h>
 
#include <Eigen/Geometry>
 
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
 
 
using namespace visionx;
using namespace armarx;
 
void
ObjectLearningByPushing::CreateInitialObjectHypotheses(const Ice::Current& c)
{
    listener->resetHypothesesStatus();
    currentState = eCreatingInitialHypotheses;
}
 
void
ObjectLearningByPushing::ValidateInitialObjectHypotheses(const Ice::Current& c)
{
    listener->resetHypothesesStatus();
    currentState = eValidatingInitialHypotheses;
}
 
void
ObjectLearningByPushing::RevalidateConfirmedObjectHypotheses(const Ice::Current& c)
{
    listener->resetHypothesesStatus();
    currentState = eRevalidatingConfirmedHypotheses;
}
 
types::PointList
visionx::ObjectLearningByPushing::getObjectHypothesisPoints(const Ice::Current& c)
{
    types::PointList hypothesisPoints;
    std::vector<CHypothesisPoint*>* pPoints;
    SelectPreferredHypothesis(pPoints);
 
    if (pPoints)
    {
        for (size_t i = 0; i < pPoints->size(); i++)
        {
            Eigen::Vector3f p(pPoints->at(i)->vPosition.x,
                              pPoints->at(i)->vPosition.y,
                              pPoints->at(i)->vPosition.z);
            armarx::Vector3Ptr point = new armarx::Vector3(p);
            hypothesisPoints.push_back(point);
        }
    }
 
    return hypothesisPoints;
}
 
types::PointList
ObjectLearningByPushing::getScenePoints(const Ice::Current& c)
{
    types::PointList scenePoints;
    std::vector<CHypothesisPoint*>* points = m_pAllNewDepthMapPoints;
    if (m_pAllNewDepthMapPoints->size() == 0)
    {
        points = m_pAllOldDepthMapPoints;
    }
 
    for (size_t i = 0; i < points->size(); i++)
    {
        Eigen::Vector3f p(
            points->at(i)->vPosition.x, points->at(i)->vPosition.y, points->at(i)->vPosition.z);
        armarx::Vector3Ptr point = new armarx::Vector3(p);
        scenePoints.push_back(point);
    }
    return scenePoints;
}
 
Vector3BasePtr
ObjectLearningByPushing::getUpwardsVector(const Ice::Current& c)
{
    Vector3BasePtr upVector = new Vector3(upwardsVector.x, upwardsVector.y, upwardsVector.z);
    return upVector;
}
 
std::string
ObjectLearningByPushing::getReferenceFrameName(const Ice::Current& c)
{
    return cameraFrameName;
}
 
PoseBasePtr
ObjectLearningByPushing::getLastObjectTransformation(const Ice::Current& c)
{
    std::vector<CHypothesisPoint*>* pPoints;
    CObjectHypothesis* pHypothesis = SelectPreferredHypothesis(pPoints);
    PoseBasePtr result = NULL;
 
    if (pHypothesis)
    {
        Eigen::Matrix3f rot = tools::convertIVTtoEigen(pHypothesis->mLastRotation);
        Eigen::Vector3f trans = tools::convertIVTtoEigen(pHypothesis->vLastTranslation);
        result = new Pose(rot, trans);
    }
 
    return result;
}
 
void
ObjectLearningByPushing::recognizeObject(const std::string& objectName, const Ice::Current& c)
{
    UpdateDataFromRobot();
    SetHeadToPlatformTransformation(
        tHeadToPlatformTransformation.translation, tHeadToPlatformTransformation.rotation, true);
 
    m_pFeatureCalculation->GetAllFeaturePoints(colorImageLeft,
                                               greyImageLeft,
                                               pointCloudPtr,
                                               OLP_DEPTH_MAP_PIXEL_DISTANCE,
                                               *m_pAllNewSIFTPoints,
                                               *m_pAllNewMSERs,
                                               *m_pAllNewDepthMapPoints,
                                               calibration);
 
    RecognizeHypotheses(colorImageLeft, objectName);
}
 
void
ObjectLearningByPushing::onInitPointCloudAndImageProcessor()
{
    // todo: remove this again when done with the old learned objects
    //convertFileOLPtoPCL("/homes/staff/schieben/datalog/test/BunteKanne.dat");
 
    ARMARX_VERBOSE << "entering onInitPointCloudAndImageProcessor()";
 
    imageProviderName = getProperty<std::string>("ImageProviderAdapterName").getValue();
    usingImageProvider(imageProviderName);
 
    robotStateProxyName = getProperty<std::string>("RobotStateProxyName").getValue();
    usingProxy(robotStateProxyName);
 
 
    cameraFrameName = getProperty<std::string>("CameraFrameName").getValue();
 
    connected = false;
    currentState = eNoHypotheses;
 
 
    offeringTopic("ObjectLearningByPushing");
    pointcloudProviderName = getProperty<std::string>("PointCloudProviderAdapterName").getValue();
    usingPointCloudProvider(pointcloudProviderName);
 
 
    offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
}
 
void
ObjectLearningByPushing::onConnectPointCloudAndImageProcessor()
{
    ARMARX_VERBOSE << "entering onConnectPointCloudAndImageProcessor()";
 
    // connect to point cloud provider
    ARMARX_INFO << "Connecting to " << pointcloudProviderName;
    // pointcloudProviderProxy = getProxy<CapturingPointCloudAndImageAndCalibrationProviderInterfacePrx>(pointcloudProviderName);
    // calibration = tools::convert(pointcloudProviderProxy->getMonocularCalibration()); // == left stereo image
    pointcloudProviderProxy = getProxy<RGBDPointCloudProviderInterfacePrx>(pointcloudProviderName);
    calibration = tools::convert(pointcloudProviderProxy->getStereoCalibration().calibrationLeft);
 
    if (cameraFrameName != pointcloudProviderProxy->getReferenceFrame())
    {
        ARMARX_WARNING << "Specified camera frame name: '" << cameraFrameName
                       << "' does not match frame given by provider '"
                       << pointcloudProviderProxy->getReferenceFrame() << "'!";
    }
 
    // FIXME: workaround for depth camera sim frame missalignment
    if (pointcloudProviderProxy->getReferenceFrame() == "DepthCameraSim")
    {
        ARMARX_WARNING << "Simulation detected. Applying fix for DepthCameraSim missalignment.";
 
        // see http://ivt.sourceforge.net/doxygen/class_c_calibration.html#details
        Mat3d R = tools::convert(
            pointcloudProviderProxy->getStereoCalibration().calibrationLeft.cameraParam.rotation);
        // Eigen::Matrix3f worldToCamera = (Eigen::Matrix3f() << R.r1, R.r2, R.r3, R.r4, R.r5, R.r6, R.r7, R.r8, R.r9).finished()
        // Eigen::Matrix3f cameraToImg = Eigen::AngleAxisf(-M_PI / 2.0f, Eigen::Vector3f::UnitZ()).toRotationMatrix();
        Eigen::Matrix3f worldToCamera;
        worldToCamera << R.r1, R.r2, R.r3, R.r4, R.r5, R.r6, R.r7, R.r8, R.r9;
        Eigen::Matrix3f cameraToImg;
        cameraToImg << 0, -1, 0, 1, 0, 0, 0, 0, 1; // rotate -90° around z
 
        worldToCamera = cameraToImg * worldToCamera;
        R.r1 = worldToCamera(0, 0);
        R.r2 = worldToCamera(0, 1);
        R.r3 = worldToCamera(0, 2);
        R.r4 = worldToCamera(1, 0);
        R.r5 = worldToCamera(1, 1);
        R.r6 = worldToCamera(1, 2);
        R.r7 = worldToCamera(2, 0);
        R.r8 = worldToCamera(2, 1);
        R.r9 = worldToCamera(2, 2);
 
        calibration->SetRotation(R);
    }
 
    pointCloudPtr.reset(new pcl::PointCloud<pcl::PointXYZRGBA>());
 
    // connect to image provider
    ARMARX_INFO << "Connecting to " << imageProviderName;
    visionx::ImageProviderInfo imageProviderInfo = getImageProvider(imageProviderName);
    imageProviderProxy = getProxy<ImageProviderInterfacePrx>(imageProviderName);
 
    colorImageLeft = tools::createByteImage(imageProviderInfo);
    colorImageRight = tools::createByteImage(imageProviderInfo);
 
    colorImageLeftOld = new CByteImage(colorImageLeft);
    greyImageLeft =
        new CByteImage(colorImageLeft->width, colorImageLeft->height, CByteImage::eGrayScale);
    greyImageRight = new CByteImage(greyImageLeft);
    resultImageLeft = new CByteImage(colorImageLeft);
    resultImageRight = new CByteImage(colorImageLeft);
    tempResultImage = new CByteImage(colorImageLeft);
 
    cameraImages = new CByteImage*[2];
    cameraImages[0] = colorImageLeft;
    cameraImages[1] = colorImageRight;
 
    resultImages = new CByteImage*[2];
    resultImages[0] = resultImageLeft;
    resultImages[1] = resultImageRight;
 
    m_pFeatureCalculation = new CFeatureCalculation();
 
    // CHypothesisGeneration creates the initial object hypotheses
    m_pHypothesisGeneration = new CHypothesisGeneration(calibration);
 
    // visualizes the hypotheses
    m_pHypothesisVisualization = new CHypothesisVisualization(calibration);
 
 
    // screenshots, if wanted
    m_sScreenshotPath = OLP_SCREENSHOT_PATH;
    m_bMakeIntermediateScreenshots = OLP_MAKE_INTERMEDIATE_SCREENSHOTS;
 
    iterationCounter = 0;
 
 
    ARMARX_INFO << "Connecting to " << robotStateProxyName;
    robotStateProxy = getProxy<RobotStateComponentInterfacePrx>(robotStateProxyName);
 
 
    localRobot = RemoteRobot::createLocalCloneFromFile(
        robotStateProxy, VirtualRobot::RobotIO::RobotDescription::eFull);
 
 
    int nNumOMPThreads;
#if defined OMP_NUM_THREADS
    nNumOMPThreads = OMP_NUM_THREADS;
#elif defined OLP_USE_DBVISION
    nNumOMPThreads = omp_get_num_procs() - 1;
#else
    nNumOMPThreads = omp_get_num_procs();
#endif
 
    if (nNumOMPThreads < 1)
    {
        nNumOMPThreads = 1;
    }
 
    omp_set_num_threads(nNumOMPThreads);
 
    m_pObjectHypotheses = new CObjectHypothesisArray();
    m_pConfirmedHypotheses = new CObjectHypothesisArray();
    m_pInitialHypothesesAtLocalMaxima = new CObjectHypothesisArray();
    m_pAllNewSIFTPoints = new CSIFTFeatureArray();
    m_pAllOldSIFTPoints = new CSIFTFeatureArray();
    m_pAllNewMSERs = new std::vector<CMSERDescriptor3D*>();
    m_pAllOldMSERs = new std::vector<CMSERDescriptor3D*>();
    m_pCorrespondingMSERs = new std::vector<CMSERDescriptor3D*>();
    m_pAllNewDepthMapPoints = new std::vector<CHypothesisPoint*>();
    m_pAllOldDepthMapPoints = new std::vector<CHypothesisPoint*>();
 
    m_pGaussBackground = new CGaussBackground(OLP_IMG_WIDTH, OLP_IMG_HEIGHT);
 
    Math3d::SetVec(m_tHeadToPlatformTransformation.translation, Math3d::zero_vec);
    Math3d::SetVec(m_tHeadToPlatformTransformationOld.translation, Math3d::zero_vec);
    Math3d::SetMat(m_tHeadToPlatformTransformation.rotation, Math3d::unit_mat);
    Math3d::SetMat(m_tHeadToPlatformTransformationOld.rotation, Math3d::unit_mat);
 
    Math3d::SetVec(upwardsVector, Math3d::zero_vec);
 
 
    listener = getTopic<ObjectLearningByPushingListenerPrx>("ObjectLearningByPushing");
 
    enableResultImages(
        2, imageProviderInfo.imageFormat.dimension, imageProviderInfo.destinationImageType);
 
    // debug
    m_pSegmentedBackgroundImage =
        new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
    m_pDisparityImage = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
 
    connected = true;
 
    debugDrawer = getTopic<DebugDrawerInterfacePrx>(
        getProperty<std::string>("DebugDrawerTopicName").getValue());
    debugDrawer->clearLayer(getName());
 
    ARMARX_IMPORTANT << "init finished";
}
 
void
ObjectLearningByPushing::onExitPointCloudAndImageProcessor()
{
    ARMARX_VERBOSE << "entering onExitPointCloudAndImageProcessor()";
 
    delete m_pHypothesisVisualization;
    delete m_pHypothesisGeneration;
 
    for (int i = 0; i < m_pObjectHypotheses->GetSize(); i++)
    {
        delete (*m_pObjectHypotheses)[i];
    }
 
    delete m_pObjectHypotheses;
 
    for (int i = 0; i < m_pConfirmedHypotheses->GetSize(); i++)
    {
        delete (*m_pConfirmedHypotheses)[i];
    }
 
    delete m_pConfirmedHypotheses;
    delete m_pInitialHypothesesAtLocalMaxima;
 
 
    for (int i = 0; i < m_pAllNewSIFTPoints->GetSize(); i++)
    {
        delete (*m_pAllNewSIFTPoints)[i];
    }
 
    delete m_pAllNewSIFTPoints;
 
    for (int i = 0; i < m_pAllOldSIFTPoints->GetSize(); i++)
    {
        delete (*m_pAllOldSIFTPoints)[i];
    }
 
    delete m_pAllOldSIFTPoints;
 
    for (size_t i = 0; i < m_pAllNewMSERs->size(); i++)
    {
        delete m_pAllNewMSERs->at(i);
    }
 
    delete m_pAllNewMSERs;
 
    for (size_t i = 0; i < m_pAllOldMSERs->size(); i++)
    {
        delete m_pAllOldMSERs->at(i);
    }
 
    delete m_pAllOldMSERs;
 
    for (size_t i = 0; i < m_pCorrespondingMSERs->size(); i++)
    {
        delete m_pCorrespondingMSERs->at(i);
    }
 
    delete m_pCorrespondingMSERs;
 
    for (size_t i = 0; i < m_pAllNewDepthMapPoints->size(); i++)
    {
        delete m_pAllNewDepthMapPoints->at(i);
    }
 
    delete m_pAllNewDepthMapPoints;
 
    for (size_t i = 0; i < m_pAllOldDepthMapPoints->size(); i++)
    {
        delete m_pAllOldDepthMapPoints->at(i);
    }
 
    delete m_pAllOldDepthMapPoints;
 
    delete m_pGaussBackground;
}
 
void
ObjectLearningByPushing::process()
{
    if (!connected)
    {
        ARMARX_INFO << "process() called before ready (weird race condition with the "
                       "ImageProcessor::process)";
        return;
    }
 
    bool bMakeScreenshots = OLP_MAKE_RESULT_SCREENSHOTS;
 
    std::lock_guard<std::mutex> lock(processingLock);
 
    switch (currentState)
    {
        case eCreatingInitialHypotheses:
        {
            // get new camera images and camera pose from robot
            UpdateDataFromRobot();
            SetHeadToPlatformTransformation(tHeadToPlatformTransformation.translation,
                                            tHeadToPlatformTransformation.rotation,
                                            true);
 
            // create initial object hypotheses
            CreateInitialObjectHypothesesInternal(
                greyImageLeft, greyImageRight, colorImageLeft, colorImageRight, iterationCounter);
 
            // show hypotheses
            VisualizeHypotheses(greyImageLeft,
                                greyImageRight,
                                colorImageLeft,
                                colorImageRight,
                                false,
                                resultImageLeft,
                                resultImageRight,
                                bMakeScreenshots);
            provideResultImages(resultImages);
 
            // return information about object hypotheses and go to new state
            if (GetNumberOfNonconfirmedHypotheses() > 0)
            {
                ARMARX_INFO << "Found " << GetNumberOfNonconfirmedHypotheses()
                            << " possible objects";
 
                // return object position etc
                ReportObjectPositionInformationToObserver();
                listener->reportInitialObjectHypothesesCreated(true);
 
                currentState = eHasInitialHypotheses;
            }
            else
            {
                ARMARX_INFO << "No object hypothesis found";
 
                listener->reportInitialObjectHypothesesCreated(false);
 
                currentState = eNoHypotheses;
            }
 
            iterationCounter++;
 
            break;
        }
 
        case eValidatingInitialHypotheses:
        {
            // get new camera images and camera pose from robot
            ::ImageProcessor::CopyImage(colorImageLeft, colorImageLeftOld);
            UpdateDataFromRobot();
            SetHeadToPlatformTransformation(tHeadToPlatformTransformation.translation,
                                            tHeadToPlatformTransformation.rotation,
                                            true);
 
            // try to validate the hypotheses
            bool validationSuccessful = ValidateInitialObjectHypothesesInternal(
                greyImageLeft, greyImageRight, colorImageLeft, colorImageRight, iterationCounter);
 
            // show initial hypotheses plus those generated in changed image areas
            VisualizeHypotheses(greyImageLeft,
                                greyImageRight,
                                colorImageLeftOld,
                                colorImageRight,
                                false,
                                resultImageRight,
                                tempResultImage,
                                bMakeScreenshots);
            // show validated hypotheses
            VisualizeHypotheses(greyImageLeft,
                                greyImageRight,
                                colorImageLeft,
                                colorImageRight,
                                true,
                                resultImageLeft,
                                tempResultImage,
                                bMakeScreenshots);
            provideResultImages(resultImages);
 
            if (validationSuccessful)
            {
                ARMARX_INFO << "At least one object hypothesis was confirmed";
 
                // return object position etc
                ReportObjectPositionInformationToObserver();
                listener->reportObjectHypothesesValidated(true);
 
                currentState = eHasConfirmedHypotheses;
            }
            else
            {
                ARMARX_INFO << "No object hypothesis was confirmed";
 
                listener->reportObjectHypothesesValidated(false);
 
                currentState = eNoHypotheses;
            }
 
            iterationCounter++;
 
            break;
        }
 
        case eRevalidatingConfirmedHypotheses:
        {
            // get new camera images and camera pose from robot
            ::ImageProcessor::CopyImage(colorImageLeft, colorImageLeftOld);
            UpdateDataFromRobot();
            SetHeadToPlatformTransformation(tHeadToPlatformTransformation.translation,
                                            tHeadToPlatformTransformation.rotation,
                                            true);
 
            // try to revalidate the hypotheses
            bool validationSuccessful = RevalidateConfirmedObjectHypothesesInternal(
                greyImageLeft, greyImageRight, colorImageLeft, colorImageRight, iterationCounter);
 
            // show validated hypothesis
            VisualizeHypotheses(greyImageLeft,
                                greyImageRight,
                                colorImageLeft,
                                colorImageRight,
                                true,
                                resultImageLeft,
                                resultImageRight,
                                bMakeScreenshots);
            provideResultImages(resultImages);
 
            ARMARX_INFO << "The confirmed object hypotheses were revalidated";
            ARMARX_IMPORTANT << "Was revalidation successful: " << validationSuccessful;
 
            // return object position etc
            ReportObjectPositionInformationToObserver();
            listener->reportObjectHypothesesValidated(validationSuccessful);
 
            currentState = eHasConfirmedHypotheses;
 
            iterationCounter++;
 
            break;
        }
 
        case eHasInitialHypotheses:
        case eHasConfirmedHypotheses:
        {
            provideResultImages(resultImages);
            usleep(300000);
            break;
        }
 
        case eNoHypotheses:
        {
            UpdateDataFromRobot();
            ::ImageProcessor::CopyImage(cameraImages[0], resultImages[0]);
            ::ImageProcessor::CopyImage(cameraImages[0], resultImages[1]);
            provideResultImages(resultImages);
            usleep(300000);
            break;
        }
 
        case eQuit:
        {
            ARMARX_IMPORTANT << "Received quit signal, but dont know what to do!";
            break;
        }
 
        default:
        {
            ARMARX_WARNING << "The current mode is not handled here!";
            break;
        }
    }
}
 
void
ObjectLearningByPushing::SwapAllPointsArraysToOld()
{
    CSIFTFeatureArray* pTempSIFTPoints;
    pTempSIFTPoints = m_pAllOldSIFTPoints;
    m_pAllOldSIFTPoints = m_pAllNewSIFTPoints;
    m_pAllNewSIFTPoints = pTempSIFTPoints;
 
    for (int i = 0; i < m_pAllNewSIFTPoints->GetSize(); i++)
    {
        delete (*m_pAllNewSIFTPoints)[i];
    }
 
    m_pAllNewSIFTPoints->Clear();
 
    std::vector<CMSERDescriptor3D*>* pTempMSERs;
    pTempMSERs = m_pAllOldMSERs;
    m_pAllOldMSERs = m_pAllNewMSERs;
    m_pAllNewMSERs = pTempMSERs;
 
    for (size_t i = 0; i < m_pAllNewMSERs->size(); i++)
    {
        delete m_pAllNewMSERs->at(i);
    }
 
    m_pAllNewMSERs->clear();
 
    std::vector<CHypothesisPoint*>* pTempDepthMapPoints;
    pTempDepthMapPoints = m_pAllOldDepthMapPoints;
    m_pAllOldDepthMapPoints = m_pAllNewDepthMapPoints;
    m_pAllNewDepthMapPoints = pTempDepthMapPoints;
 
    for (size_t i = 0; i < m_pAllNewDepthMapPoints->size(); i++)
    {
        delete m_pAllNewDepthMapPoints->at(i);
    }
 
    m_pAllNewDepthMapPoints->clear();
}
 
void
ObjectLearningByPushing::CreateInitialObjectHypothesesInternal(CByteImage* pImageGreyLeft,
                                                               CByteImage* pImageGreyRight,
                                                               CByteImage* pImageColorLeft,
                                                               CByteImage* pImageColorRight,
                                                               int nImageNumber)
{
    // clean up
    for (int i = 0; i < m_pObjectHypotheses->GetSize(); i++)
    {
        delete (*m_pObjectHypotheses)[i];
    }
 
    m_pObjectHypotheses->Clear();
 
    for (int i = 0; i < m_pConfirmedHypotheses->GetSize(); i++)
    {
        delete (*m_pConfirmedHypotheses)[i];
    }
 
    m_pConfirmedHypotheses->Clear();
    m_pInitialHypothesesAtLocalMaxima->Clear();
 
    // get all interest points with descriptors
    //m_pFeatureCalculation->GetAllFeaturePoints(pImageColorLeft, pImageColorRight, pImageGreyLeft, pImageGreyRight, 2*OLP_DEPTH_MAP_PIXEL_DISTANCE,
    //                                           m_pStereoCalibration, *m_pAllNewSIFTPoints, *m_pAllNewMSERs, *m_pAllNewDepthMapPoints, m_pDisparityImage);
    m_pFeatureCalculation->GetAllFeaturePoints(pImageColorLeft,
                                               pImageGreyLeft,
                                               pointCloudPtr,
                                               2 * OLP_DEPTH_MAP_PIXEL_DISTANCE,
                                               *m_pAllNewSIFTPoints,
                                               *m_pAllNewMSERs,
                                               *m_pAllNewDepthMapPoints,
                                               calibration);
    //m_pFeatureCalculation->GetAllFeaturePoints(pImageColorLeft, pImageGreyLeft, pointCloudPtr, OLP_DEPTH_MAP_PIXEL_DISTANCE, *m_pAllNewSIFTPoints,
    //        *m_pAllNewMSERs, *m_pAllNewDepthMapPoints, calibration);
 
    if (m_bMakeIntermediateScreenshots)
    {
        std::string sScreenshotFile = m_sScreenshotPath + "disp0000.bmp";
        COLPTools::SetNumberInFileName(sScreenshotFile, nImageNumber);
        m_pDisparityImage->SaveToFile(sScreenshotFile.c_str());
 
        //        // TODO: test!
        //        CByteImage* pEdges = new CByteImage(m_pDisparityImage);
        //        COLPTools::CalculateEdgeImageFromImageAndDisparity(pImageGreyLeft, m_pDisparityImage, pEdges);
        //        sScreenshotFile = m_sScreenshotPath+"edges0000.bmp";
        //        COLPTools::SetNumberInFileName(sScreenshotFile, nImageNumber);
        //        pEdges->SaveToFile(sScreenshotFile.c_str());
        //        delete pEdges;
    }
 
 
    // learn background
    m_pGaussBackground->LearnBackgroundRGB(&pImageColorLeft, 1);
 
    // create hypotheses
    m_pHypothesisGeneration->FindObjectHypotheses(pImageColorLeft,
                                                  pImageColorRight,
                                                  pImageGreyLeft,
                                                  pImageGreyRight,
                                                  *m_pAllNewSIFTPoints,
                                                  *m_pAllNewMSERs,
                                                  *m_pAllNewDepthMapPoints,
                                                  *m_pObjectHypotheses);
 
 
#ifdef OLP_FILTER_INITIAL_HYPOTHESES_WITH_MAXIMUMNESS
 
    // remove hypotheses that are not local maxima
    std::vector<Vec3d> aMaxima;
    CByteImage* pMaximumnessImage =
        new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
    CSaliencyCalculation::FindLocalMaxima(*m_pAllNewDepthMapPoints,
                                          m_tHeadToPlatformTransformation.rotation,
                                          m_tHeadToPlatformTransformation.translation,
                                          calibration,
                                          aMaxima,
                                          pMaximumnessImage);
 
    if (m_bMakeIntermediateScreenshots)
    {
        std::string sScreenshotFile = m_sScreenshotPath + "max0000.bmp";
        COLPTools::SetNumberInFileName(sScreenshotFile, nImageNumber);
        pMaximumnessImage->SaveToFile(sScreenshotFile.c_str());
    }
 
    for (int n = 0; n < m_pObjectHypotheses->GetSize(); n++)
    {
        CObjectHypothesis* pHypothesis = (*m_pObjectHypotheses)[n];
        float fForegroundRatio;
        int nNumForegroundPoints;
        COLPTools::CalculateForegroundRatioOfHypothesis(
            pHypothesis, pMaximumnessImage, calibration, fForegroundRatio, nNumForegroundPoints);
 
        //ARMARX_VERBOSE << "Hypo " << n << ": " << pHypothesis->aNewPoints.size() << " p, maxness " << 100*fForegroundRatio;
        if (fForegroundRatio > 0.4f)
        {
            m_pInitialHypothesesAtLocalMaxima->AddElement(pHypothesis);
        }
    }
 
    if ((m_pInitialHypothesesAtLocalMaxima->GetSize() == 0) && (m_pObjectHypotheses->GetSize() > 0))
    {
        m_pInitialHypothesesAtLocalMaxima->AddElement((*m_pObjectHypotheses)[0]);
    }
 
    m_pHypothesisVisualization->VisualizeHypotheses(pImageColorLeft,
                                                    pImageColorRight,
                                                    *m_pInitialHypothesesAtLocalMaxima,
                                                    *m_pAllOldSIFTPoints,
                                                    *m_pAllOldMSERs,
                                                    *m_pCorrespondingMSERs,
                                                    false,
                                                    NULL,
                                                    NULL,
                                                    m_bMakeIntermediateScreenshots);
 
    delete pMaximumnessImage;
#else
 
    for (int i = 0; i < m_pObjectHypotheses->GetSize(); i++)
    {
        m_pInitialHypothesesAtLocalMaxima->AddElement((*m_pObjectHypotheses)[i]);
    }
 
#endif
 
    // copy all points to arrays for old ones
    SwapAllPointsArraysToOld();
}
 
bool
ObjectLearningByPushing::ValidateInitialObjectHypothesesInternal(CByteImage* pImageGreyLeft,
                                                                 CByteImage* pImageGreyRight,
                                                                 CByteImage* pImageColorLeft,
                                                                 CByteImage* pImageColorRight,
                                                                 int nImageNumber)
{
    //timeval tStart, tEnd;
 
    // clean up
    for (int i = 0; i < (int)m_pCorrespondingMSERs->size(); i++)
    {
        delete m_pCorrespondingMSERs->at(i);
    }
 
    m_pCorrespondingMSERs->clear();
 
    // calculate background segmentation
    //gettimeofday(&tStart, 0);
    m_pGaussBackground->GetBinaryForegroundImageRGB(pImageColorLeft, m_pSegmentedBackgroundImage);
    BoundingBoxInForegroundImage(m_pSegmentedBackgroundImage, 100, 540, 100, 480);
    m_pGaussBackground->LearnBackgroundRGB(&pImageColorLeft, 1);
 
    //gettimeofday(&tEnd, 0);
    //long tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
    //ARMARX_VERBOSE << "Time for background: " << tTimeDiff << " ms";
    if (m_bMakeIntermediateScreenshots)
    {
        std::string sScreenshotFile = m_sScreenshotPath + "bg0000.bmp";
        COLPTools::SetNumberInFileName(sScreenshotFile, nImageNumber);
        m_pSegmentedBackgroundImage->SaveToFile(sScreenshotFile.c_str());
    }
 
    // find new features in the actual image
    m_pFeatureCalculation->GetAllFeaturePoints(pImageColorLeft,
                                               pImageGreyLeft,
                                               pointCloudPtr,
                                               OLP_DEPTH_MAP_PIXEL_DISTANCE,
                                               *m_pAllNewSIFTPoints,
                                               *m_pAllNewMSERs,
                                               *m_pAllNewDepthMapPoints,
                                               calibration);
 
 
    if (m_bMakeIntermediateScreenshots)
    {
        std::string sScreenshotFile = m_sScreenshotPath + "disp0000.bmp";
        COLPTools::SetNumberInFileName(sScreenshotFile, nImageNumber);
        m_pDisparityImage->SaveToFile(sScreenshotFile.c_str());
 
        // TODO: test!
        CByteImage* pEdges = new CByteImage(m_pDisparityImage);
        COLPTools::CalculateEdgeImageFromImageAndDisparity(
            pImageGreyLeft, m_pDisparityImage, pEdges);
        sScreenshotFile = m_sScreenshotPath + "edges0000.bmp";
        COLPTools::SetNumberInFileName(sScreenshotFile, nImageNumber);
        pEdges->SaveToFile(sScreenshotFile.c_str());
        delete pEdges;
    }
 
    CByteImage* pHSVImage = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eRGB24);
    ::ImageProcessor::CalculateHSVImage(pImageColorLeft, pHSVImage);
 
 
    CHypothesisValidationRGBD::ValidateInitialHypotheses(*m_pAllOldDepthMapPoints,
                                                         *m_pAllNewDepthMapPoints,
                                                         m_pSegmentedBackgroundImage,
                                                         pHSVImage,
                                                         calibration,
                                                         upwardsVector,
                                                         m_pObjectHypotheses,
                                                         m_pConfirmedHypotheses);
 
 
    int nNumValidatedObjects = m_pConfirmedHypotheses->GetSize();
 
    // copy all points to arrays for old ones
    SwapAllPointsArraysToOld();
 
    for (int i = 0; i < m_pConfirmedHypotheses->GetSize(); i++)
    {
        ARMARX_VERBOSE << "Hypothesis " << (*m_pConfirmedHypotheses)[i]->nHypothesisNumber
                       << " has been revalidated. "
                       << (*m_pConfirmedHypotheses)[i]->aConfirmedPoints.size()
                       << " confirmed points, " << (*m_pConfirmedHypotheses)[i]->aNewPoints.size()
                       << " new candidates. Position: (" << (*m_pConfirmedHypotheses)[i]->vCenter.x
                       << ", " << (*m_pConfirmedHypotheses)[i]->vCenter.y << ", "
                       << (*m_pConfirmedHypotheses)[i]->vCenter.z << ")";
    }
 
    // sort by size
    for (int i = 1; i < m_pConfirmedHypotheses->GetSize(); i++)
    {
        for (int j = i; j > 0; j--)
        {
            if ((*m_pConfirmedHypotheses)[j - 1]->aConfirmedPoints.size() >=
                (*m_pConfirmedHypotheses)[j]->aConfirmedPoints.size())
            {
                break;
            }
 
            CObjectHypothesis* pTemp = (*m_pConfirmedHypotheses)[j];
            (*m_pConfirmedHypotheses)[j] = (*m_pConfirmedHypotheses)[j - 1];
            (*m_pConfirmedHypotheses)[j - 1] = pTemp;
        }
    }
 
    if (OLP_SAVE_CONFIRMED_OBJECT)
    {
        SaveHistogramOfConfirmedHypothesis("NewObject", nImageNumber);
    }
 
    delete pHSVImage;
 
    return (nNumValidatedObjects > 0);
}
 
bool
ObjectLearningByPushing::RevalidateConfirmedObjectHypothesesInternal(CByteImage* pImageGreyLeft,
                                                                     CByteImage* pImageGreyRight,
                                                                     CByteImage* pImageColorLeft,
                                                                     CByteImage* pImageColorRight,
                                                                     int nImageNumber)
{
    //timeval tStart, tEnd;
 
    // clean up
    for (int i = 0; i < (int)m_pCorrespondingMSERs->size(); i++)
    {
        delete m_pCorrespondingMSERs->at(i);
    }
 
    m_pCorrespondingMSERs->clear();
 
    // calculate background segmentation
    //gettimeofday(&tStart, 0);
    m_pGaussBackground->GetBinaryForegroundImageRGB(pImageColorLeft, m_pSegmentedBackgroundImage);
    BoundingBoxInForegroundImage(m_pSegmentedBackgroundImage, 100, 540, 100, 480);
    m_pGaussBackground->LearnBackgroundRGB(&pImageColorLeft, 1);
 
    //gettimeofday(&tEnd, 0);
    //long tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
    //ARMARX_VERBOSE << "Time for background: " << tTimeDiff << " ms";
    if (m_bMakeIntermediateScreenshots)
    {
        std::string sScreenshotFile = m_sScreenshotPath + "bg0000.bmp";
        COLPTools::SetNumberInFileName(sScreenshotFile, nImageNumber);
        m_pSegmentedBackgroundImage->SaveToFile(sScreenshotFile.c_str());
    }
 
 
    // find new features in the actual image
    m_pFeatureCalculation->GetAllFeaturePoints(pImageColorLeft,
                                               pImageGreyLeft,
                                               pointCloudPtr,
                                               OLP_DEPTH_MAP_PIXEL_DISTANCE,
                                               *m_pAllNewSIFTPoints,
                                               *m_pAllNewMSERs,
                                               *m_pAllNewDepthMapPoints,
                                               calibration);
 
 
    if (OLP_SAVE_COMPLETE_POINTCLOUD)
    {
        ARMARX_INFO << "Saving complete scene pointcloud";
 
        // get subset of depthmap without nan and (0,0,0) points
        std::vector<Vec3d> all3DPoints;
 
        for (size_t i = 0; i < pointCloudPtr->size(); i++)
        {
            pcl::PointXYZRGBA point = pointCloudPtr->at(i);
 
            if (!isnan(point.x))
            {
                if (point.x * point.x + point.y * point.y + point.z * point.z > 0)
                {
                    Vec3d point3d = {point.x, point.y, point.z};
                    all3DPoints.push_back(point3d);
                }
            }
        }
 
        // all 3D points
        setlocale(LC_NUMERIC, "C");
        std::string sFileName = OLP_OBJECT_LEARNING_DIR;
        sFileName.append("all3Dpoints00.dat");
        COLPTools::SetNumberInFileName(sFileName, nImageNumber, 2);
        FILE* pFile = fopen(sFileName.c_str(), "wt");
        fprintf(pFile, "%ld \n", all3DPoints.size());
        Vec3d center = {0, 0, 0};
 
        for (size_t i = 0; i < all3DPoints.size(); i++)
        {
            fprintf(pFile,
                    "%e %e %e \n",
                    all3DPoints.at(i).x,
                    all3DPoints.at(i).y,
                    all3DPoints.at(i).z);
            Math3d::AddToVec(center, all3DPoints.at(i));
        }
 
        fclose(pFile);
 
        // all 3D points with zero mean
        sFileName = OLP_OBJECT_LEARNING_DIR;
        sFileName.append("all3Dpoints-centered00.dat");
        COLPTools::SetNumberInFileName(sFileName, nImageNumber, 2);
        pFile = fopen(sFileName.c_str(), "wt");
        fprintf(pFile, "%ld \n", all3DPoints.size());
        Math3d::MulVecScalar(center, 1.0 / all3DPoints.size(), center);
 
        for (size_t i = 0; i < all3DPoints.size(); i++)
        {
            fprintf(pFile,
                    "%e %e %e \n",
                    all3DPoints.at(i).x - center.x,
                    all3DPoints.at(i).y - center.y,
                    all3DPoints.at(i).z - center.z);
        }
 
        fclose(pFile);
 
        // all used points
        setlocale(LC_NUMERIC, "C");
        sFileName = OLP_OBJECT_LEARNING_DIR;
        sFileName.append("allusedpoints00.dat");
        COLPTools::SetNumberInFileName(sFileName, nImageNumber, 2);
        pFile = fopen(sFileName.c_str(), "wt");
        fprintf(pFile, "%ld \n", all3DPoints.size());
        Math3d::SetVec(center, Math3d::zero_vec);
 
        for (size_t i = 0; i < m_pAllNewDepthMapPoints->size(); i++)
        {
            fprintf(pFile,
                    "%e %e %e \n",
                    m_pAllNewDepthMapPoints->at(i)->vPosition.x,
                    m_pAllNewDepthMapPoints->at(i)->vPosition.y,
                    m_pAllNewDepthMapPoints->at(i)->vPosition.z);
            Math3d::AddToVec(center, m_pAllNewDepthMapPoints->at(i)->vPosition);
        }
 
        fclose(pFile);
 
        // all used points with zero mean
        sFileName = OLP_OBJECT_LEARNING_DIR;
        sFileName.append("allusedpoints-centered00.dat");
        COLPTools::SetNumberInFileName(sFileName, nImageNumber, 2);
        pFile = fopen(sFileName.c_str(), "wt");
        fprintf(pFile, "%ld \n", all3DPoints.size());
        Math3d::MulVecScalar(center, 1.0 / all3DPoints.size(), center);
 
        for (size_t i = 0; i < m_pAllNewDepthMapPoints->size(); i++)
        {
            fprintf(pFile,
                    "%e %e %e \n",
                    m_pAllNewDepthMapPoints->at(i)->vPosition.x - center.x,
                    m_pAllNewDepthMapPoints->at(i)->vPosition.y - center.y,
                    m_pAllNewDepthMapPoints->at(i)->vPosition.z - center.z);
        }
 
        fclose(pFile);
 
        // camera image
        sFileName = OLP_OBJECT_LEARNING_DIR;
        sFileName.append("cameraimage00.bmp");
        COLPTools::SetNumberInFileName(sFileName, nImageNumber, 2);
        pImageColorLeft->SaveToFile(sFileName.c_str());
 
        // vector in direction of gravity (up)
        sFileName = OLP_OBJECT_LEARNING_DIR;
        sFileName.append("upwardsvector00.dat");
        COLPTools::SetNumberInFileName(sFileName, nImageNumber, 2);
        pFile = fopen(sFileName.c_str(), "wt");
        fprintf(pFile, "%e %e %e \n", upwardsVector.x, upwardsVector.y, upwardsVector.z);
        fclose(pFile);
    }
 
 
    if (m_bMakeIntermediateScreenshots)
    {
        std::string sScreenshotFile = m_sScreenshotPath + "disp0000.bmp";
        COLPTools::SetNumberInFileName(sScreenshotFile, nImageNumber);
        m_pDisparityImage->SaveToFile(sScreenshotFile.c_str());
    }
 
 
    // re-validate the confirmed hypothesis
 
    CByteImage* pHSVImage = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eRGB24);
    ::ImageProcessor::CalculateHSVImage(pImageColorLeft, pHSVImage);
 
    CHypothesisValidationRGBD::RevalidateHypotheses(*m_pAllNewDepthMapPoints,
                                                    m_pSegmentedBackgroundImage,
                                                    pHSVImage,
                                                    calibration,
                                                    upwardsVector,
                                                    m_pConfirmedHypotheses);
 
 
    // sort by size
    for (int i = 1; i < m_pConfirmedHypotheses->GetSize(); i++)
    {
        for (int j = i; j > 0; j--)
        {
            if ((*m_pConfirmedHypotheses)[j - 1]->aConfirmedPoints.size() >=
                (*m_pConfirmedHypotheses)[j]->aConfirmedPoints.size())
            {
                break;
            }
 
            CObjectHypothesis* pTemp = (*m_pConfirmedHypotheses)[j];
            (*m_pConfirmedHypotheses)[j] = (*m_pConfirmedHypotheses)[j - 1];
            (*m_pConfirmedHypotheses)[j - 1] = pTemp;
        }
    }
 
 
    // remove hypotheses that did not move the last 1 times, but keep at least one
    const int nNumTimesHypothesisMustHaveMoved = 1;
    bool nothingMoved = false;
 
    for (int i = m_pConfirmedHypotheses->GetSize() - 1;
         i >= 0 && m_pConfirmedHypotheses->GetSize() > 1;
         i--)
    {
        bool bHasMoved = false;
        const int nNumEntries = (*m_pConfirmedHypotheses)[i]->aHypothesisHasMoved.size();
 
        for (int j = nNumEntries - 1; j >= 0 && j >= nNumEntries - nNumTimesHypothesisMustHaveMoved;
             j--)
        {
            bHasMoved |= (*m_pConfirmedHypotheses)[i]->aHypothesisHasMoved.at(j);
        }
 
        if (!bHasMoved)
        {
            ARMARX_VERBOSE << "Removing hypothesis "
                           << (*m_pConfirmedHypotheses)[i]->nHypothesisNumber
                           << " because it did not move the last "
                           << nNumTimesHypothesisMustHaveMoved << " times.";
            delete (*m_pConfirmedHypotheses)[i];
 
            for (int k = i; k < m_pConfirmedHypotheses->GetSize() - 1; k++)
            {
                (*m_pConfirmedHypotheses)[k] = (*m_pConfirmedHypotheses)[k + 1];
            }
 
            m_pConfirmedHypotheses->DeleteElement(m_pConfirmedHypotheses->GetSize() - 1);
        }
    }
 
    // if none of the hypotheses moved, return false in the end
    if (m_pConfirmedHypotheses->GetSize() == 1)
    {
        bool bHasMoved = false;
        const int nNumEntries = (*m_pConfirmedHypotheses)[0]->aHypothesisHasMoved.size();
 
        for (int j = nNumEntries - 1; j >= 0 && j >= nNumEntries - nNumTimesHypothesisMustHaveMoved;
             j--)
        {
            bHasMoved |= (*m_pConfirmedHypotheses)[0]->aHypothesisHasMoved.at(j);
        }
 
        if (!bHasMoved)
        {
            nothingMoved = true;
        }
    }
 
 
    // fuse hypotheses that overlap significantly
    for (int i = 0; i < m_pConfirmedHypotheses->GetSize(); i++)
    {
        for (int j = i + 1; j < m_pConfirmedHypotheses->GetSize(); j++)
        {
            float fOverlapRatio = COLPTools::GetHypothesesIntersectionRatio(
                (*m_pConfirmedHypotheses)[i], (*m_pConfirmedHypotheses)[j], calibration);
            ARMARX_VERBOSE << "Overlap " << (*m_pConfirmedHypotheses)[i]->nHypothesisNumber << " "
                           << (*m_pConfirmedHypotheses)[j]->nHypothesisNumber << ": "
                           << fOverlapRatio;
 
            if (fOverlapRatio > 0.5)
            {
                ARMARX_VERBOSE << "Removing hypothesis "
                               << (*m_pConfirmedHypotheses)[j]->nHypothesisNumber;
                delete (*m_pConfirmedHypotheses)[j];
 
                for (int k = j; k < m_pConfirmedHypotheses->GetSize() - 1; k++)
                {
                    (*m_pConfirmedHypotheses)[k] = (*m_pConfirmedHypotheses)[k + 1];
                }
 
                m_pConfirmedHypotheses->DeleteElement(m_pConfirmedHypotheses->GetSize() - 1);
                j--;
            }
        }
    }
 
 
    // copy all points to arrays for old ones
    SwapAllPointsArraysToOld();
 
    for (int i = 0; i < m_pConfirmedHypotheses->GetSize(); i++)
    {
        ARMARX_VERBOSE << "Hypothesis " << (*m_pConfirmedHypotheses)[i]->nHypothesisNumber
                       << " has been revalidated. "
                       << (*m_pConfirmedHypotheses)[i]->aConfirmedPoints.size()
                       << " confirmed points, " << (*m_pConfirmedHypotheses)[i]->aNewPoints.size()
                       << " new candidates. Position: (" << (*m_pConfirmedHypotheses)[i]->vCenter.x
                       << ", " << (*m_pConfirmedHypotheses)[i]->vCenter.y << ", "
                       << (*m_pConfirmedHypotheses)[i]->vCenter.z << ")";
    }
 
    if (OLP_SAVE_CONFIRMED_OBJECT)
    {
        SaveHistogramOfConfirmedHypothesis("NewObject", nImageNumber);
    }
 
    delete pHSVImage;
 
    // return false if none of the hypotheses moved (or the movement estimation failed)
    return !nothingMoved;
}
 
void
ObjectLearningByPushing::UpdateDataFromRobot()
{
    if (!waitForPointClouds(pointcloudProviderName) || !waitForImages(imageProviderName))
    {
        ARMARX_WARNING << "Timeout or error while waiting for point cloud data";
    }
    else
    {
        if (getPointClouds(pointcloudProviderName, pointCloudPtr))
        {
            // get images
            Ice::Long timestamp;
            if (pointCloudPtr->isOrganized())
            {
                size_t h = cameraImages[0]->height; // pointCloudPtr->height;
                size_t w = cameraImages[0]->width; // pointCloudPtr->width;
 
                for (size_t y = 0; y < h; y++)
                {
                    for (size_t x = 0; x < w; x++)
                    {
                        const pcl::PointXYZRGBA& p = pointCloudPtr->at(x, y);
                        cameraImages[0]->pixels[3 * (y * w + x) + 0] = p.r;
                        cameraImages[0]->pixels[3 * (y * w + x) + 1] = p.g;
                        cameraImages[0]->pixels[3 * (y * w + x) + 2] = p.b;
                    }
                }
 
                timestamp = pointCloudPtr->header.stamp;
            }
            else
            {
                armarx::MetaInfoSizeBasePtr metaInfo;
                int nNumberImages = getImages(imageProviderName, cameraImages, metaInfo);
                ARMARX_DEBUG << "got " << nNumberImages << " images";
 
                timestamp = metaInfo->timeProvided;
            }
 
            RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateProxy, timestamp);
 
            ::ImageProcessor::ConvertImage(colorImageLeft, greyImageLeft);
            //::ImageProcessor::ConvertImage(colorImageRight, greyImageRight);
 
            Eigen::Matrix4f cameraNodePose =
                localRobot->getRobotNode(cameraFrameName)->getPoseInRootFrame();
            tHeadToPlatformTransformation.rotation.r1 = cameraNodePose(0, 0);
            tHeadToPlatformTransformation.rotation.r2 = cameraNodePose(0, 1);
            tHeadToPlatformTransformation.rotation.r3 = cameraNodePose(0, 2);
            tHeadToPlatformTransformation.rotation.r4 = cameraNodePose(1, 0);
            tHeadToPlatformTransformation.rotation.r5 = cameraNodePose(1, 1);
            tHeadToPlatformTransformation.rotation.r6 = cameraNodePose(1, 2);
            tHeadToPlatformTransformation.rotation.r7 = cameraNodePose(2, 0);
            tHeadToPlatformTransformation.rotation.r8 = cameraNodePose(2, 1);
            tHeadToPlatformTransformation.rotation.r9 = cameraNodePose(2, 2);
            tHeadToPlatformTransformation.translation.x = cameraNodePose(0, 3);
            tHeadToPlatformTransformation.translation.y = cameraNodePose(1, 3);
            tHeadToPlatformTransformation.translation.z = cameraNodePose(2, 3);
 
            Eigen::Vector3f upInRootCS = {0, 0, 1};
            Eigen::Vector3f upInCamCS = cameraNodePose.block<3, 3>(0, 0).inverse() * upInRootCS;
            Math3d::SetVec(upwardsVector, upInCamCS(0), upInCamCS(1), upInCamCS(2));
        }
    }
}
 
bool
ObjectLearningByPushing::SaveHistogramOfConfirmedHypothesis(std::string sObjectName,
                                                            int nDescriptorNumber)
{
    if (m_pConfirmedHypotheses->GetSize() > 0)
    {
        ARMARX_INFO << "Saving object descriptor";
        CObjectRecognition::SaveObjectDescriptorRGBD(
            (*m_pConfirmedHypotheses)[0], sObjectName, nDescriptorNumber);
        CObjectRecognition::SaveObjectDescriptorPCD(
            (*m_pConfirmedHypotheses)[0], sObjectName, nDescriptorNumber);
        return true;
    }
    else
    {
        ARMARX_VERBOSE << "No confirmed hypothesis that can be saved!";
        return false;
    }
}
 
void
ObjectLearningByPushing::RecognizeHypotheses(CByteImage* pImageColorLeft,
                                             const std::string objectName)
{
    CByteImage* pHSVImage =
        new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eRGB24, false);
    ::ImageProcessor::CalculateHSVImage(pImageColorLeft, pHSVImage);
 
    if (!objectName.empty())
    {
        CObjectRecognition::FindObjectWithManyDescriptorsRGBD(pHSVImage,
                                                              pImageColorLeft,
                                                              *m_pAllNewDepthMapPoints,
                                                              calibration,
                                                              upwardsVector,
                                                              objectName,
                                                              5);
    }
    else if (m_pConfirmedHypotheses->GetSize() != 0 && false)
    {
        Vec3d vPosition;
        Mat3d mOrientation;
        float fdistance, fProbability;
        CObjectRecognition::FindObjectRGBD((*m_pConfirmedHypotheses)[0],
                                           pHSVImage,
                                           *m_pAllNewDepthMapPoints,
                                           calibration,
                                           upwardsVector,
                                           vPosition,
                                           mOrientation,
                                           fdistance,
                                           fProbability);
    }
    else if (false)
    {
        std::vector<std::string> aNames;
        std::vector<Vec3d> aPositions;
        std::vector<Mat3d> aOrientations;
        std::vector<float> aProbabilities;
        CObjectRecognition::FindAllObjectsRGBD(pHSVImage,
                                               pImageColorLeft,
                                               *m_pAllNewDepthMapPoints,
                                               calibration,
                                               upwardsVector,
                                               aNames,
                                               aPositions,
                                               aOrientations,
                                               aProbabilities);
        //CObjectRecognition::FindAllObjectsRFCH(pImageColorLeft, calibration, *m_pAllOldDepthMapPoints, aNames, aPositions, aOrientations, aProbabilities);
    }
    else if (false)
    {
        //CObjectHypothesis* pHypothesis;
        //Vec3d vPosition;
        //Mat3d mOrientation;
        //float fdistance, fProbability;
        //CObjectRecognition::LoadObjectDescriptorRGBD("BlueBowl", 3, pHypothesis);
        //CObjectRecognition::FindObject(pHypothesis, pHSVImage, pImageColorLeft, *m_pAllOldDepthMapPoints, calibration, vPosition, mOrientation, fdistance, fProbability);
    }
 
 
    delete pHSVImage;
}
 
void
ObjectLearningByPushing::VisualizeHypotheses(CByteImage* pImageGreyLeft,
                                             CByteImage* pImageGreyRight,
                                             CByteImage* pImageColorLeft,
                                             CByteImage* pImageColorRight,
                                             bool bShowConfirmedHypotheses,
                                             CByteImage* resultImageLeft,
                                             CByteImage* resultImageRight,
                                             bool bMakeScreenshot)
{
    if (bShowConfirmedHypotheses)
    {
        m_pHypothesisVisualization->VisualizeHypotheses(pImageColorLeft,
                                                        pImageColorRight,
                                                        *m_pConfirmedHypotheses,
                                                        *m_pAllOldSIFTPoints,
                                                        *m_pAllOldMSERs,
                                                        *m_pCorrespondingMSERs,
                                                        true,
                                                        resultImageLeft,
                                                        resultImageRight,
                                                        bMakeScreenshot);
    }
    else
    {
        m_pHypothesisVisualization->VisualizeHypotheses(pImageColorLeft,
                                                        pImageColorRight,
                                                        *m_pObjectHypotheses,
                                                        *m_pAllOldSIFTPoints,
                                                        *m_pAllOldMSERs,
                                                        *m_pCorrespondingMSERs,
                                                        false,
                                                        resultImageLeft,
                                                        resultImageRight,
                                                        bMakeScreenshot);
    }
}
 
Vec3d
ObjectLearningByPushing::GetObjectPosition(float& fObjectExtent, bool bPreferCentralObject)
{
    Vec3d vRet = {0, 0, 0};
    fObjectExtent = 0;
 
    std::vector<CHypothesisPoint*>* pPoints;
    CObjectHypothesis* pHypothesis = SelectPreferredHypothesis(pPoints, bPreferCentralObject);
 
    if (pHypothesis)
    {
        vRet.x = pHypothesis->vCenter.x;
        vRet.y = pHypothesis->vCenter.y;
        vRet.z = pHypothesis->vCenter.z;
        fObjectExtent = pHypothesis->fMaxExtent;
    }
 
    return vRet;
}
 
void
ObjectLearningByPushing::ApplyHeadMotionTransformation(Mat3d mRotation, Vec3d vTranslation)
{
    if (m_pConfirmedHypotheses->GetSize() > 0)
    {
        for (size_t i = 0; i < (*m_pConfirmedHypotheses)[0]->aNewPoints.size(); i++)
        {
            Math3d::MulMatVec(mRotation,
                              (*m_pConfirmedHypotheses)[0]->aNewPoints.at(i)->vPosition,
                              vTranslation,
                              (*m_pConfirmedHypotheses)[0]->aNewPoints.at(i)->vPosition);
        }
 
        for (size_t i = 0; i < (*m_pConfirmedHypotheses)[0]->aConfirmedPoints.size(); i++)
        {
            Math3d::MulMatVec(mRotation,
                              (*m_pConfirmedHypotheses)[0]->aConfirmedPoints.at(i)->vPosition,
                              vTranslation,
                              (*m_pConfirmedHypotheses)[0]->aConfirmedPoints.at(i)->vPosition);
        }
 
        for (size_t i = 0; i < (*m_pConfirmedHypotheses)[0]->aDoubtablePoints.size(); i++)
        {
            Math3d::MulMatVec(mRotation,
                              (*m_pConfirmedHypotheses)[0]->aDoubtablePoints.at(i)->vPosition,
                              vTranslation,
                              (*m_pConfirmedHypotheses)[0]->aDoubtablePoints.at(i)->vPosition);
        }
    }
}
 
void
ObjectLearningByPushing::GetHypothesisBoundingBox(int& nMinX, int& nMaxX, int& nMinY, int& nMaxY)
{
    std::vector<CHypothesisPoint*>* pPoints;
    SelectPreferredHypothesis(pPoints);
 
    nMinX = 1000;
    nMaxX = -1000;
    nMinY = 1000;
    nMaxY = -1000;
    Vec2d vPoint2D;
 
    if (m_pConfirmedHypotheses->GetSize() > 0)
    {
        for (size_t i = 0; i < pPoints->size(); i++)
        {
            calibration->CameraToImageCoordinates(pPoints->at(i)->vPosition, vPoint2D, false);
 
            if (vPoint2D.x < nMinX)
            {
                nMinX = vPoint2D.x;
            }
 
            if (vPoint2D.x > nMaxX)
            {
                nMaxX = vPoint2D.x;
            }
 
            if (vPoint2D.y < nMinY)
            {
                nMinY = vPoint2D.y;
            }
 
            if (vPoint2D.y > nMaxY)
            {
                nMaxY = vPoint2D.y;
            }
        }
    }
 
    ARMARX_VERBOSE << "ObjectLearningByPushing::GetHypothesisBoundingBox: " << nMinX << " " << nMaxX
                   << " " << nMinY << " " << nMaxY;
}
 
void
ObjectLearningByPushing::GetHypothesisPrincipalAxesAndBoundingBox(Vec3d& vPrincipalAxis1,
                                                                  Vec3d& vPrincipalAxis2,
                                                                  Vec3d& vPrincipalAxis3,
                                                                  Vec3d& vEigenValues,
                                                                  Vec3d& vMaxExtentFromCenter,
                                                                  Vec2d& vBoundingBoxLU,
                                                                  Vec2d& vBoundingBoxRU,
                                                                  Vec2d& vBoundingBoxLL,
                                                                  Vec2d& vBoundingBoxRL)
{
    std::vector<CHypothesisPoint*>* pPoints;
    SelectPreferredHypothesis(pPoints);
 
    const int nNumberOfSamples = pPoints->size();
 
    if (nNumberOfSamples < 3)
    {
        ARMARX_WARNING << "Hypothesis contains " << nNumberOfSamples << " points - aborting!";
        return;
    }
 
    // compute the principal axes with PCA
 
    Vec3d vAvgPoint = {0.0f, 0.0f, 0.0f};
 
    for (int i = 0; i < nNumberOfSamples; i++)
    {
        vAvgPoint.x += pPoints->at(i)->vPosition.x;
        vAvgPoint.y += pPoints->at(i)->vPosition.y;
        vAvgPoint.z += pPoints->at(i)->vPosition.z;
    }
 
    vAvgPoint.x /= (float)nNumberOfSamples;
    vAvgPoint.y /= (float)nNumberOfSamples;
    vAvgPoint.z /= (float)nNumberOfSamples;
    //ARMARX_VERBOSE << "vAvgPoint: (" << vAvgPoint.x << ", " << vAvgPoint.y << ", " << vAvgPoint.z << ")";
 
    CFloatMatrix mSamplePoints(3, nNumberOfSamples);
 
    for (int i = 0; i < nNumberOfSamples; i++)
    {
        mSamplePoints(0, i) = pPoints->at(i)->vPosition.x - vAvgPoint.x;
        mSamplePoints(1, i) = pPoints->at(i)->vPosition.y - vAvgPoint.y;
        mSamplePoints(2, i) = pPoints->at(i)->vPosition.z - vAvgPoint.z;
    }
 
    CFloatMatrix mEigenVectors(3, 3);
    CFloatMatrix mEigenValues(1, 3);
 
    LinearAlgebra::PCA(&mSamplePoints, &mEigenVectors, &mEigenValues);
 
    vPrincipalAxis1.x = mEigenVectors(0, 0);
    vPrincipalAxis1.y = mEigenVectors(1, 0);
    vPrincipalAxis1.z = mEigenVectors(2, 0);
 
    vPrincipalAxis2.x = mEigenVectors(0, 1);
    vPrincipalAxis2.y = mEigenVectors(1, 1);
    vPrincipalAxis2.z = mEigenVectors(2, 1);
 
    vPrincipalAxis3.x = mEigenVectors(0, 2);
    vPrincipalAxis3.y = mEigenVectors(1, 2);
    vPrincipalAxis3.z = mEigenVectors(2, 2);
 
    vEigenValues.x = mEigenValues(0, 0);
    vEigenValues.y = mEigenValues(0, 1);
    vEigenValues.z = mEigenValues(0, 2);
    //ARMARX_VERBOSE << "Eigenvalues: (" << pow(vEigenValues.x, 0.3333f) << ", " << pow(vEigenValues.y, 0.3333f) << ", " << pow(vEigenValues.z, 0.3333f) << ")";
 
    // matrix that transforms the 3D points into the coordinate system
    // given by the eigenvectors
    Mat3d mTransformation;
    mTransformation.r1 = mEigenVectors(0, 0);
    mTransformation.r2 = mEigenVectors(1, 0);
    mTransformation.r3 = mEigenVectors(2, 0);
    mTransformation.r4 = mEigenVectors(0, 1);
    mTransformation.r5 = mEigenVectors(1, 1);
    mTransformation.r6 = mEigenVectors(2, 1);
    mTransformation.r7 = mEigenVectors(0, 2);
    mTransformation.r8 = mEigenVectors(1, 2);
    mTransformation.r9 = mEigenVectors(2, 2);
 
    // find the maximal extent along the principal axes
    Vec3d vTransformedPoint;
    float fMaxExtentX = 0, fMaxExtentY = 0, fMaxExtentZ = 0;
 
    for (int i = 0; i < nNumberOfSamples; i++)
    {
        Vec3d vTempPoint = {mSamplePoints(0, i),
                            mSamplePoints(1, i),
                            mSamplePoints(2, i)}; // = original point minus average point
        Math3d::MulMatVec(mTransformation, vTempPoint, vTransformedPoint);
 
        if (fabsf(vTransformedPoint.x) > fMaxExtentX)
        {
            fMaxExtentX = fabsf(vTransformedPoint.x);
        }
 
        if (fabsf(vTransformedPoint.y) > fMaxExtentY)
        {
            fMaxExtentY = fabsf(vTransformedPoint.y);
        }
 
        if (fabsf(vTransformedPoint.z) > fMaxExtentZ)
        {
            fMaxExtentZ = fabsf(vTransformedPoint.z);
        }
    }
 
    vMaxExtentFromCenter.x = fMaxExtentX;
    vMaxExtentFromCenter.y = fMaxExtentY;
    vMaxExtentFromCenter.z = fMaxExtentZ;
    //ARMARX_VERBOSE << "vMaxExtentFromCenter: (" << vMaxExtentFromCenter.x << ", " << vMaxExtentFromCenter.y << ", " << vMaxExtentFromCenter.z << ")";
 
 
    // calculate the corners of the surface defined by the first two principal axes
    Vec3d vTranslationPrincipalAxis1, vTranslationPrincipalAxis2;
    Math3d::MulVecScalar(vPrincipalAxis1,
                         0.5f * (2.5f * pow(vEigenValues.x, 0.3333f) + vMaxExtentFromCenter.x),
                         vTranslationPrincipalAxis1);
    Math3d::MulVecScalar(vPrincipalAxis2,
                         0.5f * (2.5f * pow(vEigenValues.y, 0.3333f) + vMaxExtentFromCenter.y),
                         vTranslationPrincipalAxis2);
 
    Vec3d vCorner1, vCorner2, vCorner3, vCorner4;
 
    Math3d::SubtractVecVec(vAvgPoint, vTranslationPrincipalAxis1, vCorner1);
    Math3d::SubtractFromVec(vCorner1, vTranslationPrincipalAxis2);
 
    Math3d::SubtractVecVec(vAvgPoint, vTranslationPrincipalAxis1, vCorner2);
    Math3d::AddToVec(vCorner2, vTranslationPrincipalAxis2);
 
    Math3d::AddVecVec(vAvgPoint, vTranslationPrincipalAxis1, vCorner3);
    Math3d::SubtractFromVec(vCorner3, vTranslationPrincipalAxis2);
 
    Math3d::AddVecVec(vAvgPoint, vTranslationPrincipalAxis1, vCorner4);
    Math3d::AddToVec(vCorner4, vTranslationPrincipalAxis2);
 
    // project the corners into the image
    calibration->CameraToImageCoordinates(vCorner1, vBoundingBoxLU, false);
    calibration->CameraToImageCoordinates(vCorner2, vBoundingBoxRU, false);
    calibration->CameraToImageCoordinates(vCorner3, vBoundingBoxLL, false);
    calibration->CameraToImageCoordinates(vCorner4, vBoundingBoxRL, false);
 
    // sort the projected points: first assert that the two upper are above the two lower,
    // then check left and right for each pair
    Vec2d vTemp;
 
    if (vBoundingBoxLU.y > vBoundingBoxLL.y)
    {
        Math2d::SetVec(vTemp, vBoundingBoxLU);
        Math2d::SetVec(vBoundingBoxLU, vBoundingBoxLL);
        Math2d::SetVec(vBoundingBoxLL, vTemp);
    }
 
    if (vBoundingBoxLU.y > vBoundingBoxRL.y)
    {
        Math2d::SetVec(vTemp, vBoundingBoxLU);
        Math2d::SetVec(vBoundingBoxLU, vBoundingBoxRL);
        Math2d::SetVec(vBoundingBoxRL, vTemp);
    }
 
    if (vBoundingBoxRU.y > vBoundingBoxLL.y)
    {
        Math2d::SetVec(vTemp, vBoundingBoxRU);
        Math2d::SetVec(vBoundingBoxRU, vBoundingBoxLL);
        Math2d::SetVec(vBoundingBoxLL, vTemp);
    }
 
    if (vBoundingBoxRU.y > vBoundingBoxRL.y)
    {
        Math2d::SetVec(vTemp, vBoundingBoxRU);
        Math2d::SetVec(vBoundingBoxRU, vBoundingBoxRL);
        Math2d::SetVec(vBoundingBoxRL, vTemp);
    }
 
    if (vBoundingBoxLU.x > vBoundingBoxRU.x)
    {
        Math2d::SetVec(vTemp, vBoundingBoxLU);
        Math2d::SetVec(vBoundingBoxLU, vBoundingBoxRU);
        Math2d::SetVec(vBoundingBoxRU, vTemp);
    }
 
    if (vBoundingBoxLL.x > vBoundingBoxRL.x)
    {
        Math2d::SetVec(vTemp, vBoundingBoxLL);
        Math2d::SetVec(vBoundingBoxLL, vBoundingBoxRL);
        Math2d::SetVec(vBoundingBoxRL, vTemp);
    }
 
    //ARMARX_VERBOSE << "GetHypothesisPrincipalAxesAndBoundingBox(): BB: (" << vBoundingBoxLU.x << ", " << vBoundingBoxLU.y << ")  (" << vBoundingBoxRU.x << ", "
    //               << vBoundingBoxRU.y << ")  (" << vBoundingBoxLL.x << ", " << vBoundingBoxLL.y << ")  (" << vBoundingBoxRL.x << ", " << vBoundingBoxRL.y << ")";
}
 
void
ObjectLearningByPushing::ReportObjectPositionInformationToObserver()
{
    Vec3d vHypothesisPosition, vPrincipalAxis1, vPrincipalAxis2, vPrincipalAxis3, vEigenValues,
        vMaxExtents;
    Vec2d vBoundingBoxLeftUpper, vBoundingBoxRightUpper, vBoundingBoxLeftLower,
        vBoundingBoxRightLower;
    float fHypothesisExtent;
    vHypothesisPosition = GetObjectPosition(fHypothesisExtent);
    GetHypothesisPrincipalAxesAndBoundingBox(vPrincipalAxis1,
                                             vPrincipalAxis2,
                                             vPrincipalAxis3,
                                             vEigenValues,
                                             vMaxExtents,
                                             vBoundingBoxLeftUpper,
                                             vBoundingBoxRightUpper,
                                             vBoundingBoxLeftLower,
                                             vBoundingBoxRightLower);
 
    Eigen::Vector3f vec(vHypothesisPosition.x, vHypothesisPosition.y, vHypothesisPosition.z);
    armarx::FramedPositionPtr hypothesisPosition =
        new armarx::FramedPosition(vec, cameraFrameName, localRobot->getName());
    armarx::Vector3Ptr principalAxis1, principalAxis2, principalAxis3, eigenvalues;
    principalAxis1 = new armarx::Vector3();
    principalAxis1->x = vPrincipalAxis1.x;
    principalAxis1->y = vPrincipalAxis1.y;
    principalAxis1->z = vPrincipalAxis1.z;
    principalAxis2 = new armarx::Vector3();
    principalAxis2->x = vPrincipalAxis2.x;
    principalAxis2->y = vPrincipalAxis2.y;
    principalAxis2->z = vPrincipalAxis2.z;
    principalAxis3 = new armarx::Vector3();
    principalAxis3->x = vPrincipalAxis3.x;
    principalAxis3->y = vPrincipalAxis3.y;
    principalAxis3->z = vPrincipalAxis3.z;
    eigenvalues = new armarx::Vector3();
    eigenvalues->x = vEigenValues.x;
    eigenvalues->y = vEigenValues.y;
    eigenvalues->z = vEigenValues.z;
 
 
    FramedPositionPtr globalObjPose = hypothesisPosition->toGlobal(localRobot);
    debugDrawer->setSphereVisu(getName(), "GlobalObj", globalObjPose, DrawColor{0, 0, 1, 1}, 50.0);
 
    listener->reportObjectHypothesisPosition(hypothesisPosition,
                                             fHypothesisExtent,
                                             principalAxis1,
                                             principalAxis2,
                                             principalAxis3,
                                             eigenvalues);
}
 
void
ObjectLearningByPushing::SetHeadToPlatformTransformation(Vec3d vTranslation,
                                                         Mat3d mRotation,
                                                         bool bResetOldTransformation)
{
    if (bResetOldTransformation)
    {
        Math3d::SetVec(m_tHeadToPlatformTransformationOld.translation, vTranslation);
        Math3d::SetMat(m_tHeadToPlatformTransformationOld.rotation, mRotation);
    }
    else
    {
        Math3d::SetVec(m_tHeadToPlatformTransformationOld.translation,
                       m_tHeadToPlatformTransformation.translation);
        Math3d::SetMat(m_tHeadToPlatformTransformationOld.rotation,
                       m_tHeadToPlatformTransformation.rotation);
    }
 
    Math3d::SetVec(m_tHeadToPlatformTransformation.translation, vTranslation);
    Math3d::SetMat(m_tHeadToPlatformTransformation.rotation, mRotation);
}
 
void
ObjectLearningByPushing::LoadAndFuseObjectSegmentations(std::string sObjectName)
{
 
    const int nFileIndexStart = 1;
 
    for (int nMaxFileIndex = 2; nMaxFileIndex <= 51; nMaxFileIndex++)
    {
        std::vector<CObjectHypothesis*> aObjectHypotheses;
        CObjectHypothesis* pHypothesis;
 
        if (!CObjectRecognition::LoadObjectDescriptorRGBD(sObjectName, nMaxFileIndex, pHypothesis))
        {
            return;
        }
 
        delete pHypothesis;
 
        int nFileIndex = nFileIndexStart;
 
        while (CObjectRecognition::LoadObjectDescriptorRGBD(sObjectName, nFileIndex, pHypothesis) &&
               nFileIndex <= nMaxFileIndex)
        {
            aObjectHypotheses.push_back(pHypothesis);
            nFileIndex++;
        }
 
        pHypothesis = NULL;
        CSegmentedPointCloudFusion::FusePointClouds(aObjectHypotheses, pHypothesis);
 
        CObjectRecognition::SaveObjectDescriptorRGBD(
            pHypothesis, sObjectName + "-fused", aObjectHypotheses.size());
        CObjectRecognition::SaveObjectDescriptorPCD(
            pHypothesis, sObjectName + "-fused", aObjectHypotheses.size());
 
        for (size_t i = 0; i < pHypothesis->aVisibleConfirmedPoints.size(); i++)
        {
            pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition.z += 250;
        }
 
        CByteImage* pImg = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eRGB24);
        COLPTools::DrawHypothesis(pHypothesis, calibration, pImg, aObjectHypotheses.size());
        std::string sFileName = OLP_OBJECT_LEARNING_DIR + sObjectName + "-fused00.bmp";
        COLPTools::SetNumberInFileName(sFileName, nMaxFileIndex, 2);
        pImg->SaveToFile(sFileName.c_str());
        delete pImg;
 
        for (size_t i = 0; i < aObjectHypotheses.size(); i++)
        {
            delete aObjectHypotheses.at(i);
        }
 
        delete pHypothesis;
    }
}
 
CObjectHypothesis*
ObjectLearningByPushing::SelectPreferredHypothesis(std::vector<CHypothesisPoint*>*& pPoints,
                                                   const bool bPreferCentralObject)
{
    CObjectHypothesis* pHypothesis = NULL;
    pPoints = NULL;
 
    CObjectHypothesisArray* hypotheses = NULL;
 
    if (m_pConfirmedHypotheses->GetSize() > 0)
    {
        hypotheses = m_pConfirmedHypotheses;
    }
    else if (m_pInitialHypothesesAtLocalMaxima->GetSize() > 0)
    {
        hypotheses = m_pInitialHypothesesAtLocalMaxima;
    }
 
    if (hypotheses)
    {
        if (bPreferCentralObject)
        {
            // get central point in camera coordinates
            Vec3d vCenterPlatform = {OLP_CENTRAL_POSITION_FOR_PUSHING_X,
                                     OLP_CENTRAL_POSITION_FOR_PUSHING_Y,
                                     OLP_CENTRAL_POSITION_FOR_PUSHING_Z};
            Mat3d mRotInv;
            Vec3d vCenter, vTemp;
            Math3d::SubtractVecVec(
                vCenterPlatform, m_tHeadToPlatformTransformation.translation, vTemp);
            Math3d::Transpose(m_tHeadToPlatformTransformation.rotation, mRotInv);
            Math3d::MulMatVec(mRotInv, vTemp, vCenter);
            ARMARX_VERBOSE << "Central point in platform cs: " << vCenterPlatform.x << ", "
                           << vCenterPlatform.y << ", " << vCenterPlatform.z;
            ARMARX_VERBOSE << "m_tHeadToPlatformTransformation.translation: "
                           << m_tHeadToPlatformTransformation.translation.x << ", "
                           << m_tHeadToPlatformTransformation.translation.y << ", "
                           << m_tHeadToPlatformTransformation.translation.z;
            ARMARX_VERBOSE << "m_tHeadToPlatformTransformation.rotation:  "
                           << m_tHeadToPlatformTransformation.rotation.r1 << ", "
                           << m_tHeadToPlatformTransformation.rotation.r2 << ", "
                           << m_tHeadToPlatformTransformation.rotation.r3 << ",   "
                           << m_tHeadToPlatformTransformation.rotation.r4 << ", "
                           << m_tHeadToPlatformTransformation.rotation.r5 << ", "
                           << m_tHeadToPlatformTransformation.rotation.r6 << ",   "
                           << m_tHeadToPlatformTransformation.rotation.r7 << ", "
                           << m_tHeadToPlatformTransformation.rotation.r8 << ", "
                           << m_tHeadToPlatformTransformation.rotation.r9;
            ARMARX_VERBOSE << "Central point in camera cs: " << vCenter.x << ", " << vCenter.y
                           << ", " << vCenter.z;
            //Math3d::SetVec(vCenter, 0, 0, 500);
            //ARMARX_IMPORTANT << "Manually set central point in camera coordinates: " << vCenter.x << ", " << vCenter.y << ", " << vCenter.z;
 
 
            int nBestIndex = 0;
            float fMinDist = Math3d::Distance((*hypotheses)[0]->vCenter, vCenter);
 
            for (int i = 1; i < hypotheses->GetSize(); i++)
            {
                if (Math3d::Distance((*hypotheses)[i]->vCenter, vCenter) < fMinDist)
                {
                    // only accept hypotheses that contain at least half as many points as the biggest one
                    //if (2*(*m_pInitialHypothesesAtLocalMaxima)[i]->aNewPoints.size() > (*m_pInitialHypothesesAtLocalMaxima)[0]->aNewPoints.size())
                    {
                        fMinDist = Math3d::Distance((*hypotheses)[i]->vCenter, vCenter);
                        nBestIndex = i;
                    }
                }
            }
 
            pHypothesis = (*hypotheses)[nBestIndex];
        }
        else
        {
            pHypothesis = (*hypotheses)[0];
        }
    }
 
    if (pHypothesis)
    {
        if (pHypothesis->aConfirmedPoints.size() > 0)
        {
            pPoints = &(pHypothesis->aConfirmedPoints);
        }
        else
        {
            pPoints = &(pHypothesis->aNewPoints);
        }
 
        ARMARX_VERBOSE << "Hypothesis " << pHypothesis->nHypothesisNumber << " ("
                       << pHypothesis->vCenter.x << ", " << pHypothesis->vCenter.y << ", "
                       << pHypothesis->vCenter.z << ") is chosen for pushing";
    }
    else
    {
        ARMARX_WARNING << "No hypothesis available";
    }
 
    return pHypothesis;
}
 
void
ObjectLearningByPushing::convertFileOLPtoPCL(std::string filename, bool includeCandidatePoints)
{
    setlocale(LC_NUMERIC, "C");
 
    FILE* filePtr = fopen(filename.c_str(), "rt");
 
    if (filePtr == NULL)
    {
        ARMARX_WARNING << "Cannot open object file: " << filename;
        return;
    }
 
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr objectPointCloud(
        new pcl::PointCloud<pcl::PointXYZRGBA>());
 
    // read the confirmed points
    size_t nNumPoints;
    float r, g, b, intensity;
    pcl::PointXYZRGBA point;
    int dummy = fscanf(filePtr, "%lu", &nNumPoints);
 
    for (size_t i = 0; i < nNumPoints; i++)
    {
        dummy += fscanf(filePtr, "%e %e %e", &point.x, &point.y, &point.z);
        dummy += fscanf(filePtr, "%e %e %e %e \n", &r, &g, &b, &intensity);
        point.r = r * 255 * intensity;
        point.g = g * 255 * intensity;
        point.b = b * 255 * intensity;
        objectPointCloud->push_back(point);
    }
 
    if (includeCandidatePoints)
    {
        // read the candidate points
        dummy += fscanf(filePtr, "%lu", &nNumPoints);
 
        for (size_t i = 0; i < nNumPoints; i++)
        {
            dummy += fscanf(filePtr, "%e %e %e", &point.x, &point.y, &point.z);
            dummy += fscanf(filePtr, "%e %e %e %e \n", &r, &g, &b, &intensity);
            point.r = r * 255 * intensity;
            point.g = g * 255 * intensity;
            point.b = b * 255 * intensity;
            objectPointCloud->push_back(point);
        }
    }
 
    int n = (int)filename.length();
    filename.at(n - 3) = 'p';
    filename.at(n - 2) = 'c';
    filename.at(n - 1) = 'd';
    pcl::io::savePCDFile(filename, *objectPointCloud);
}
 
void
ObjectLearningByPushing::BoundingBoxInForegroundImage(CByteImage* image,
                                                      int minX,
                                                      int maxX,
                                                      int minY,
                                                      int maxY)
{
    for (int i = 0; i < image->height; i++)
    {
        if (i < minY || i > maxY)
        {
            for (int j = 0; j < image->width; j++)
            {
                if (j < minX || j > maxX)
                {
                    image->pixels[i * image->width + j] = 0;
                }
            }
        }
    }
}