HypothesisGeneration.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 "HypothesisGeneration.h"
 
#include "OLPTools.h"
#include "SaliencyCalculation.h"
 
// IVT
#include "Math/FloatMatrix.h"
#include "Math/FloatVector.h"
#include "Math/LinearAlgebra.h"
#include <Calibration/Calibration.h>
#include <Features/SIFTFeatures/SIFTFeatureCalculator.h>
#include <Features/SIFTFeatures/SIFTFeatureEntry.h>
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
 
 
// OpenMP
#include <sys/time.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <omp.h>
 
CHypothesisGeneration::CHypothesisGeneration(CCalibration* calibration)
{
    ARMARX_INFO_S << "CHypothesisGeneration constructor";
 
    // create StereoCalibration
    this->calibration = calibration;
 
    // create SIFTFeatureCalculator
    m_pSIFTFeatureCalculator = new CSIFTFeatureCalculator();
 
    //m_nTimeSum = 0;
 
    m_nIterations = 0;
 
    m_pSaliencyImage = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
    m_pHypothesesCoveringImage =
        new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
    m_pSaliencyHypothesisRegionsImage =
        new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
    m_pTempImageGray = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
 
 
#ifdef OLP_USE_LCCP
    // LCCPSegmentationWrapper::LCCPSegmentationWrapper(const float voxel_resolution, const float seed_resolution, const float color_importance,
    // const float spatial_importance, const float normal_importance,, const bool use_single_cam_transform, const float concavity_tolerance_threshold,
    // const float smoothnessThreshold, const unsigned int min_segment_size, const bool use_extended_convexity, const bool use_sanity_criterion, const float maxZ)
    lccpSegmentation1 =
        new LCCPSegmentationWrapper(6, 15.0, 0.0, 1.0, 4.0, false, 10.0, 0.1, 0, false, true, 800);
    lccpSegmentation2 =
        new LCCPSegmentationWrapper(15, 26.0, 0.0, 1.0, 4.0, false, 10.0, 0.1, 0, false, true, 800);
    //lccpSegmentation = new LCCPSegmentationWrapper(7.5, 30.0, 0.0, 1.0, 4.0, false, 10.0, 0.1, 0, false, true);
#endif
    // debug only
    //fopen_s(&m_pOutFile, "c:/img/points.txt","wt");
    //fopen_s(&m_pOutFile2, "c:/img/centers.txt","wt");
    //fopen_s(&m_pOutFile3, "c:/img/center.txt","wt");
    //m_bTest = true;
}
 
CHypothesisGeneration::~CHypothesisGeneration(void)
{
    //fclose(m_pOutFile);
    //fclose(m_pOutFile2);
    //fclose(m_pOutFile3);
 
    delete m_pSIFTFeatureCalculator;
    delete m_pSaliencyImage;
    delete m_pHypothesesCoveringImage;
    delete m_pSaliencyHypothesisRegionsImage;
    delete m_pTempImageGray;
 
#ifdef OLP_USE_LCCP
    delete lccpSegmentation1;
    delete lccpSegmentation2;
#endif
}
 
void
CHypothesisGeneration::FindObjectHypotheses(CByteImage* pImageLeftColor,
                                            CByteImage* pImageRightColor,
                                            CByteImage* pImageLeftGrey,
                                            CByteImage* pImageRightGrey,
                                            CSIFTFeatureArray& aAllSIFTPoints,
                                            std::vector<CMSERDescriptor3D*>& aAllMSERs,
                                            std::vector<CHypothesisPoint*>& aPointsFromDisparity,
                                            CObjectHypothesisArray& aObjectHypotheses)
{
    ARMARX_INFO_S << "Iteration " << m_nIterations;
 
    omp_set_nested(true);
 
    timeval tStartAll, tEndAll;
    timeval tStart, tEnd;
    long tTimeDiff;
    gettimeofday(&tStartAll, 0);
 
 
    //**************************************************************************************************************
    // get all 3D Harris interest points, MSER regions and points from the depth map
    //**************************************************************************************************************
 
    m_vFeaturePoints3d.Clear();
 
    // SIFT features at Harris Corner Points
 
    for (int i = 0; i < aAllSIFTPoints.GetSize(); i++)
    {
        m_vFeaturePoints3d.AddElement(aAllSIFTPoints[i]->point3d);
    }
 
 
    // MSER regions
 
    SortMSERsByX(aAllMSERs);
 
    std::vector<Vec3d> aMSERSigmaPoints;
 
    // add the central points of the regions to the 3D points
    for (int i = 0; i < (int)aAllMSERs.size(); i++)
    {
        m_vFeaturePoints3d.AddElement(aAllMSERs.at(i)->vPosition);
 
        // if the regions are big, also add the sigma points
 
        if (aAllMSERs.at(i)->pRegionLeft->fEigenvalue1 > 25.0f) // 36  <=> sqrt(n) pixels
        {
            m_vFeaturePoints3d.AddElement(aAllMSERs.at(i)->vSigmaPoint1a);
            m_vFeaturePoints3d.AddElement(aAllMSERs.at(i)->vSigmaPoint1b);
            aMSERSigmaPoints.push_back(aAllMSERs.at(i)->vSigmaPoint1a);
            aMSERSigmaPoints.push_back(aAllMSERs.at(i)->vSigmaPoint1b);
        }
 
        if (aAllMSERs.at(i)->pRegionLeft->fEigenvalue2 > 25.0f)
        {
            m_vFeaturePoints3d.AddElement(aAllMSERs.at(i)->vSigmaPoint2a);
            m_vFeaturePoints3d.AddElement(aAllMSERs.at(i)->vSigmaPoint2b);
            aMSERSigmaPoints.push_back(aAllMSERs.at(i)->vSigmaPoint2a);
            aMSERSigmaPoints.push_back(aAllMSERs.at(i)->vSigmaPoint2b);
        }
    }
 
 
    ARMARX_VERBOSE_S << m_vFeaturePoints3d.GetSize() << " 3D-points";
 
 
    //**************************************************************************************************************
    // create hypotheses from large MSERs
    //**************************************************************************************************************
 
    CVec3dArray* paSingleColoredRegions = new CVec3dArray[OLP_MAX_NUM_HYPOTHESES];
    int nNumSingleColoredRegionHypotheses = 0;
 
#ifdef OLP_FIND_UNICOLORED_HYPOTHESES
 
    gettimeofday(&tStart, 0);
 
    // get the 2D image coordinates of all depth map points
    const int nNumPoints = aPointsFromDisparity.size();
    Vec2d* pDepthMapPoints2D = new Vec2d[nNumPoints];
 
    for (int i = 0; i < nNumPoints; i++)
    {
        calibration->WorldToImageCoordinates(
            aPointsFromDisparity.at(i)->vPosition, pDepthMapPoints2D[i], false);
    }
 
    // remove too small or too large MSERs
    std::vector<CMSERDescriptor3D*> aFilteredMSERs;
 
    for (size_t i = 0; i < aAllMSERs.size(); i++)
    {
        if (aAllMSERs.at(i)->pRegionLeft->nSize >= OLP_MSER_HYPOTHESIS_MIN_SIZE &&
            aAllMSERs.at(i)->pRegionLeft->nSize < OLP_MSER_HYPOTHESIS_MAX_SIZE)
        {
            aFilteredMSERs.push_back(aAllMSERs.at(i));
        }
    }
 
    // remove duplicate MSERs
    SortMSERsBySize(aFilteredMSERs);
 
    for (size_t i = 0; i < aFilteredMSERs.size(); i++)
    {
        for (size_t j = i + 1; j < aFilteredMSERs.size(); j++)
        {
            // if the centers are too close to each other, remove the smaller region
            if (Math3d::Distance(aFilteredMSERs.at(i)->vPosition, aFilteredMSERs.at(j)->vPosition) <
                0.5 * OLP_TOLERANCE_CONCURRENT_MOTION)
            {
                // overwrite it and move all other entries one index lower
                for (size_t k = j + 1; k < aFilteredMSERs.size(); k++)
                {
                    aFilteredMSERs.at(k - 1) = aFilteredMSERs.at(k);
                }
 
                aFilteredMSERs.pop_back();
            }
        }
    }
 
    // hypotheses contain all points that lie within the respective MSER
    const float fPixelDistanceTolerance = 2 * OLP_DEPTH_MAP_PIXEL_DISTANCE;
    const float fPixelDistanceTolerance2 = fPixelDistanceTolerance * fPixelDistanceTolerance;
#pragma omp parallel for schedule(dynamic, 1)
    for (size_t i = 0; i < aFilteredMSERs.size(); i++)
    {
        if (nNumSingleColoredRegionHypotheses >= OLP_MAX_NUM_HYPOTHESES)
        {
            continue;
        }
 
        const std::vector<Vec2d>* pRegionPoints = aFilteredMSERs.at(i)->pRegionLeft->pPoints2D;
 
        // bounding box of the MSER
        float minX = pRegionPoints->at(0).x;
        float minY = pRegionPoints->at(0).y;
        float maxX = pRegionPoints->at(0).x;
        float maxY = pRegionPoints->at(0).y;
 
        for (size_t k = 1; k < pRegionPoints->size(); k++)
        {
            minX = (pRegionPoints->at(k).x < minX) ? pRegionPoints->at(k).x : minX;
            minY = (pRegionPoints->at(k).y < minY) ? pRegionPoints->at(k).y : minY;
            maxX = (pRegionPoints->at(k).x > maxX) ? pRegionPoints->at(k).x : maxX;
            maxY = (pRegionPoints->at(k).y > maxY) ? pRegionPoints->at(k).y : maxY;
        }
 
        minX -= fPixelDistanceTolerance;
        minY -= fPixelDistanceTolerance;
        maxX += fPixelDistanceTolerance;
        maxY += fPixelDistanceTolerance;
 
        CVec3dArray pNewColorHypothesisPoints;
 
        for (int j = 0; j < nNumPoints; j++)
        {
            const float x = pDepthMapPoints2D[j].x;
            const float y = pDepthMapPoints2D[j].y;
 
            if (minX < x && minY < y && x < maxX && y < maxY)
            {
                //for (int k = 0; k < aFilteredMSERs.at(i)->pRegionLeft->nSize; k++)
                for (size_t k = 0; k < pRegionPoints->size(); k++)
                {
                    if ((x - pRegionPoints->at(k).x) * (x - pRegionPoints->at(k).x) +
                            (y - pRegionPoints->at(k).y) * (y - pRegionPoints->at(k).y) <
                        fPixelDistanceTolerance2)
                    {
                        pNewColorHypothesisPoints.AddElement(aPointsFromDisparity.at(j)->vPosition);
                        break;
                    }
                }
            }
        }
 
#pragma omp critical
        {
            if (nNumSingleColoredRegionHypotheses < OLP_MAX_NUM_HYPOTHESES)
            {
                if (pNewColorHypothesisPoints.GetSize() > OLP_MIN_NUM_FEATURES)
                {
                    for (int j = 0; j < pNewColorHypothesisPoints.GetSize(); j++)
                    {
                        paSingleColoredRegions[nNumSingleColoredRegionHypotheses].AddElement(
                            pNewColorHypothesisPoints[j]);
                    }
 
                    nNumSingleColoredRegionHypotheses++;
                }
 
                pNewColorHypothesisPoints.Clear();
            }
        }
    }
 
    gettimeofday(&tEnd, 0);
    tTimeDiff =
        (1000 * tEnd.tv_sec + tEnd.tv_usec / 1000) - (1000 * tStart.tv_sec + tStart.tv_usec / 1000);
 
    ARMARX_VERBOSE_S << "Number of MSERs: " << aAllMSERs.size() << " total, "
                     << aFilteredMSERs.size() << " unique MSERs. Found "
                     << nNumSingleColoredRegionHypotheses << " unicolored object hypotheses. Took "
                     << tTimeDiff << "ms.";
 
#endif
 
 
    //**************************************************************************************************************
    // get segments from LCCP segmentation
    //**************************************************************************************************************
 
#ifdef OLP_USE_LCCP
 
    gettimeofday(&tStart, 0);
    const int maxNumLccpHypotheses = 3000;
    CVec3dArray* lccpSegmentPoints1 = new CVec3dArray[maxNumLccpHypotheses];
    CVec3dArray* lccpSegmentPoints2 = new CVec3dArray[maxNumLccpHypotheses];
    int nNumLCCPSegmentHypotheses1 = 0;
    int nNumLCCPSegmentHypotheses2 = 0;
 
    lccpSegmentation1->CreateHypothesesFromLCCPSegments(
        aPointsFromDisparity, maxNumLccpHypotheses, lccpSegmentPoints1, nNumLCCPSegmentHypotheses1);
    lccpSegmentation2->CreateHypothesesFromLCCPSegments(
        aPointsFromDisparity, maxNumLccpHypotheses, lccpSegmentPoints2, nNumLCCPSegmentHypotheses2);
 
    int nNumLCCPSegmentHypotheses = nNumLCCPSegmentHypotheses1 + nNumLCCPSegmentHypotheses2;
    CVec3dArray* lccpSegmentPoints = new CVec3dArray[nNumLCCPSegmentHypotheses];
    for (int i = 0; i < nNumLCCPSegmentHypotheses1; i++)
    {
        lccpSegmentPoints[i] = lccpSegmentPoints1[i];
    }
    for (int i = 0; i < nNumLCCPSegmentHypotheses2; i++)
    {
        lccpSegmentPoints[nNumLCCPSegmentHypotheses1 + i] = lccpSegmentPoints2[i];
    }
    //delete lccpSegmentPoints1;
    //delete lccpSegmentPoints2;
 
 
    gettimeofday(&tEnd, 0);
    tTimeDiff =
        (1000 * tEnd.tv_sec + tEnd.tv_usec / 1000) - (1000 * tStart.tv_sec + tStart.tv_usec / 1000);
    ARMARX_VERBOSE_S << "Time for finding LCCP segments: " << tTimeDiff << " ms";
 
#endif
 
 
    //**************************************************************************************************************
    // search for planes, spheres and cylinders
    //**************************************************************************************************************
 
    gettimeofday(&tStart, 0);
 
    CVec3dArray* paPlanes = new CVec3dArray[OLP_MAX_NUM_HYPOTHESES];
    Vec3d* pPlaneNormals = new Vec3d[OLP_MAX_NUM_HYPOTHESES];
    float* pPlaneDs = new float[OLP_MAX_NUM_HYPOTHESES];
    int nNumberOfPlanes = 0;
    const int nMaxNumberOfClustersPerPlane = OLP_MAX_NUM_CLUSTERING_PARTS;
    int nNumberOfPlanesAfterClustering = 0;
    const float fBICFactorPlanes = OLP_CLUSTERING_FACTOR_PLANES; // (bigger -> more clusters)
    CVec3dArray** pPlanePointsAfterClustering =
        new CVec3dArray*[nMaxNumberOfClustersPerPlane * OLP_MAX_NUM_HYPOTHESES];
 
    CVec3dArray* paCylinders = new CVec3dArray[OLP_MAX_NUM_HYPOTHESES];
    Vec3d* pCylinderAxes = new Vec3d[OLP_MAX_NUM_HYPOTHESES];
    Vec3d* pCylinderCenters = new Vec3d[OLP_MAX_NUM_HYPOTHESES];
    float* pCylinderRadiuses = new float[OLP_MAX_NUM_HYPOTHESES];
    int nNumberOfCylinders = 0;
    const int nMaxNumberOfClustersPerCylinder = nMaxNumberOfClustersPerPlane;
    int nNumberOfCylindersAfterClustering = 0;
    const float fBICFactorCylinders = fBICFactorPlanes;
    CVec3dArray** pCylinderPointsAfterClustering =
        new CVec3dArray*[nMaxNumberOfClustersPerCylinder * OLP_MAX_NUM_HYPOTHESES];
    Vec3d* pCylinderAxesAfterClustering =
        new Vec3d[nMaxNumberOfClustersPerCylinder * OLP_MAX_NUM_HYPOTHESES];
    Vec3d* pCylinderCentersAfterClustering =
        new Vec3d[nMaxNumberOfClustersPerCylinder * OLP_MAX_NUM_HYPOTHESES];
    float* pCylinderRadiusesAfterClustering =
        new float[nMaxNumberOfClustersPerCylinder * OLP_MAX_NUM_HYPOTHESES];
 
    CVec3dArray* paSpheres = new CVec3dArray[OLP_MAX_NUM_HYPOTHESES];
    Vec3d* pSphereCenters = new Vec3d[OLP_MAX_NUM_HYPOTHESES];
    float* pSphereRadiuses = new float[OLP_MAX_NUM_HYPOTHESES];
    int nNumberOfSpheres = 0;
 
#ifdef OLP_FIND_PLANES
    const float fRansacThresholdPlanes = OLP_TOLERANCE_MODIFICATOR * 3.0f; // 2.0
    const int nRansacIterationsPlanes = (int)(OLP_EFFORT_MODIFICATOR * 500); // 1000
#endif
#ifdef OLP_FIND_CYLINDERS
    const float fRansacThresholdCylinders = OLP_TOLERANCE_MODIFICATOR * 5.0f; // 5.0
    const float fMaxCylinderRadius = 50.0f;
    bool bFoundCylinder;
#endif
#ifdef OLP_FIND_SPHERES
    const float fRansacThresholdSpheres = OLP_TOLERANCE_MODIFICATOR * 5.0f; // 5.0
    const int nRansacIterationsSpheres = (int)(OLP_EFFORT_MODIFICATOR * 20000); // 30000
    const float fMaxSphereRadius = 80.0f;
    bool bFoundSphere;
#endif
 
    bool bFoundObject = true;
    bool bFoundPlane;
    int nNumberOfPointsOnPlane, nNumberOfPointsOnSphere;
    int nNumberOfPointsOnCylinder = 1000000;
 
 
    for (int i = 0; (i < OLP_MAX_NUM_HYPOTHESES) && bFoundObject &&
                    (m_vFeaturePoints3d.GetSize() > OLP_MIN_NUM_FEATURES);
         i++)
    {
#ifdef OLP_FIND_PLANES
        //gettimeofday(&tStart, 0);
        bFoundPlane = RANSACPlane(m_vFeaturePoints3d,
                                  fRansacThresholdPlanes,
                                  nRansacIterationsPlanes,
                                  paPlanes[nNumberOfPlanes],
                                  pPlaneNormals[nNumberOfPlanes],
                                  pPlaneDs[nNumberOfPlanes]);
        nNumberOfPointsOnPlane = bFoundPlane ? paPlanes[nNumberOfPlanes].GetSize() : 0;
        bFoundPlane &= nNumberOfPointsOnPlane > OLP_MIN_NUM_FEATURES;
        //gettimeofday(&tEnd, 0);
        //tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
        //ARMARX_VERBOSE_S << "time for plane: %ld ms\n", tTimeDiff);
#else
        bFoundPlane = false;
        nNumberOfPointsOnPlane = 0;
#endif
 
#ifdef OLP_FIND_SPHERES
        bFoundSphere = RANSACSphere(m_vFeaturePoints3d,
                                    fRansacThresholdSpheres,
                                    fMaxSphereRadius,
                                    nRansacIterationsSpheres,
                                    paSpheres[nNumberOfSpheres],
                                    pSphereCenters[nNumberOfSpheres],
                                    pSphereRadiuses[nNumberOfSpheres]);
 
        if (bFoundSphere)
        {
            nNumberOfPointsOnSphere = (paSpheres[nNumberOfSpheres].GetSize() > OLP_MIN_NUM_FEATURES)
                                          ? paSpheres[nNumberOfSpheres].GetSize()
                                          : 0;
        }
        else
        {
            nNumberOfPointsOnSphere = 0;
        }
 
#else
        //bFoundSphere = false;
        nNumberOfPointsOnSphere = 0;
#endif
 
#ifdef OLP_FIND_CYLINDERS
 
        if ((nNumberOfPointsOnPlane < nNumberOfPointsOnCylinder) &&
            (nNumberOfPointsOnSphere < nNumberOfPointsOnCylinder))
        {
            //gettimeofday(&tStart, 0);
            bFoundCylinder = RANSACCylinders2(m_vFeaturePoints3d,
                                              calibration,
                                              fRansacThresholdCylinders,
                                              fMaxCylinderRadius,
                                              paCylinders[nNumberOfCylinders],
                                              pCylinderAxes[nNumberOfCylinders],
                                              pCylinderCenters[nNumberOfCylinders],
                                              pCylinderRadiuses[nNumberOfCylinders]);
 
            if (bFoundCylinder)
            {
                nNumberOfPointsOnCylinder =
                    (paCylinders[nNumberOfCylinders].GetSize() > OLP_MIN_NUM_FEATURES)
                        ? paCylinders[nNumberOfCylinders].GetSize()
                        : 0;
            }
            else
            {
                nNumberOfPointsOnCylinder = 0;
            }
 
            //gettimeofday(&tEnd, 0);
            //tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
            //ARMARX_VERBOSE_S << "time for cylinder: %ld ms\n", tTimeDiff);
        }
 
#else
        nNumberOfPointsOnCylinder = 0;
#endif
 
        bFoundObject = true;
 
        if ((nNumberOfPointsOnCylinder > nNumberOfPointsOnPlane) &&
            (nNumberOfPointsOnCylinder > nNumberOfPointsOnSphere))
        {
            RemoveOutliers(paCylinders[nNumberOfCylinders], 1.7f, NULL);
 
            // find best k-means clustering for the cylinder and divide it if appropriate
            std::vector<CVec3dArray*> aaPointClusters;
            ClusterXMeans(paCylinders[nNumberOfCylinders],
                          nMaxNumberOfClustersPerCylinder,
                          fBICFactorCylinders,
                          aaPointClusters);
            Vec3d vTemp;
 
            for (int i = 0; i < (int)aaPointClusters.size(); i++)
            {
                RemoveOutliers(*aaPointClusters.at(i), 1.7f, NULL);
 
                if (aaPointClusters.at(i)->GetSize() > OLP_MIN_NUM_FEATURES)
                {
                    if (aaPointClusters.at(i)->GetSize() /
                            GetVariance(*aaPointClusters.at(i), vTemp) >
                        0.004f) // 0.004
                    {
                        pCylinderPointsAfterClustering[nNumberOfCylindersAfterClustering] =
                            aaPointClusters.at(i);
                        pCylinderAxesAfterClustering[nNumberOfCylindersAfterClustering] =
                            pCylinderAxes[nNumberOfCylinders];
                        pCylinderCentersAfterClustering[nNumberOfCylindersAfterClustering] =
                            pCylinderCenters[nNumberOfCylinders];
                        pCylinderRadiusesAfterClustering[nNumberOfCylindersAfterClustering] =
                            pCylinderRadiuses[nNumberOfCylinders];
 
                        // remove points belonging to the cylinder
                        for (int j = 0;
                             j < pCylinderPointsAfterClustering[nNumberOfCylindersAfterClustering]
                                     ->GetSize();
                             j++)
                        {
                            for (int k = 0; k < m_vFeaturePoints3d.GetSize(); k++)
                            {
                                if (m_vFeaturePoints3d[k].x ==
                                        (*pCylinderPointsAfterClustering
                                             [nNumberOfCylindersAfterClustering])[j]
                                            .x &&
                                    m_vFeaturePoints3d[k].y ==
                                        (*pCylinderPointsAfterClustering
                                             [nNumberOfCylindersAfterClustering])[j]
                                            .y &&
                                    m_vFeaturePoints3d[k].z ==
                                        (*pCylinderPointsAfterClustering
                                             [nNumberOfCylindersAfterClustering])[j]
                                            .z)
                                {
                                    const int nEnd = m_vFeaturePoints3d.GetSize() - 1;
                                    m_vFeaturePoints3d[k] = m_vFeaturePoints3d[nEnd];
                                    m_vFeaturePoints3d.DeleteElement(nEnd);
                                    break;
                                }
                            }
                        }
 
                        nNumberOfCylindersAfterClustering++;
                    }
                }
            }
 
            aaPointClusters.clear();
 
            //ARMARX_VERBOSE_S << "\nFound a cylinder with %d points\nAxis: ( %.2f  %.2f  %.2f )   Radius: %.1f\n\n", paCylinders[nNumberOfCylinders].GetSize(),
            //  pCylinderAxes[nNumberOfCylinders].x, pCylinderAxes[nNumberOfCylinders].y, pCylinderAxes[nNumberOfCylinders].z, pCylinderRadiuses[nNumberOfCylinders]);
            nNumberOfCylinders++;
        }
        else if (nNumberOfPointsOnSphere > nNumberOfPointsOnPlane)
        {
            RemoveOutliers(paSpheres[nNumberOfSpheres], 1.6f, NULL);
 
            // remove points belonging to found sphere
            if (paSpheres[nNumberOfSpheres].GetSize() > OLP_MIN_NUM_FEATURES)
            {
                for (int j = 0; j < paSpheres[nNumberOfSpheres].GetSize(); j++)
                {
                    for (int l = 0; l < m_vFeaturePoints3d.GetSize(); l++)
                    {
                        if (m_vFeaturePoints3d[l].x == paSpheres[nNumberOfSpheres][j].x &&
                            m_vFeaturePoints3d[l].y == paSpheres[nNumberOfSpheres][j].y &&
                            m_vFeaturePoints3d[l].z == paSpheres[nNumberOfSpheres][j].z)
                        {
                            const int nEnd = m_vFeaturePoints3d.GetSize() - 1;
                            m_vFeaturePoints3d[l] = m_vFeaturePoints3d[nEnd];
                            m_vFeaturePoints3d.DeleteElement(nEnd);
                            break;
                        }
                    }
                }
 
                nNumberOfSpheres++;
            }
        }
        else if (bFoundPlane)
        {
            RemoveOutliers(paPlanes[nNumberOfPlanes], 1.7f, NULL);
 
            // find best k-means clustering for the plane and divide it if appropriate
            std::vector<CVec3dArray*> aaPointClusters;
            ClusterXMeans(paPlanes[nNumberOfPlanes],
                          nMaxNumberOfClustersPerPlane,
                          fBICFactorPlanes,
                          aaPointClusters);
            Vec3d vTemp;
 
            for (int i = 0; i < (int)aaPointClusters.size(); i++)
            {
                RemoveOutliers(*aaPointClusters.at(i), 1.7f, NULL);
                RemoveOutliers(*aaPointClusters.at(i), 2.0f, NULL);
 
                if (aaPointClusters.at(i)->GetSize() > OLP_MIN_NUM_FEATURES)
                {
                    if (aaPointClusters.at(i)->GetSize() /
                            GetVariance(*aaPointClusters.at(i), vTemp) >
                        0.004f) // 0.004
                    {
                        pPlanePointsAfterClustering[nNumberOfPlanesAfterClustering] =
                            aaPointClusters.at(i);
 
                        // remove points belonging to the plane
                        for (int j = 0;
                             j <
                             pPlanePointsAfterClustering[nNumberOfPlanesAfterClustering]->GetSize();
                             j++)
                        {
                            for (int k = 0; k < m_vFeaturePoints3d.GetSize(); k++)
                            {
                                if (m_vFeaturePoints3d[k].x ==
                                        (*pPlanePointsAfterClustering
                                             [nNumberOfPlanesAfterClustering])[j]
                                            .x &&
                                    m_vFeaturePoints3d[k].y ==
                                        (*pPlanePointsAfterClustering
                                             [nNumberOfPlanesAfterClustering])[j]
                                            .y &&
                                    m_vFeaturePoints3d[k].z ==
                                        (*pPlanePointsAfterClustering
                                             [nNumberOfPlanesAfterClustering])[j]
                                            .z)
                                {
                                    const int nEnd = m_vFeaturePoints3d.GetSize() - 1;
                                    m_vFeaturePoints3d[k] = m_vFeaturePoints3d[nEnd];
                                    m_vFeaturePoints3d.DeleteElement(nEnd);
                                    break;
                                }
                            }
                        }
 
                        nNumberOfPlanesAfterClustering++;
                    }
                }
            }
 
            aaPointClusters.clear();
 
            //ARMARX_VERBOSE_S << "\nFound a plane with %d points\nNormal: ( %.2f  %.2f  %.2f )\n\n", paPlanes[nNumberOfPlanes].GetSize(),
            //                                  pPlaneNormals[nNumberOfPlanes].x, pPlaneNormals[nNumberOfPlanes].y, pPlaneNormals[nNumberOfPlanes].z);
            nNumberOfPlanes++;
        }
        else
        {
            bFoundObject = false;
        }
    }
 
 
    ARMARX_VERBOSE_S << "Found " << nNumberOfCylinders << " cylinders and " << nNumberOfPlanes
                     << " planes (" << nNumberOfPlanesAfterClustering
                     << " planes after clustering)";
 
    gettimeofday(&tEnd, 0);
    tTimeDiff =
        (1000 * tEnd.tv_sec + tEnd.tv_usec / 1000) - (1000 * tStart.tv_sec + tStart.tv_usec / 1000);
    ARMARX_VERBOSE_S << "Time for finding planes, cylinders and spheres: " << tTimeDiff << " ms";
 
    //m_nTimeSum += tTimeDiff;
    //ARMARX_VERBOSE_S << "Avg. time for finding objects: " << m_nTimeSum/(m_nIterations+1) << " ms";
 
 
    //**************************************************************************************************************
    // try to find clusters amongst the leftover points
    //**************************************************************************************************************
 
    gettimeofday(&tStart, 0);
 
 
    int nNumberOfUnstructuredClusters = 0;
    CVec3dArray** pUnstructuredPointClusters = NULL;
#ifdef OLP_FIND_IRREGULAR_CLUSTERS
    std::vector<CVec3dArray*> aaUnstructuredPointClusters;
    const int nMaxNumberOfUnstructuredClusters = 15 + (int)(2 * sqrtf(OLP_EFFORT_MODIFICATOR));
    const float fBICFactorLeftoverPoints = 4.0f * OLP_CLUSTERING_FACTOR_PLANES;
    //cv::Mat mSamples;
    //cv::Mat mClusterLabels;
    //cv::Mat* pmClusterCenters = NULL;
    //const int nNumberOfDifferentInitialisations = 5;
    //cv::TermCriteria tTerminationCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 50, 0.01);
    pUnstructuredPointClusters = new CVec3dArray*[nMaxNumberOfUnstructuredClusters];
 
    // remove points that lie far off
    RemoveOutliers(m_vFeaturePoints3d, 2.0f);
 
    // find best k-means clustering for the points
    ClusterXMeans(m_vFeaturePoints3d,
                  nMaxNumberOfUnstructuredClusters,
                  fBICFactorLeftoverPoints,
                  aaUnstructuredPointClusters);
    Vec3d vTemp;
 
    // check if the clusters form plausible hypotheses
    for (int i = 0; i < (int)aaUnstructuredPointClusters.size(); i++)
    {
        RemoveOutliers(*aaUnstructuredPointClusters.at(i), 1.5f, NULL);
        RemoveOutliers(*aaUnstructuredPointClusters.at(i), 2.0f, NULL);
 
        if (aaUnstructuredPointClusters.at(i)->GetSize() > OLP_MIN_NUM_FEATURES)
        {
            if (aaUnstructuredPointClusters.at(i)->GetSize() /
                    GetVariance(*aaUnstructuredPointClusters.at(i), vTemp) >
                0.004f) // 0.003
            {
                pUnstructuredPointClusters[nNumberOfUnstructuredClusters] =
                    aaUnstructuredPointClusters.at(i);
 
                // remove points belonging to the plane
                for (int j = 0;
                     j < pUnstructuredPointClusters[nNumberOfUnstructuredClusters]->GetSize();
                     j++)
                {
                    for (int k = 0; k < m_vFeaturePoints3d.GetSize(); k++)
                    {
                        if (m_vFeaturePoints3d[k].x ==
                                (*pUnstructuredPointClusters[nNumberOfUnstructuredClusters])[j].x &&
                            m_vFeaturePoints3d[k].y ==
                                (*pUnstructuredPointClusters[nNumberOfUnstructuredClusters])[j].y &&
                            m_vFeaturePoints3d[k].z ==
                                (*pUnstructuredPointClusters[nNumberOfUnstructuredClusters])[j].z)
                        {
                            const int nEnd = m_vFeaturePoints3d.GetSize() - 1;
                            m_vFeaturePoints3d[k] = m_vFeaturePoints3d[nEnd];
                            m_vFeaturePoints3d.DeleteElement(nEnd);
                            break;
                        }
                    }
                }
 
                nNumberOfUnstructuredClusters++;
            }
        }
    }
 
    aaUnstructuredPointClusters.clear();
#endif
 
 
    //ARMARX_VERBOSE_S << "Found %d clusters amongst the leftover features\n", nNumberOfUnstructuredClusters);
 
    //gettimeofday(&tEnd, 0);
    //tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
    //ARMARX_VERBOSE_S << "Time for clustering of leftover features: %ld ms\n", tTimeDiff);
 
 
    //**************************************************************************************************************
    // generate hypothesis descriptors
    //**************************************************************************************************************
 
 
    // add the plane, cylinder and sphere hypotheses, sorted by the number of points that they contain
 
    CObjectHypothesis* pHypothesis;
 
    // add the planes to the hypothesis list
    for (int i = 0; i < nNumberOfPlanesAfterClustering; i++)
    {
        pHypothesis = CreatePlaneHypothesisDescriptor(*pPlanePointsAfterClustering[i],
                                                      aAllMSERs,
                                                      aMSERSigmaPoints,
                                                      pImageLeftColor,
                                                      pImageLeftGrey);
        aObjectHypotheses.AddElement(pHypothesis);
    }
 
    // add the cylinders to the hypothesis list
    for (int i = 0; i < nNumberOfCylindersAfterClustering; i++)
    {
        pHypothesis = CreateCylinderHypothesisDescriptor(*pCylinderPointsAfterClustering[i],
                                                         pCylinderAxesAfterClustering[i],
                                                         pCylinderCentersAfterClustering[i],
                                                         pCylinderRadiusesAfterClustering[i],
                                                         aAllMSERs,
                                                         aMSERSigmaPoints,
                                                         pImageLeftColor,
                                                         pImageLeftGrey);
        aObjectHypotheses.AddElement(pHypothesis);
    }
 
 
    // add the spheres to the hypothesis list
    for (int i = 0; i < nNumberOfSpheres; i++)
    {
        pHypothesis = CreateSphereHypothesisDescriptor(paSpheres[i],
                                                       pSphereCenters[i],
                                                       pSphereRadiuses[i],
                                                       aAllMSERs,
                                                       aMSERSigmaPoints,
                                                       pImageLeftColor,
                                                       pImageLeftGrey);
        aObjectHypotheses.AddElement(pHypothesis);
    }
 
 
    // add the unstructured clusters
    for (int i = 0; i < nNumberOfUnstructuredClusters; i++)
    {
        pHypothesis = CreatePlaneHypothesisDescriptor(*pUnstructuredPointClusters[i],
                                                      aAllMSERs,
                                                      aMSERSigmaPoints,
                                                      pImageLeftColor,
                                                      pImageLeftGrey);
        pHypothesis->eType = CObjectHypothesis::eUnstructured;
        aObjectHypotheses.AddElement(pHypothesis);
    }
 
    // add the hypotheses from unicolored regions
    for (int i = 0; i < nNumSingleColoredRegionHypotheses; i++)
    {
        pHypothesis = CreatePlaneHypothesisDescriptor(paSingleColoredRegions[i],
                                                      aAllMSERs,
                                                      aMSERSigmaPoints,
                                                      pImageLeftColor,
                                                      pImageLeftGrey);
        pHypothesis->eType = CObjectHypothesis::eSingleColored;
        aObjectHypotheses.AddElement(pHypothesis);
    }
 
#ifdef OLP_USE_LCCP
 
    ARMARX_VERBOSE_S << "Adding " << nNumLCCPSegmentHypotheses << " hypotheses from LCCP";
    int counter = 0;
    for (int i = 0; i < nNumLCCPSegmentHypotheses; i++)
    {
        if ((lccpSegmentPoints[i].GetSize() > OLP_MIN_SIZE_LCCP_SEGMENT) &&
            (lccpSegmentPoints[i].GetSize() < OLP_MAX_SIZE_LCCP_SEGMENT))
        {
            pHypothesis = CreatePlaneHypothesisDescriptor(
                lccpSegmentPoints[i], aAllMSERs, aMSERSigmaPoints, pImageLeftColor, pImageLeftGrey);
            pHypothesis->eType = CObjectHypothesis::eRGBD;
            aObjectHypotheses.AddElement(pHypothesis);
 
#ifdef OLP_MAKE_LCCP_SEG_IMAGES
            CByteImage* segmentationImage =
                new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
            COLPTools::CreateSegmentationProbabilityMap(
                pHypothesis, calibration, segmentationImage);
 
            std::string fileName = OLP_SCREENSHOT_PATH;
            fileName.append("lccp0000.bmp");
            int fileNumber = m_nIterations * 1000 + counter;
            COLPTools::SetNumberInFileName(fileName, fileNumber);
            counter++;
            ARMARX_VERBOSE_S << "Saving LCCP segmentation image for segment " << i << ": "
                             << fileName;
            segmentationImage->SaveToFile(fileName.c_str());
            delete segmentationImage;
#endif
        }
    }
 
#endif
 
 
    //**************************************************************************************************************
    // generate hypotheses in the not-yet-covered salient regions
    //**************************************************************************************************************
 
#ifdef OLP_FIND_SALIENCY_HYPOTHESES
 
    gettimeofday(&tStart, 0);
 
    // calculate saliency map
    CSaliencyCalculation::FindSalientRegionsAchanta(pImageLeftColor, m_pSaliencyImage);
 
    // calculate area already covered by other hypotheses
    ImageProcessor::Zero(m_pHypothesesCoveringImage);
 
    for (int i = 0; i < aObjectHypotheses.GetSize(); i++)
    {
        COLPTools::CreateObjectSegmentationMask(
            aObjectHypotheses[i], calibration, m_pTempImageGray);
        ImageProcessor::Or(
            m_pHypothesesCoveringImage, m_pTempImageGray, m_pHypothesesCoveringImage);
    }
 
    ImageProcessor::Dilate(m_pHypothesesCoveringImage, m_pTempImageGray);
    ImageProcessor::Dilate(m_pTempImageGray, m_pHypothesesCoveringImage);
    ImageProcessor::Dilate(m_pHypothesesCoveringImage, m_pTempImageGray);
    ImageProcessor::Dilate(m_pTempImageGray, m_pHypothesesCoveringImage);
    ImageProcessor::Dilate(m_pHypothesesCoveringImage, m_pTempImageGray);
    ImageProcessor::Erode(m_pTempImageGray, m_pHypothesesCoveringImage);
    ImageProcessor::Erode(m_pHypothesesCoveringImage, m_pTempImageGray);
    ImageProcessor::Erode(m_pTempImageGray, m_pHypothesesCoveringImage);
    //m_pHypothesesCoveringImage->SaveToFile("/home/staff/schieben/SalHypRegionsCover.bmp");
 
    // get remaining salient regions
    ImageProcessor::HistogramStretching(
        m_pSaliencyImage, m_pSaliencyHypothesisRegionsImage, 0.5f, 1.0f);
 
    for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
    {
        m_pSaliencyHypothesisRegionsImage->pixels[i] =
            (m_pHypothesesCoveringImage->pixels[i]) ? 0
                                                    : m_pSaliencyHypothesisRegionsImage->pixels[i];
    }
 
    //m_pSaliencyHypothesisRegionsImage->SaveToFile("/homes/staff/schieben/SalHypRegions.bmp");
 
    // get and cluster points in remaining salient regions
    std::vector<CHypothesisPoint*> aPointsInSalientRegions;
    COLPTools::FilterForegroundPoints(aPointsFromDisparity,
                                      m_pSaliencyHypothesisRegionsImage,
                                      calibration,
                                      aPointsInSalientRegions);
 
    std::vector<std::vector<CHypothesisPoint*>> aSalientRegionsPointsClusters;
    COLPTools::ClusterXMeans(aPointsInSalientRegions,
                             1,
                             20,
                             10 * OLP_CLUSTERING_FACTOR_FOREGROUND_HYPOTHESES,
                             aSalientRegionsPointsClusters);
    ARMARX_VERBOSE_S << aSalientRegionsPointsClusters.size() << " clusters in salient regions";
 
    // add hypotheses
    for (size_t i = 0; i < aSalientRegionsPointsClusters.size(); i++)
    {
        CVec3dArray aTempArray;
 
        for (size_t j = 0; j < aSalientRegionsPointsClusters.at(i).size(); j++)
        {
            aTempArray.AddElement(aSalientRegionsPointsClusters.at(i).at(j)->vPosition);
        }
 
        RemoveOutliers(aTempArray, 1.5f, NULL);
 
        if (aTempArray.GetSize() > OLP_MIN_NUM_FEATURES)
        {
            pHypothesis = CreatePlaneHypothesisDescriptor(
                aTempArray, aAllMSERs, aMSERSigmaPoints, pImageLeftColor, pImageLeftGrey);
            pHypothesis->eType = CObjectHypothesis::eRGBD;
            aObjectHypotheses.AddElement(pHypothesis);
        }
    }
 
    gettimeofday(&tEnd, 0);
    tTimeDiff =
        (1000 * tEnd.tv_sec + tEnd.tv_usec / 1000) - (1000 * tStart.tv_sec + tStart.tv_usec / 1000);
    ARMARX_VERBOSE_S << "Time for saliency hypotheses: " << tTimeDiff << " ms";
 
#endif
 
 
    //**************************************************************************************************************
    // sort the hypotheses in decreasing order
    //**************************************************************************************************************
 
    int nSize;
 
    for (int i = 1; i < aObjectHypotheses.GetSize(); i++)
    {
        pHypothesis = aObjectHypotheses[i];
        nSize = (int)aObjectHypotheses[i]->aNewPoints.size();
        int j = i;
 
        while ((j > 0) && ((int)aObjectHypotheses[j - 1]->aNewPoints.size() < nSize))
        {
            aObjectHypotheses[j] = aObjectHypotheses[j - 1];
            j--;
        }
 
        aObjectHypotheses[j] = pHypothesis;
    }
 
    // keep only as many hypotheses as wanted
    for (int i = aObjectHypotheses.GetSize() - 1; i > OLP_MAX_NUM_HYPOTHESES; i--)
    {
        delete aObjectHypotheses[i];
        aObjectHypotheses.DeleteElement(i);
    }
 
    // give the hypotheses their numbers
    for (int i = 0; i < aObjectHypotheses.GetSize(); i++)
    {
        aObjectHypotheses[i]->nHypothesisNumber = i;
    }
 
 
    //    // print hypothesis numbers and types
    //  for (int i=0; i<aObjectHypotheses.GetSize(); i++)
    //  {
    //        if (i%2 == 0) ARMARX_VERBOSE_S << "\n");
    //      ARMARX_VERBOSE_S << "Hyp. %d: ", i);
    //        if (aObjectHypotheses[i]->eType == CObjectHypothesis::eCylinder)
    //      {
    //            ARMARX_VERBOSE_S << "C (%d p, r=%.1f)  \t", (int)aObjectHypotheses[i]->aNewPoints.size(), aObjectHypotheses[i]->fRadius);
    //      }
    //        else if (aObjectHypotheses[i]->eType == CObjectHypothesis::ePlane)
    //      {
    //            ARMARX_VERBOSE_S << "P (%d p)            \t", (int)aObjectHypotheses[i]->aNewPoints.size());
    //      }
    //        else if (aObjectHypotheses[i]->eType == CObjectHypothesis::eSphere)
    //      {
    //            ARMARX_VERBOSE_S << "S (%d p, r=%.1f)  \t", (int)aObjectHypotheses[i]->aNewPoints.size(), aObjectHypotheses[i]->fRadius);
    //      }
    //      else
    //      {
    //            ARMARX_VERBOSE_S << "U (%d p)  \t", (int)aObjectHypotheses[i]->aNewPoints.size());
    //        }
    //  }
    //  ARMARX_VERBOSE_S << "\n\n");
 
 
    // clean up
    delete[] paPlanes;
    delete[] pPlaneNormals;
    delete[] pPlaneDs;
 
    for (int i = 0; i < nNumberOfPlanesAfterClustering; i++)
    {
        delete pPlanePointsAfterClustering[i];
    }
 
    delete[] pPlanePointsAfterClustering;
 
    delete[] paCylinders;
    delete[] pCylinderAxes;
    delete[] pCylinderCenters;
    delete[] pCylinderRadiuses;
 
    for (int i = 0; i < nNumberOfCylindersAfterClustering; i++)
    {
        delete pCylinderPointsAfterClustering[i];
    }
 
    delete[] pCylinderPointsAfterClustering;
    delete[] pCylinderAxesAfterClustering;
    delete[] pCylinderCentersAfterClustering;
    delete[] pCylinderRadiusesAfterClustering;
 
    delete[] paSpheres;
    delete[] pSphereCenters;
    delete[] pSphereRadiuses;
 
    delete[] pUnstructuredPointClusters;
 
    m_nIterations++;
 
 
    gettimeofday(&tEndAll, 0);
    tTimeDiff = (1000 * tEndAll.tv_sec + tEndAll.tv_usec / 1000) -
                (1000 * tStartAll.tv_sec + tStartAll.tv_usec / 1000);
    ARMARX_VERBOSE_S << "Time for complete hypothesis generation: " << tTimeDiff << " ms";
}
 
float
CHypothesisGeneration::GetVariance(const CVec3dArray& avPoints, Vec3d& vMean)
{
    vMean.x = 0;
    vMean.y = 0;
    vMean.z = 0;
    float fVariance = 0;
 
    // calculate mean
    for (int i = 0; i < avPoints.GetSize(); i++)
    {
        vMean.x += avPoints[i].x;
        vMean.y += avPoints[i].y;
        vMean.z += avPoints[i].z;
    }
 
    vMean.x /= (float)avPoints.GetSize();
    vMean.y /= (float)avPoints.GetSize();
    vMean.z /= (float)avPoints.GetSize();
 
    // calculate variance
    for (int i = 0; i < avPoints.GetSize(); i++)
    {
        fVariance += (vMean.x - avPoints[i].x) * (vMean.x - avPoints[i].x) +
                     (vMean.y - avPoints[i].y) * (vMean.y - avPoints[i].y) +
                     (vMean.z - avPoints[i].z) * (vMean.z - avPoints[i].z);
    }
 
    return (fVariance / (float)avPoints.GetSize());
}
 
void
CHypothesisGeneration::RemoveOutliers(CVec3dArray& avPoints,
                                      float fStdDevFactor,
                                      std::vector<int>* paIndices)
{
    if (avPoints.GetSize() < 9)
    {
        return;
    }
 
    Vec3d vMean;
    const float fStandardDeviation = sqrtf(GetVariance(avPoints, vMean));
    const float fThreshold2 =
        fStdDevFactor * fStdDevFactor * fStandardDeviation * fStandardDeviation;
 
    // remove all points that are further away from the mean than fStdDevFactor x standard deviation
    float fDist2;
 
    if (paIndices != NULL)
    {
        for (int i = 0; i < avPoints.GetSize(); i++)
        {
            fDist2 = (vMean.x - avPoints[i].x) * (vMean.x - avPoints[i].x) +
                     (vMean.y - avPoints[i].y) * (vMean.y - avPoints[i].y) +
                     (vMean.z - avPoints[i].z) * (vMean.z - avPoints[i].z);
 
            if (fDist2 > fThreshold2)
            {
                avPoints[i] = avPoints[avPoints.GetSize() - 1];
                paIndices->at(i) = paIndices->at(avPoints.GetSize() - 1);
                avPoints.DeleteElement(avPoints.GetSize() - 1);
                paIndices->pop_back();
                i--;
            }
        }
    }
    else
    {
        for (int i = 0; i < avPoints.GetSize(); i++)
        {
            fDist2 = (vMean.x - avPoints[i].x) * (vMean.x - avPoints[i].x) +
                     (vMean.y - avPoints[i].y) * (vMean.y - avPoints[i].y) +
                     (vMean.z - avPoints[i].z) * (vMean.z - avPoints[i].z);
 
            if (fDist2 > fThreshold2)
            {
                avPoints[i] = avPoints[avPoints.GetSize() - 1];
                avPoints.DeleteElement(avPoints.GetSize() - 1);
                i--;
            }
        }
    }
}
 
bool
CHypothesisGeneration::FindCylinder(const CVec3dArray& avSamplePoints,
                                    const CVec3dArray& avAllPoints,
                                    const float fToleranceThreshold,
                                    const float fMaxRadius,
                                    CVec3dArray& avResultPoints,
                                    Vec3d& vCylinderAxis,
                                    Vec3d& vCylinderCenter,
                                    float& fCylinderRadius)
{
    avResultPoints.Clear();
 
    const int nNumberOfSamples = avSamplePoints.GetSize();
 
    if (nNumberOfSamples < 5)
    {
        return false;
    }
 
    const int nNumberOfAllPoints = avAllPoints.GetSize();
 
    const Vec3d vZAxis = {0.0f, 0.0f, 1.0f};
 
    Vec3d vBestCylinderCenter, vBestCylinderAxis;
    float fBestCylinderRadius = 0;
    int nMaxNumberOfPointsOnCylinder = 0;
 
    Vec3d vAvgPoint = {0.0f, 0.0f, 0.0f};
 
    for (int i = 0; i < nNumberOfSamples; i++)
    {
        vAvgPoint.x += avSamplePoints[i].x;
        vAvgPoint.y += avSamplePoints[i].y;
        vAvgPoint.z += avSamplePoints[i].z;
    }
 
    vAvgPoint.x /= (float)nNumberOfSamples;
    vAvgPoint.y /= (float)nNumberOfSamples;
    vAvgPoint.z /= (float)nNumberOfSamples;
 
    CFloatMatrix mSamplePoints(3, nNumberOfSamples);
 
    for (int i = 0; i < nNumberOfSamples; i++)
    {
        mSamplePoints(0, i) = avSamplePoints[i].x - vAvgPoint.x;
        mSamplePoints(1, i) = avSamplePoints[i].y - vAvgPoint.y;
        mSamplePoints(2, i) = avSamplePoints[i].z - vAvgPoint.z;
    }
 
    CFloatMatrix mEigenVectors(3, 3);
    CFloatMatrix mEigenValues(1, 3);
    CFloatMatrix mProjection(2, 3);
    Mat3d mTransformation;
 
    LinearAlgebra::PCA(&mSamplePoints, &mEigenVectors, &mEigenValues);
 
    //ARMARX_VERBOSE_S << "\nAvg: ( %.1f  %.1f  %.1f )\n", vAvgPoint.x, vAvgPoint.y, vAvgPoint.z);
    //ARMARX_VERBOSE_S << "\nEigenvectors and Eigenvalues:\n");
    //ARMARX_VERBOSE_S << "( %.3f  %.3f  %.3f )   %.3f\n", mEigenVectors(0,0), mEigenVectors(0,1), mEigenVectors(0,2), sqrtf(mEigenValues(0,0)));
    //ARMARX_VERBOSE_S << "( %.3f  %.3f  %.3f )   %.3f\n", mEigenVectors(1,0), mEigenVectors(1,1), mEigenVectors(1,2), sqrtf(mEigenValues(0,1)));
    //ARMARX_VERBOSE_S << "( %.3f  %.3f  %.3f )   %.3f\n\n", mEigenVectors(2,0), mEigenVectors(2,1), mEigenVectors(2,2), sqrtf(mEigenValues(0,2)));
 
    Vec3d vTemp1, vTemp2;
 
    for (int n = 0; n < 3; n++)
    {
        switch (n)
        {
            case 0:
                // use eigenvector with largest eigenvalue as cylinder axis
                vCylinderAxis.x = mEigenVectors(0, 0);
                vCylinderAxis.y = mEigenVectors(0, 1);
                vCylinderAxis.z = mEigenVectors(0, 2);
                Math3d::NormalizeVec(vCylinderAxis);
 
                // matrix that projects the points to the subspace plane that is orthogonal to that
                // eigenvector, i.e. the plane spanned by the second and third eigenvector
                mProjection(0, 0) = mEigenVectors(1, 0);
                mProjection(0, 1) = mEigenVectors(1, 1);
                mProjection(0, 2) = mEigenVectors(1, 2);
                mProjection(1, 0) = mEigenVectors(2, 0);
                mProjection(1, 1) = mEigenVectors(2, 1);
                mProjection(1, 2) = mEigenVectors(2, 2);
 
                // matrix that transforms 3D points into the coordinate system given by
                // the cylinder axis and two orthogonal vectors
                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);
 
                break;
 
            case 1:
                // use eigenvector with second largest eigenvalue as cylinder axis
                vCylinderAxis.x = mEigenVectors(1, 0);
                vCylinderAxis.y = mEigenVectors(1, 1);
                vCylinderAxis.z = mEigenVectors(1, 2);
                Math3d::NormalizeVec(vCylinderAxis);
 
                // matrix that projects the points to the subspace plane that is orthogonal to that
                // eigenvector, i.e. the plane spanned by the first and third eigenvector
                mProjection(0, 0) = mEigenVectors(0, 0);
                mProjection(0, 1) = mEigenVectors(0, 1);
                mProjection(0, 2) = mEigenVectors(0, 2);
                mProjection(1, 0) = mEigenVectors(2, 0);
                mProjection(1, 1) = mEigenVectors(2, 1);
                mProjection(1, 2) = mEigenVectors(2, 2);
 
                // matrix that transforms 3D points into the coordinate system given by
                // the cylinder axis and two orthogonal vectors
                mTransformation.r1 = mEigenVectors(1, 0);
                mTransformation.r2 = mEigenVectors(0, 0);
                mTransformation.r3 = mEigenVectors(2, 0);
                mTransformation.r4 = mEigenVectors(1, 1);
                mTransformation.r5 = mEigenVectors(0, 1);
                mTransformation.r6 = mEigenVectors(2, 1);
                mTransformation.r7 = mEigenVectors(1, 2);
                mTransformation.r8 = mEigenVectors(0, 2);
                mTransformation.r9 = mEigenVectors(2, 2);
 
                break;
 
            case 2:
 
                // use eigenvector with largest y value and turn it more towards the y-axis
                if (fabs(mEigenVectors(0, 1)) > fabs(mEigenVectors(1, 1)))
                {
                    if (fabs(mEigenVectors(0, 1)) > fabs(mEigenVectors(2, 1)))
                    {
                        vCylinderAxis.x = mEigenVectors(0, 0);
                        vCylinderAxis.y = 2.0f * mEigenVectors(0, 1);
                        vCylinderAxis.z = mEigenVectors(0, 2);
                    }
                    else
                    {
                        vCylinderAxis.x = mEigenVectors(2, 0);
                        vCylinderAxis.y = 2.0f * mEigenVectors(2, 1);
                        vCylinderAxis.z = mEigenVectors(2, 2);
                    }
                }
                else
                {
                    if (fabs(mEigenVectors(1, 1)) > fabs(mEigenVectors(2, 1)))
                    {
                        vCylinderAxis.x = mEigenVectors(1, 0);
                        vCylinderAxis.y = 2.0f * mEigenVectors(1, 1);
                        vCylinderAxis.z = mEigenVectors(1, 2);
                        Math3d::NormalizeVec(vCylinderAxis);
                    }
                    else
                    {
                        vCylinderAxis.x = mEigenVectors(2, 0);
                        vCylinderAxis.y = 2.0f * mEigenVectors(2, 1);
                        vCylinderAxis.z = mEigenVectors(2, 2);
                    }
                }
 
                Math3d::NormalizeVec(vCylinderAxis);
 
                // create two axes orthogonal to the cylinder axis
                Math3d::CrossProduct(vCylinderAxis, vZAxis, vTemp1);
                Math3d::NormalizeVec(vTemp1);
                Math3d::CrossProduct(vCylinderAxis, vTemp1, vTemp2);
 
                // projection matrix to the plane that is orthogonal to the cylinder axis
                mProjection(0, 0) = vTemp1.x;
                mProjection(0, 1) = vTemp1.y;
                mProjection(0, 2) = vTemp1.z;
                mProjection(1, 0) = vTemp2.x;
                mProjection(1, 1) = vTemp2.y;
                mProjection(1, 2) = vTemp2.z;
 
                // matrix that transforms 3D points into the coordinate system given by
                // the cylinder axis and two orthogonal vectors
                mTransformation.r1 = vCylinderAxis.x;
                mTransformation.r2 = vTemp1.x;
                mTransformation.r3 = vTemp2.x;
                mTransformation.r4 = vCylinderAxis.y;
                mTransformation.r5 = vTemp1.y;
                mTransformation.r6 = vTemp2.y;
                mTransformation.r7 = vCylinderAxis.z;
                mTransformation.r8 = vTemp1.z;
                mTransformation.r9 = vTemp2.z;
 
                break;
 
            case 3:
                // use y-axis as cylinder axis
                vCylinderAxis.x = 0.0f;
                vCylinderAxis.y = 1.0f;
                vCylinderAxis.z = 0.0f;
 
                // matrix that projects the points to the subspace plane that is spanned
                // by the x- and z-axis
                mProjection(0, 0) = 1.0f;
                mProjection(0, 1) = 0.0f;
                mProjection(0, 2) = 0.0f;
                mProjection(1, 0) = 0.0f;
                mProjection(1, 1) = 0.0f;
                mProjection(1, 2) = 1.0f;
 
                // matrix that transforms 3D points into the coordinate system given by
                // the cylinder axis and two orthogonal vectors
                mTransformation.r1 = 0.0f;
                mTransformation.r2 = 1.0f;
                mTransformation.r3 = 0.0f;
                mTransformation.r4 = 1.0f;
                mTransformation.r5 = 0.0f;
                mTransformation.r6 = 0.0f;
                mTransformation.r7 = 0.0f;
                mTransformation.r8 = 0.0f;
                mTransformation.r9 = 1.0f;
        }
 
 
        // project the points
        CFloatMatrix mProjectedPoints(2, nNumberOfSamples);
        LinearAlgebra::MulMatMat(&mSamplePoints, &mProjection, &mProjectedPoints);
 
        //for (int i=0; i<nNumberOfSamples; i++)
        //{
        //ARMARX_VERBOSE_S << "3D: ( %.3f  %.3f  %.3f )   2D: ( %.3f  %.3f )\n", mSamplePoints(0,i), mSamplePoints(1,i), mSamplePoints(2,i), mProjectedPoints(0,i), mProjectedPoints(1,i));
        //fARMARX_VERBOSE_S << m_pOutFile, "%f %f\n", mProjectedPoints(0,i), mProjectedPoints(1,i));
        //}
        //ARMARX_VERBOSE_S << "\n\n");
 
        // sort the projected points by their first coordinate to support sampling
        QuickSort2DPointsX(mProjectedPoints, 0, nNumberOfSamples - 1);
 
        //for (int i=0; i<nNumberOfSamples; i++)
        //{
        //  ARMARX_VERBOSE_S << "2D sorted %d: ( %.3f  %.3f )\n", i, mProjectedPoints(0,i), mProjectedPoints(1,i));
        //}
        //ARMARX_VERBOSE_S << "\n\n");
 
        float fCenterX = 0, fCenterY = 0;
 
        // calculate the respective average of some points with low, medium and large x value,
        // then estimate the circle spanned by the three average points
        float xi = 0, yi = 0, xj = 0, yj = 0, xk = 0, yk = 0;
        //int nIndex;
        int nTwentyPercent = nNumberOfSamples / 5;
 
        //for (int i=0; i<nNumberOfSamples; i++)
        //{
        //  nIndex = (rand() % (nTwentyPercent));   // between 0 and 20 %
        //  xi += mProjectedPoints(0,nIndex);
        //  yi += mProjectedPoints(1,nIndex);
        //
        //  nIndex = 2*nTwentyPercent + (rand() % (nTwentyPercent));    // between 40 and 60 %
        //  xj += mProjectedPoints(0,nIndex);
        //  yj += mProjectedPoints(1,nIndex);
        //
        //  nIndex = 4*nTwentyPercent + (rand() % (nTwentyPercent))-1;  // between 80 and 100 %
        //  xk += mProjectedPoints(0,nIndex);
        //  yk += mProjectedPoints(1,nIndex);
        //}
        for (int i = 0; i < nTwentyPercent; i++) // between 0 and 20 %
        {
            xi += mProjectedPoints(0, i);
            yi += mProjectedPoints(1, i);
        }
 
        for (int i = 2 * nTwentyPercent; i < 3 * nTwentyPercent; i++) // between 40 and 60 %
        {
            xj += mProjectedPoints(0, i);
            yj += mProjectedPoints(1, i);
        }
 
        for (int i = 4 * nTwentyPercent; i < nNumberOfSamples; i++) // between 80 and 100 %
        {
            xk += mProjectedPoints(0, i);
            yk += mProjectedPoints(1, i);
        }
 
        const float fOneByNumUsedSamples = 1.0f / (float)nNumberOfSamples;
        xi *= fOneByNumUsedSamples;
        yi *= fOneByNumUsedSamples;
        xj *= fOneByNumUsedSamples;
        yj *= fOneByNumUsedSamples;
        xk *= fOneByNumUsedSamples;
        yk *= fOneByNumUsedSamples;
 
        // calculate the circumscribing circle of these three points
        const float fDelta1 = (xi * (yk - yj) + xj * (yi - yk) + xk * (yj - yi));
 
        //const float fDelta2 = (yi*(xk-xj)+yj*(xi-xk)+yk*(xj-xi)); //(xk-xj)*(yj-yi) - (xj-xi)*(yk-yi) <- wrong;
        //ARMARX_VERBOSE_S << "fDelta1: %.3f\n", fDelta1);
        //ARMARX_VERBOSE_S << "fDelta2: %.3f\n", fDelta2);
        //float fX, fY;
        if (fabs(fDelta1) > 0.001f)
        {
            fCenterX = 0.5f / fDelta1 *
                       ((yk - yj) * (xi * xi + yi * yi) + (yi - yk) * (xj * xj + yj * yj) +
                        (yj - yi) * (xk * xk + yk * yk));
            fCenterY = -0.5f / fDelta1 *
                       ((xk - xj) * (xi * xi + yi * yi) + (xi - xk) * (xj * xj + yj * yj) +
                        (xj - xi) * (xk * xk + yk * yk));
 
            //ARMARX_VERBOSE_S << "%.1f %.1f\n", xi, yi);
            //ARMARX_VERBOSE_S << "%.1f %.1f\n", xj, yj);
            //ARMARX_VERBOSE_S << "%.1f %.1f\n", xk, yk);
            //ARMARX_VERBOSE_S << "Center: ( %.1f %.1f )\n", fCenterX, fCenterY);
 
            //fARMARX_VERBOSE_S << m_pOutFile2, "%f %f\n", xi, yi);
            //fARMARX_VERBOSE_S << m_pOutFile2, "%f %f\n", xj, yj);
            //fARMARX_VERBOSE_S << m_pOutFile2, "%f %f\n", xk, yk);
        }
        else
        {
            return false; // TODO: find a better solution
        }
 
 
        /*
        int nNumberOfValidSampleCircles = 0;
        int nInd1a, nInd1b, nInd1c, nInd1d, nInd2a, nInd2b, nInd2c, nInd2d, nInd3a, nInd3b, nInd3c, nInd3d;
        //int nFivePercent = nNumberOfSamples/20;
        for (int i=0; (i<nNumberOfSamplesToUse) && (i<nNumberOfSamples); i++)
        {
            // choose three random points
            //nInd1a = 0*nFivePercent + (rand() % (3*nFivePercent));    // between 0 and 30 %
            //nInd1b = 0*nFivePercent + (rand() % (3*nFivePercent));
            //nInd1c = 0*nFivePercent + (rand() % (3*nFivePercent));
            //nInd1d = 0*nFivePercent + (rand() % (3*nFivePercent));
            //nInd2a = 8*nFivePercent + (rand() % (4*nFivePercent)); // between 35 and 65 %
            //nInd2b = 8*nFivePercent + (rand() % (4*nFivePercent));
            //nInd2c = 8*nFivePercent + (rand() % (4*nFivePercent));
            //nInd2d = 8*nFivePercent + (rand() % (4*nFivePercent));
            //nInd3a = 17*nFivePercent + (rand() % (3*nFivePercent)) - 1; // between 70 and 100 %
            //nInd3b = 17*nFivePercent + (rand() % (3*nFivePercent)) - 1;
            //nInd3c = 17*nFivePercent + (rand() % (3*nFivePercent)) - 1;
            //nInd3d = 17*nFivePercent + (rand() % (3*nFivePercent)) - 1;
            nInd1a = rand() % nNumberOfSamples;
            do { nInd2a = rand() % nNumberOfSamples; } while (nInd2a == nInd1a);
            do { nInd3a = rand() % nNumberOfSamples; } while ((nInd3a == nInd1a) || (nInd3a == nInd2a));
 
            //const float xi = 0.25f * (mProjectedPoints(0,nInd1a)+mProjectedPoints(0,nInd1b)+mProjectedPoints(0,nInd1c)+mProjectedPoints(0,nInd1d));
            //const float yi = 0.25f * (mProjectedPoints(1,nInd1a)+mProjectedPoints(1,nInd1b)+mProjectedPoints(1,nInd1c)+mProjectedPoints(1,nInd1d));
            //const float xj = 0.25f * (mProjectedPoints(0,nInd2a)+mProjectedPoints(0,nInd2b)+mProjectedPoints(0,nInd2c)+mProjectedPoints(0,nInd2d));
            //const float yj = 0.25f * (mProjectedPoints(1,nInd2a)+mProjectedPoints(1,nInd2b)+mProjectedPoints(1,nInd2c)+mProjectedPoints(1,nInd2d));
            //const float xk = 0.25f * (mProjectedPoints(0,nInd3a)+mProjectedPoints(0,nInd3b)+mProjectedPoints(0,nInd3c)+mProjectedPoints(0,nInd3d));
            //const float yk = 0.25f * (mProjectedPoints(1,nInd3a)+mProjectedPoints(1,nInd3b)+mProjectedPoints(1,nInd3c)+mProjectedPoints(1,nInd3d));
            const float xi = mProjectedPoints(0,nInd1a);
            const float yi = mProjectedPoints(1,nInd1a);
            const float xj = mProjectedPoints(0,nInd2a);
            const float yj = mProjectedPoints(1,nInd2a);
            const float xk = mProjectedPoints(0,nInd3a);
            const float yk = mProjectedPoints(1,nInd3a);
 
            // calculate the circumscribing circle of these three points and add its center to
            // the sum for averaging
            const float fDelta = (xi*(yk-yj)+xj*(yi-yk)+xk*(yj-yi)); //(xk-xj)*(yj-yi) - (xj-xi)*(yk-yi); <-wrong
            //ARMARX_VERBOSE_S << "fDelta1: %.3f\n", fDelta1);
            //ARMARX_VERBOSE_S << "fDelta2: %.3f\n", fDelta2);
            float fX, fY, fDist;
            if (fabs(fDelta)>0.001)
            {
                fX =   0.5f / fDelta * ( (yk-yj)*(xi*xi+yi*yi) + (yi-yk)*(xj*xj+yj*yj) + (yj-yi)*(xk*xk+yk*yk) );
                fY = - 0.5f / fDelta * ( (xk-xj)*(xi*xi+yi*yi) + (xi-xk)*(xj*xj+yj*yj) + (xj-xi)*(xk*xk+yk*yk) );
 
                fDist = (xi-fX)*(xi-fX) + (yi-fY)*(yi-fY);
                if ( fDist < 90000 )
                {
                    fCenterX += fX;
                    fCenterY += fY;
 
                    fARMARX_VERBOSE_S << m_pOutFile, "%f %f\n", xi, yi);
                    fARMARX_VERBOSE_S << m_pOutFile, "%f %f\n", xj, yj);
                    fARMARX_VERBOSE_S << m_pOutFile, "%f %f\n", xk, yk);
                    fARMARX_VERBOSE_S << m_pOutFile2, "%f %f\n", fX, fY);
                    nNumberOfValidSampleCircles++;
                }
            }
        }
 
        fCenterX /= (float)nNumberOfValidSampleCircles;
        fCenterY /= (float)nNumberOfValidSampleCircles;
        */
 
 
        // calculate 3D position of the center
 
        Vec3d vCenter2D = {0.0f, fCenterX, fCenterY};
 
 
        Mat3d mInverseTransformation;
        Math3d::Invert(mTransformation, mInverseTransformation);
        Math3d::MulMatVec(mInverseTransformation, vCenter2D, vCylinderCenter);
 
        vCylinderCenter.x += vAvgPoint.x;
        vCylinderCenter.y += vAvgPoint.y;
        vCylinderCenter.z += vAvgPoint.z;
 
 
        // calculate optimal radius
        fCylinderRadius = 0;
 
        for (int i = 0; i < nNumberOfSamples; i++)
        {
            Math3d::SubtractVecVec(avSamplePoints[i], vCylinderCenter, vTemp1);
            Math3d::CrossProduct(vCylinderAxis, vTemp1, vTemp2);
            fCylinderRadius += Math3d::Length(vTemp2);
        }
 
        fCylinderRadius /= (float)nNumberOfSamples;
 
 
        //ARMARX_VERBOSE_S << "\nCylinder center: ( %.1f  %.1f  %.1f )\n", vCylinderCenter.x, vCylinderCenter.y, vCylinderCenter.z);
        //ARMARX_VERBOSE_S << "Cylinder axis: ( %.3f  %.3f  %.3f )\n", vCylinderAxis.x, vCylinderAxis.y, vCylinderAxis.z);
        //ARMARX_VERBOSE_S << "Cylinder radius: %.2f\n\n", fCylinderRadius);
 
        //fARMARX_VERBOSE_S << m_pOutFile3, "%f %f\n", fCenterX, fCenterY);
 
        // if the radius is realistic
        if (fCylinderRadius < fMaxRadius)
        {
 
            // calculate the plane spanned by the cylinder axis and the vector that is
            // orthogonal to the cylinder axis and the z-axis
            Vec3d vPlaneNormal;
            Math3d::CrossProduct(vCylinderAxis, vZAxis, vTemp1);
            Math3d::CrossProduct(vCylinderAxis, vTemp1, vPlaneNormal);
            const float fD = -Math3d::ScalarProduct(vPlaneNormal, vCylinderCenter);
            float fTemp;
 
            // check how many points belong to the cylinder surface
            float fDist;
            int nNumberOfPointsOnCylinder = 0;
 
            for (int i = 0; i < nNumberOfAllPoints; i++)
            {
                // accept only points that are on the side of the cylinder that is turned towards
                // the camera. If those points are put into the plane equation, the result has the
                // same sign as the result for (0,0,0), which is = fD
                fTemp = Math3d::ScalarProduct(vPlaneNormal, avAllPoints[i]) + fD;
 
                if (fTemp * fD > 0)
                {
                    // accept only points within a tolerance margin around the cylinder surface
                    Math3d::SubtractVecVec(avAllPoints[i], vCylinderCenter, vTemp1);
                    Math3d::CrossProduct(vCylinderAxis, vTemp1, vTemp2);
                    fDist = Math3d::Length(vTemp2);
                    //ARMARX_VERBOSE_S << "%.2f\n", (fDist-fCylinderRadius));
 
                    if (fabsf(fDist - fCylinderRadius) <= fToleranceThreshold)
                    {
                        nNumberOfPointsOnCylinder++;
                    }
                }
            }
 
            if (nNumberOfPointsOnCylinder > nMaxNumberOfPointsOnCylinder)
            {
                nMaxNumberOfPointsOnCylinder = nNumberOfPointsOnCylinder;
 
                Math3d::SetVec(vBestCylinderCenter, vCylinderCenter);
                Math3d::SetVec(vBestCylinderAxis, vCylinderAxis);
                fBestCylinderRadius = fCylinderRadius;
            }
 
            //ARMARX_VERBOSE_S << "Points on the cylinder: %d\n\n", nNumberOfPointsOnCylinder);
        }
    }
 
    // if no cylinder seems realistic return false
    if (nMaxNumberOfPointsOnCylinder == 0)
    {
        return false;
    }
    else // return best cylinder
    {
        Math3d::SetVec(vCylinderCenter, vBestCylinderCenter);
        Math3d::SetVec(vCylinderAxis, vBestCylinderAxis);
        fCylinderRadius = fBestCylinderRadius;
 
        // add points belonging to the best cylinder to the result list
        float fDist;
 
        for (int i = 0; i < nNumberOfAllPoints; i++)
        {
            Math3d::SubtractVecVec(avAllPoints[i], vBestCylinderCenter, vTemp1);
            Math3d::CrossProduct(vBestCylinderAxis, vTemp1, vTemp2);
            fDist = Math3d::Length(vTemp2);
 
            //ARMARX_VERBOSE_S << "%.2f\n", (fDist-fCylinderRadius));
 
            if (fabsf(fDist - fBestCylinderRadius) <= fToleranceThreshold)
            {
                avResultPoints.AddElement(avAllPoints[i]);
            }
        }
 
        return true;
    }
}
 
void
CHypothesisGeneration::QuickSort2DPointsX(CFloatMatrix& mPoints, int nLeft, int nRight)
{
    int nPivotPos, i, j;
    float fPivotElemX, fTempElemX, fTempElemY;
 
    while (nRight - nLeft >= 16)
    {
        //ARMARX_VERBOSE_S << "l: %d   r: %d\n", nLeft, nRight);
        // pick pivot
        nPivotPos = nLeft + rand() % (nRight - nLeft);
 
        // swap of a[left], a[pivot] helps to establish the invariant (?)
        fTempElemX = mPoints(0, nLeft);
        fTempElemY = mPoints(1, nLeft);
        mPoints(0, nLeft) = mPoints(0, nPivotPos);
        mPoints(1, nLeft) = mPoints(1, nPivotPos);
        mPoints(0, nPivotPos) = fTempElemX;
        mPoints(1, nPivotPos) = fTempElemY;
 
        // i runs from left, j from right, until they meet. when i is at an element that
        // is larger then the pivot and j is at an element that is smaller then the pivot,
        // those two elements are exchanged.
        fPivotElemX = mPoints(0, nLeft);
        i = nLeft;
        j = nRight;
 
        do
        {
            while (mPoints(0, i) < fPivotElemX)
            {
                i++;
            }
 
            while (mPoints(0, j) > fPivotElemX)
            {
                j--;
            }
 
            if (i < j)
            {
                // swap elements a[i], a[j]
                fTempElemX = mPoints(0, i);
                fTempElemY = mPoints(1, i);
                mPoints(0, i) = mPoints(0, j);
                mPoints(1, i) = mPoints(1, j);
                mPoints(0, j) = fTempElemX;
                mPoints(1, j) = fTempElemY;
 
                i++;
                j--;
            }
        } while (i < j);
 
        if (i < (nLeft + nRight) / 2)
        {
            QuickSort2DPointsX(mPoints, nLeft, j);
            nLeft = j;
        }
        else
        {
            QuickSort2DPointsX(mPoints, i, nRight);
            nRight = i;
        }
    }
 
    if (nLeft < nRight)
    {
        // insertion sort for small parts
        for (i = nLeft + 1; i <= nRight; i++)
        {
            fTempElemX = mPoints(0, i);
            fTempElemY = mPoints(1, i);
            j = i;
 
            while ((j > nLeft) && (mPoints(0, j - 1) > fTempElemX))
            {
                mPoints(0, j) = mPoints(0, j - 1);
                mPoints(1, j) = mPoints(1, j - 1);
                j--;
            }
 
            mPoints(0, j) = fTempElemX;
            mPoints(1, j) = fTempElemY;
        }
    }
}
 
void
CHypothesisGeneration::ClusterPointsRegularGrid2D(const CVec3dArray& avPoints,
                                                  const CCalibration* calibration,
                                                  const int nNumSectionsX,
                                                  const int nNumSectionsY,
                                                  CVec3dArray*& pClusters,
                                                  int& nNumClusters)
{
    nNumClusters = nNumSectionsX * nNumSectionsY + (nNumSectionsX + 1) * (nNumSectionsY + 1);
    int nShiftOffset = nNumSectionsX * nNumSectionsY;
    pClusters = new CVec3dArray[nNumClusters];
    //for (int i=0; i<nNumClusters; i++) pClusters[i].Clear();
 
    const int nNumPoints = avPoints.GetSize();
    Vec2d vPoint2D;
    int nIndex, nIndexShifted;
    int nIntervalX = OLP_IMG_WIDTH / nNumSectionsX;
    int nIntervalY = OLP_IMG_HEIGHT / nNumSectionsY;
    int nIntervalX2 = OLP_IMG_WIDTH / (2 * nNumSectionsX);
    int nIntervalY2 = OLP_IMG_HEIGHT / (2 * nNumSectionsY);
 
    for (int i = 0; i < nNumPoints; i++)
    {
        calibration->CameraToImageCoordinates(avPoints[i], vPoint2D, false);
 
        nIndex = nNumSectionsX * ((int)vPoint2D.y / nIntervalY) + (int)vPoint2D.x / nIntervalX;
 
        nIndexShifted = (nNumSectionsX + 1) * (((int)vPoint2D.y + nIntervalY2) / nIntervalY) +
                        ((int)vPoint2D.x + nIntervalX2) / nIntervalX;
 
        //ARMARX_VERBOSE_S << "Point: %.2f %.2f %.2f\n", avPoints[i].x, avPoints[i].y, avPoints[i].z);
 
        if (nIndex >= 0)
        {
            if (nIndex + nIndex < nNumClusters)
            {
                pClusters[nIndex].AddElement(avPoints[i]);
            }
 
            if (nShiftOffset + nIndexShifted < nNumClusters)
            {
                pClusters[nShiftOffset + nIndexShifted].AddElement(avPoints[i]);
            }
        }
    }
 
    /*
    ARMARX_VERBOSE_S << "\nCluster sizes:\n");
    int n=8;
    for (int i=0; i<n; i++)
    {
        for (int j=0; j<n; j++)
        {
            ARMARX_VERBOSE_S << "%d  ", pClusters[n*i+j].GetSize());
        }
        ARMARX_VERBOSE_S << "\n");
    }
    ARMARX_VERBOSE_S << "\n");
 
    n = 4;
    for (int i=0; i<n; i++)
    {
        for (int j=0; j<n; j++)
        {
            ARMARX_VERBOSE_S << "%d  ", pClusters[nNumSectionsSmall*nNumSectionsSmall + n*i+j].GetSize());
        }
        ARMARX_VERBOSE_S << "\n");
    }
    ARMARX_VERBOSE_S << "\n");
    */
}
 
/*
bool CHypothesisGeneration::RANSACCylinders(const CVec3dArray& avPoints, const CCalibration* calibration,
                                                const float fToleranceThreshold, const float fMaxRadius, CVec3dArray& avCylinder)
{
    // debug
    int nOrigin;
 
    // create a regular grid of clusters at several scales
    const int nNumScales = 7;
    CVec3dArray* ppRegularGridClusters[nNumScales];
    int pnNumGridClusters[nNumScales];
 
    ClusterPointsRegularGrid2D(avPoints, calibration, 16, 12, ppRegularGridClusters[0], pnNumGridClusters[0]);
    ClusterPointsRegularGrid2D(avPoints, calibration, 12, 9, ppRegularGridClusters[1], pnNumGridClusters[1]);
    ClusterPointsRegularGrid2D(avPoints, calibration, 8, 6, ppRegularGridClusters[2], pnNumGridClusters[2]);
    ClusterPointsRegularGrid2D(avPoints, calibration, 12, 6, ppRegularGridClusters[3], pnNumGridClusters[3]);
    ClusterPointsRegularGrid2D(avPoints, calibration, 12, 12, ppRegularGridClusters[4], pnNumGridClusters[4]);
    ClusterPointsRegularGrid2D(avPoints, calibration, 10, 8, ppRegularGridClusters[5], pnNumGridClusters[5]);
    ClusterPointsRegularGrid2D(avPoints, calibration, 9, 7, ppRegularGridClusters[6], pnNumGridClusters[6]);
 
    int nNumGridClusters = 0;
    for (int i=0; i<nNumScales; i++) nNumGridClusters += pnNumGridClusters[i];
 
    CVec3dArray* pRegularGridClusters = new CVec3dArray[nNumGridClusters];
    int nOffset = 0;
    for (int i=0; i<nNumScales; i++)
    {
        for (int j=0; j<pnNumGridClusters[i]; j++) pRegularGridClusters[nOffset+j] = ppRegularGridClusters[i][j];
        nOffset += pnNumGridClusters[i];
    }
 
    // for every cluster: try to find a cylinder and count the support for it in the whole image
    CVec3dArray avCylinderPoints;
    Vec3d vCylinderAxis, vCylinderCenter, vBestCylinderAxis, vBestCylinderCenter;
    float fCylinderRadius, fBestCylinderRadius;
    int nMaxSupport = 0;
    bool bFoundCylinder;
    const int nOverallNumberOfPoints = avPoints.GetSize();
    CVec3dArray avSample;
    const int nPreferredSampleSize = 20;
    for (int i=0; i<nNumGridClusters; i++)
    {
 
        int nIterationsOnThisCluster = 10 * pRegularGridClusters[i].GetSize() / (2*nPreferredSampleSize) + 1;
 
        for (int j=0; j<nIterationsOnThisCluster; j++)
        {
            if (j==0)
            {
                for (int k=0; k<pRegularGridClusters[i].GetSize(); k++) avSample.AddElement(pRegularGridClusters[i][k]);
                RemoveOutliers(pRegularGridClusters[i], 1.5f, NULL);
            }
            else
            {
                // select a random sample from the cluster
 
                int nSampleSize;
                if (pRegularGridClusters[i].GetSize() > 2*nPreferredSampleSize) nSampleSize = nPreferredSampleSize;
                else if (pRegularGridClusters[i].GetSize() > 10) nSampleSize = (pRegularGridClusters[i].GetSize()/2);
                else if (pRegularGridClusters[i].GetSize() >= 5) nSampleSize = 5;
                else continue;
 
                int nRandomIndex;
                //if (pRegularGridClusters[i].GetSize() < 30)
                {
                    for (int k=0; k<pRegularGridClusters[i].GetSize(); k++) avSample.AddElement(pRegularGridClusters[i][k]);
                    for (int n = pRegularGridClusters[i].GetSize(); n > nSampleSize; n--)
                    {
                        nRandomIndex = rand() % n;
                        avSample.DeleteElement(nRandomIndex);
                    }
                }
            }
 
 
            bFoundCylinder = FindCylinder(avSample, avPoints, fToleranceThreshold, fMaxRadius, avCylinderPoints, vCylinderAxis, vCylinderCenter, fCylinderRadius);
 
            avSample.Clear();
 
            if (bFoundCylinder)
            {
                int nNumberOfPointsOnCylinder = avCylinderPoints.GetSize();//0;
                //float fDist;
                //Vec3d vTemp1, vTemp2;
                //for (int k=0; k<nOverallNumberOfPoints; k++)
                //{
                //  Math3d::SubtractVecVec(avPoints[k], vCylinderCenter, vTemp1);
                //  Math3d::CrossProduct(vCylinderAxis, vTemp1, vTemp2);
                //  fDist = Math3d::Length(vTemp2);
 
                //  if (fabsf(fDist-fCylinderRadius) <= fToleranceThreshold)
                //  {
                //      nNumberOfPointsOnCylinder++;
                //  }
                //}
 
                if (nNumberOfPointsOnCylinder > nMaxSupport)
                {
                    nMaxSupport = nNumberOfPointsOnCylinder;
 
                    Math3d::SetVec(vBestCylinderCenter, vCylinderCenter);
                    Math3d::SetVec(vBestCylinderAxis, vCylinderAxis);
                    fBestCylinderRadius = fCylinderRadius;
 
                    if (i<pnNumGridClusters[0]) nOrigin = 0;
                    else if (i<pnNumGridClusters[0]+pnNumGridClusters[1]) nOrigin = 1;
                    else if (i<pnNumGridClusters[0]+pnNumGridClusters[1]+pnNumGridClusters[2]) nOrigin = 2;
                    else if (i<pnNumGridClusters[0]+pnNumGridClusters[1]+pnNumGridClusters[2]+pnNumGridClusters[3]) nOrigin = 3;
                    else if (i<pnNumGridClusters[0]+pnNumGridClusters[1]+pnNumGridClusters[2]+pnNumGridClusters[3]+pnNumGridClusters[4]) nOrigin = 4;
                    else if (i<pnNumGridClusters[0]+pnNumGridClusters[1]+pnNumGridClusters[2]+pnNumGridClusters[3]+pnNumGridClusters[4]+pnNumGridClusters[5]) nOrigin = 5;
                    else nOrigin = 42;
                }
            }
        }
    }
 
    avCylinder.Clear();
 
 
    if (nMaxSupport==0)
    {
        // if no cylinder was found, return false
        return false;
    }
    else
    {
        // return points belonging to the best cylinder
        float fDist;
        Vec3d vTemp1, vTemp2;
        for (int j=0; j<nOverallNumberOfPoints; j++)
        {
            Math3d::SubtractVecVec(avPoints[j], vBestCylinderCenter, vTemp1);
            Math3d::CrossProduct(vBestCylinderAxis, vTemp1, vTemp2);
            fDist = Math3d::Length(vTemp2);
 
            if (fabsf(fDist-fBestCylinderRadius) <= fToleranceThreshold)
            {
                avCylinder.AddElement(avPoints[j]);
            }
        }
 
        ARMARX_VERBOSE_S << "\nAxis: ( %.2f  %.2f  %.2f )\nRadius: %.1f   Origin: %d\n", vBestCylinderAxis.x, vBestCylinderAxis.y, vBestCylinderAxis.z, fBestCylinderRadius, nOrigin);
 
        return true;
    }
}
*/
 
 
bool
CHypothesisGeneration::RANSACCylinders2(const CVec3dArray& avPoints,
                                        const CCalibration* calibration,
                                        const float fToleranceThreshold,
                                        const float fMaxRadius,
                                        CVec3dArray& avCylinder,
                                        Vec3d& vCylinderAxis,
                                        Vec3d& vCylinderCenter,
                                        float& fCylinderRadius)
{
    avCylinder.Clear();
    const int nMaxAxisIterations = (int)(OLP_EFFORT_MODIFICATOR * 8 + 15); // *10 + 30
    CVec3dArray avAxisBlacklist(nMaxAxisIterations);
    bool bBreakIfNextAxisIsBad = false;
    Vec3d vBestCylinderAxis, vBestCylinderCenter;
    float fBestRadius = 0;
 
    //  timeval tStart, tEnd;
    //  long tTimeDiff;
 
 
    // step 1: calculate local plane estimates and their surface normals to create the gauss map
 
    //gettimeofday(&tStart, 0);
 
    // segment the points into a regular grid to speed up nearest neighbour search
    int nNumGridClusters;
    //CVec3dArray* pRegularGridClusters;
    //ClusterPointsRegularGrid2D(avPoints, calibration, 8, 6, pRegularGridClusters, nNumGridClusters);  // 8, 6
    nNumGridClusters = 1;
    const CVec3dArray* pRegularGridClusters = &avPoints;
 
    // for each point: find its 2 nearest neighbours, create a plane through these 3 points, calculate its normal
    // and add it to the gauss map
    const int nOverallNumberOfPoints = avPoints.GetSize();
    CVec3dArray avGaussMap(nOverallNumberOfPoints);
    CVec3dArray avCorrespondingPoints(nOverallNumberOfPoints);
    int nMaxSupport = 0;
 
 
    for (int i = 0; i < nNumGridClusters; i++)
    {
        const int nClusterSize = pRegularGridClusters[i].GetSize();
 
        if (nClusterSize >= 5)
        {
#pragma omp parallel
            {
                CFloatMatrix mSampleMatrix(3, 5);
                CFloatMatrix mU(5, 5);
                CFloatMatrix mSigma(3, 5);
                CFloatMatrix mV(3, 3);
                Vec3d vPoint, vNeighbour1, vNeighbour2, vNeighbour3, vNeighbour4, vNormal, vMean,
                    vTemp1, vTemp2;
                float fMinDist1, fMinDist2, fMinDist3, fMinDist4, fDist;
 
#pragma omp for schedule(static, 10)
                for (int j = 0; j < nClusterSize; j++)
                {
                    // select a point and find its 2 closest neighbours
                    Math3d::SetVec(vPoint, pRegularGridClusters[i][j]);
                    fMinDist1 = 1000000;
                    fMinDist2 = 1000000;
                    fMinDist3 = 1000000;
                    fMinDist4 = 1000000;
 
                    for (int k = 0; k < nClusterSize; k++)
                    {
                        if (k != j)
                        {
                            fDist = Math3d::Distance(vPoint, pRegularGridClusters[i][k]);
 
                            if (fDist < fMinDist1)
                            {
                                fMinDist4 = fMinDist3;
                                Math3d::SetVec(vNeighbour4, vNeighbour3);
                                fMinDist3 = fMinDist2;
                                Math3d::SetVec(vNeighbour3, vNeighbour2);
                                fMinDist2 = fMinDist1;
                                Math3d::SetVec(vNeighbour2, vNeighbour1);
                                fMinDist1 = fDist;
                                Math3d::SetVec(vNeighbour1, pRegularGridClusters[i][k]);
                            }
                            else if (fDist < fMinDist2)
                            {
                                fMinDist4 = fMinDist3;
                                Math3d::SetVec(vNeighbour4, vNeighbour3);
                                fMinDist3 = fMinDist2;
                                Math3d::SetVec(vNeighbour3, vNeighbour2);
                                fMinDist2 = fDist;
                                Math3d::SetVec(vNeighbour2, pRegularGridClusters[i][k]);
                            }
                            else if (fDist < fMinDist3)
                            {
                                fMinDist4 = fMinDist3;
                                Math3d::SetVec(vNeighbour4, vNeighbour3);
                                fMinDist3 = fDist;
                                Math3d::SetVec(vNeighbour3, pRegularGridClusters[i][k]);
                            }
                            else if (fDist < fMinDist4)
                            {
                                fMinDist4 = fDist;
                                Math3d::SetVec(vNeighbour4, pRegularGridClusters[i][k]);
                            }
                        }
                    }
 
                    // calculate the normal of these three points and add it to the gauss map
                    /*Math3d::SubtractVecVec(vNeighbour1, vPoint, vTemp1);
                    Math3d::SubtractVecVec(vNeighbour2, vPoint, vTemp2);
                    Math3d::CrossProduct(vTemp1, vTemp2, vNormal);
                    Math3d::NormalizeVec(vNormal);*/
 
                    //if ( (fMinDist4 > 30.0f) && (fMinDist2 < 25.0f) )
                    if (fMinDist2 < 25.0f)
                    {
                        // calculate the normal of the three closest points and add it to the gauss map
                        Math3d::SubtractVecVec(vNeighbour1, vPoint, vTemp1);
                        Math3d::SubtractVecVec(vNeighbour2, vPoint, vTemp2);
                        Math3d::CrossProduct(vTemp1, vTemp2, vNormal);
                        Math3d::NormalizeVec(vNormal);
 
                        if (Math3d::Length(vNormal) > 0.9f)
                        {
#pragma omp critical
                            {
                                avGaussMap.AddElement(vNormal);
                                avCorrespondingPoints.AddElement(vPoint);
                            }
                        }
                    }
 
                    if (fMinDist4 < 50.0f)
                    {
                        vMean.x = 0.2f * (vPoint.x + vNeighbour1.x + vNeighbour2.x + vNeighbour3.x +
                                          vNeighbour4.x);
                        vMean.y = 0.2f * (vPoint.y + vNeighbour1.y + vNeighbour2.y + vNeighbour3.y +
                                          vNeighbour4.y);
                        vMean.z = 0.2f * (vPoint.z + vNeighbour1.z + vNeighbour2.z + vNeighbour3.z +
                                          vNeighbour4.z);
 
                        mSampleMatrix(0, 0) = vPoint.x - vMean.x;
                        mSampleMatrix(1, 0) = vPoint.y - vMean.y;
                        mSampleMatrix(2, 0) = vPoint.z - vMean.z;
                        mSampleMatrix(0, 1) = vNeighbour1.x - vMean.x;
                        mSampleMatrix(1, 1) = vNeighbour1.y - vMean.y;
                        mSampleMatrix(2, 1) = vNeighbour1.z - vMean.z;
                        mSampleMatrix(0, 2) = vNeighbour2.x - vMean.x;
                        mSampleMatrix(1, 2) = vNeighbour2.y - vMean.y;
                        mSampleMatrix(2, 2) = vNeighbour2.z - vMean.z;
                        mSampleMatrix(0, 3) = vNeighbour3.x - vMean.x;
                        mSampleMatrix(1, 3) = vNeighbour3.y - vMean.y;
                        mSampleMatrix(2, 3) = vNeighbour3.z - vMean.z;
                        mSampleMatrix(0, 4) = vNeighbour4.x - vMean.x;
                        mSampleMatrix(1, 4) = vNeighbour4.y - vMean.y;
                        mSampleMatrix(2, 4) = vNeighbour4.z - vMean.z;
 
                        LinearAlgebra::SVD(&mSampleMatrix, &mSigma, &mU, &mV);
 
                        vNormal.x = mV(2, 0);
                        vNormal.y = mV(2, 1);
                        vNormal.z = mV(2, 2);
                        Math3d::NormalizeVec(vNormal);
 
                        if (Math3d::Length(vNormal) < 0.9f)
                        {
                            //ARMARX_VERBOSE_S << "\n\np0: ( %f  %f  %f ) p1: ( %f  %f  %f ) p2: ( %f  %f  %f ) \nnormal: ( %f  %f  %f )\n\n\n",
                            //      vPoint.x, vPoint.y, vPoint.z, vNeighbour1.x, vNeighbour1.y, vNeighbour1.z,
                            //      vNeighbour2.x, vNeighbour2.y, vNeighbour2.z, vNormal.x, vNormal.y, vNormal.z);
                            continue;
                        }
 
#pragma omp critical
                        {
                            avGaussMap.AddElement(vNormal);
                            avCorrespondingPoints.AddElement(vPoint);
                        }
                    }
                }
            }
            //else if (nClusterSize == 4)
            //{
            //  // get the three points that are closest to the mean
            //  vMean.x = 0.25f * (pRegularGridClusters[i][0].x+pRegularGridClusters[i][1].x+pRegularGridClusters[i][2].x+pRegularGridClusters[i][3].x);
            //  vMean.y = 0.25f * (pRegularGridClusters[i][0].y+pRegularGridClusters[i][1].y+pRegularGridClusters[i][2].y+pRegularGridClusters[i][3].y);
            //  vMean.z = 0.25f * (pRegularGridClusters[i][0].z+pRegularGridClusters[i][1].z+pRegularGridClusters[i][2].z+pRegularGridClusters[i][3].z);
 
            //  if (Math3d::Distance(vMean, pRegularGridClusters[i][0]) < Math3d::Distance(vMean, pRegularGridClusters[i][1]))
            //  {
            //      if (Math3d::Distance(vMean, pRegularGridClusters[i][2]) < Math3d::Distance(vMean, pRegularGridClusters[i][3]))
            //      {
            //          Math3d::SetVec(vPoint, pRegularGridClusters[i][0]);
            //          Math3d::SetVec(vNeighbour1, pRegularGridClusters[i][2]);
 
            //          if (Math3d::Distance(vMean, pRegularGridClusters[i][1]) < Math3d::Distance(vMean, pRegularGridClusters[i][3]))
            //          {
            //              Math3d::SetVec(vNeighbour2, pRegularGridClusters[i][1]);
            //          }
            //          else
            //          {
            //              Math3d::SetVec(vNeighbour2, pRegularGridClusters[i][3]);
            //          }
            //      }
            //      else    // 3 < 2
            //      {
            //          Math3d::SetVec(vPoint, pRegularGridClusters[i][0]);
            //          Math3d::SetVec(vNeighbour1, pRegularGridClusters[i][3]);
 
            //          if (Math3d::Distance(vMean, pRegularGridClusters[i][1]) < Math3d::Distance(vMean, pRegularGridClusters[i][2]))
            //          {
            //              Math3d::SetVec(vNeighbour2, pRegularGridClusters[i][1]);
            //          }
            //          else
            //          {
            //              Math3d::SetVec(vNeighbour2, pRegularGridClusters[i][2]);
            //          }
            //      }
            //  }
            //  else    // 1 < 0
            //  {
            //      if (Math3d::Distance(vMean, pRegularGridClusters[i][2]) < Math3d::Distance(vMean, pRegularGridClusters[i][3]))
            //      {
            //          Math3d::SetVec(vPoint, pRegularGridClusters[i][1]);
            //          Math3d::SetVec(vNeighbour1, pRegularGridClusters[i][2]);
 
            //          if (Math3d::Distance(vMean, pRegularGridClusters[i][0]) < Math3d::Distance(vMean, pRegularGridClusters[i][3]))
            //          {
            //              Math3d::SetVec(vNeighbour2, pRegularGridClusters[i][0]);
            //          }
            //          else
            //          {
            //              Math3d::SetVec(vNeighbour2, pRegularGridClusters[i][3]);
            //          }
            //      }
            //      else    // 3 < 2
            //      {
            //          Math3d::SetVec(vPoint, pRegularGridClusters[i][1]);
            //          Math3d::SetVec(vNeighbour1, pRegularGridClusters[i][3]);
 
            //          if (Math3d::Distance(vMean, pRegularGridClusters[i][0]) < Math3d::Distance(vMean, pRegularGridClusters[i][2]))
            //          {
            //              Math3d::SetVec(vNeighbour2, pRegularGridClusters[i][0]);
            //          }
            //          else
            //          {
            //              Math3d::SetVec(vNeighbour2, pRegularGridClusters[i][2]);
            //          }
            //      }
            //  }
 
            //  // calculate the normal of these three points and add it to the gauss map
            //  Math3d::SubtractVecVec(vNeighbour1, vPoint, vTemp1);
            //  Math3d::SubtractVecVec(vNeighbour2, vPoint, vTemp2);
            //  Math3d::CrossProduct(vTemp1, vTemp2, vNormal);
            //  Math3d::NormalizeVec(vNormal);
            //  avGaussMap.AddElement(vNormal);
            //  avCorrespondingPoints.AddElement(vPoint);
            //}
            //else if (nClusterSize == 3)
            //{
            //  // calculate the normal of these three points and add it to the gauss map
            //  Math3d::SubtractVecVec(pRegularGridClusters[i][1], pRegularGridClusters[i][0], vTemp1);
            //  Math3d::SubtractVecVec(pRegularGridClusters[i][2], pRegularGridClusters[i][0], vTemp2);
            //  Math3d::CrossProduct(vTemp1, vTemp2, vNormal);
            //  Math3d::NormalizeVec(vNormal);
            //  avGaussMap.AddElement(vNormal);
            //  avCorrespondingPoints.AddElement(pRegularGridClusters[i][0]);
            //}
        }
    }
 
    //delete[] pRegularGridClusters;
 
    if (avGaussMap.GetSize() < OLP_MIN_NUM_FEATURES)
    {
        //ARMARX_VERBOSE_S << "\n(avGaussMap.GetSize() < 10)\n\n");
        return false;
    }
 
 
    //gettimeofday(&tEnd, 0);
    //tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
    //ARMARX_VERBOSE_S << "Time for Gauss Map: %ld ms\n", tTimeDiff);
 
    //// debug
    //if (m_bTest) // && (m_nNumHypotheses>1))
    //{
    //for (int i=0; i<avGaussMap.GetSize(); i++)
    //{
    //avCylinder.AddElement(avGaussMap[i]);
    //avCylinder[2*i].x *= -100;
    //avCylinder[2*i].y *= -100;
    //avCylinder[2*i].z *= -100;
 
    //avCylinder.AddElement(avGaussMap[i]);
    //avCylinder[2*i+1].x *= 100;
    //avCylinder[2*i+1].y *= 100;
    //avCylinder[2*i+1].z *= 100;
 
    //avCylinder[2*i].z += 500;
    //avCylinder[2*i+1].z += 500;
 
    ////avCylinder.AddElement(avGaussMap[i]);
    ////if (avCylinder[i].z > 0)
    ////{
    ////avCylinder[i].x *= -100;
    ////avCylinder[i].y *= -100;
    ////avCylinder[i].z *= -100;
    ////}
    ////else
    ////{
    ////avCylinder[i].x *= 100;
    ////avCylinder[i].y *= 100;
    ////avCylinder[i].z *= 100;
    ////}
 
 
    ////avCylinder[i].x -= 150;
    ////avCylinder[i].y -= 100;
    ////avCylinder[i].z += 500;
    //}
    //m_bTest = false;
 
    //return true;
    //}
 
 
    // repeat nMaxAxisIterations and return the best found cylinder
    for (int n = 0; n < nMaxAxisIterations; n++)
    {
 
        // step 2: find a large circle on the gauss map (which is a sphere). this is equivalent to the
        // intersection of this sphere with a plane defined by (0,0,0) and two points on the sphere.
        // The resulting circle gives the normal of the cylinder that caused the circle in the gauss map.
 
        //gettimeofday(&tStart, 0);
 
        // use RANSAC to find the best plane/circle on the gauss map
        const int nGaussCircleIterations =
            (2 * avGaussMap.GetSize() > (int)(OLP_EFFORT_MODIFICATOR * 1000))
                ? (int)(OLP_EFFORT_MODIFICATOR * 1000)
                : 2 * avGaussMap.GetSize();
        const int nNumberOfPointsInGaussMap = avGaussMap.GetSize();
        const float fGaussMapThreshold = 0.15f;
        const float fBadAxisFactor = 0.1f; // 0.15
        int nMaxAxisSupport = 0;
        Vec3d vBestNormal;
 
#pragma omp parallel
        {
            int nFirstIndex, nSecondIndex, nSupport;
            Vec3d vPoint1, vPoint2, vNormal;
            bool bCloseToBadAxis;
 
#pragma omp for schedule(static, 16)
            for (int i = 0; i < nGaussCircleIterations; i++)
            {
                // identify 2 different random points
                srand((rand() % 17) * i + i);
                nFirstIndex = rand() % nNumberOfPointsInGaussMap;
 
                do
                {
                    nSecondIndex = rand() % nNumberOfPointsInGaussMap;
                } while (nSecondIndex == nFirstIndex);
 
                vPoint1 = avGaussMap[nFirstIndex];
                vPoint2 = avGaussMap[nSecondIndex];
 
                // calculate plane defined by them and (0,0,0)
                // easy because one point is (0,0,0) => p1-p0 = p1 etc, plane parameter d=0
                Math3d::CrossProduct(vPoint1, vPoint2, vNormal);
                Math3d::NormalizeVec(vNormal);
 
                // make sure normal is not (0,0,0). that may happen when p1=p2.
                if (Math3d::ScalarProduct(vNormal, vNormal) > 0.9f)
                {
                    // check if it is close to an axis that led to a bad cylinder
                    bCloseToBadAxis = false;
 
                    for (int j = 0; j < avAxisBlacklist.GetSize(); j++)
                    {
                        bCloseToBadAxis |=
                            (fabsf(Math3d::ScalarProduct(vNormal, avAxisBlacklist[j])) >
                             1.0f - fBadAxisFactor * fGaussMapThreshold); // 0.2
                    }
 
                    if (!bCloseToBadAxis)
                    {
                        // count support
                        nSupport = 0;
 
                        for (int j = 0; j < nNumberOfPointsInGaussMap; j++)
                        {
                            if (fabsf(Math3d::ScalarProduct(vNormal, avGaussMap[j])) <
                                fGaussMapThreshold)
                            {
                                nSupport++;
                            }
                        }
 
                        // store if it is the current maximum
                        if (nSupport > nMaxAxisSupport)
                        {
#pragma omp critical
                            if (nSupport > nMaxAxisSupport)
                            {
                                nMaxAxisSupport = nSupport;
                                Math3d::SetVec(vBestNormal, vNormal);
                            }
                        }
                    }
                }
            }
        }
 
        //gettimeofday(&tEnd, 0);
        //tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
        //ARMARX_VERBOSE_S << "Time for Gauss Map circle: %ld ms\n", tTimeDiff);
 
 
        if (nMaxAxisSupport < OLP_MIN_NUM_FEATURES)
        {
            //ARMARX_VERBOSE_S << "%d\n", n);
            if (bBreakIfNextAxisIsBad)
            {
                //ARMARX_VERBOSE_S << "no axis found after %d iterations\n", n);
                break;
                //continue;
            }
            else if (2 * n > nMaxAxisIterations)
            {
                bBreakIfNextAxisIsBad = true;
                //ARMARX_VERBOSE_S << "c ");
                avAxisBlacklist.AddElement(vBestNormal);
                continue;
            }
            else
            {
                //ARMARX_VERBOSE_S << "c ");
                avAxisBlacklist.AddElement(vBestNormal);
                continue;
            }
        }
 
        //if (Math3d::Length(vBestNormal) < 0.9f)
        //{
        //  ARMARX_VERBOSE_S << "\n\n\n     ---  bad axis  ---\n\n\n");
        //}
 
        //ARMARX_VERBOSE_S << "Best normal: ( %f  %f  %f )\n", vBestNormal.x, vBestNormal.y, vBestNormal.z);
 
 
        // collect the points whose local normals constitute that circle
        CVec3dArray avCylinderCandidates;
 
        for (int j = 0; j < nNumberOfPointsInGaussMap; j++)
        {
            if (fabsf(Math3d::ScalarProduct(vBestNormal, avGaussMap[j])) <=
                1.0f * fGaussMapThreshold)
            {
                // check if the point is already amongst the candidates
                bool bDouble = false;
 
                for (int i = 0; i < avCylinderCandidates.GetSize(); i++)
                {
                    if (avCorrespondingPoints[j].x == avCylinderCandidates[i].x)
                    {
                        if ((avCorrespondingPoints[j].y == avCylinderCandidates[i].y) &&
                            (avCorrespondingPoints[j].z == avCylinderCandidates[i].z))
                        {
                            bDouble = true;
                            break;
                        }
                    }
                }
 
                if (!bDouble)
                {
                    avCylinderCandidates.AddElement(avCorrespondingPoints[j]);
                }
 
                //ARMARX_VERBOSE_S << "c.p.: ( %.1f  %.1f  %.1f )\n", avCorrespondingPoints[j].x, avCorrespondingPoints[j].y, avCorrespondingPoints[j].z);
            }
        }
 
 
        // project the points to the plane orthogonal to the cylinder axis
        const Vec3d vCylinderAxis = {vBestNormal.x, vBestNormal.y, vBestNormal.z};
        const Vec3d vZAxis = {0, 0, 1};
        Vec3d vTemp1, vTemp2;
        Math3d::CrossProduct(vCylinderAxis, vZAxis, vTemp1);
        Math3d::CrossProduct(vCylinderAxis, vTemp1, vTemp2);
        const Mat3d mTransform = {vCylinderAxis.x,
                                  vTemp1.x,
                                  vTemp2.x,
                                  vCylinderAxis.y,
                                  vTemp1.y,
                                  vTemp2.y,
                                  vCylinderAxis.z,
                                  vTemp1.z,
                                  vTemp2.z};
        Mat3d mTransformInv;
        Math3d::Invert(mTransform, mTransformInv);
        const int nNumCylinderCandidates = avCylinderCandidates.GetSize();
        CVec3dArray avCylinderCandidatesTransformed(nNumCylinderCandidates);
 
        for (int i = 0; i < nNumCylinderCandidates; i++)
        {
            Math3d::MulMatVec(mTransform, avCylinderCandidates[i], vTemp1);
            avCylinderCandidatesTransformed.AddElement(vTemp1);
        }
 
 
        // choose 3 random points and calculate a circle on the plane orthogonal to the cylinder axis
        // get the best cylinder that is defined by the axis and such a circle
 
        //ARMARX_VERBOSE_S << "nNumCylinderCandidates: %d\n", nNumCylinderCandidates);
 
        //gettimeofday(&tStart, 0);
        if (nNumCylinderCandidates < OLP_MIN_NUM_FEATURES)
        {
            //ARMARX_VERBOSE_S << "\n(nNumCylinderCandidates < 10)\n\n");
            continue;
        }
 
        const float fMaxRadius2 =
            fMaxRadius * fMaxRadius; // avoid sqrt in the loop as far as possible
        const float fToleranceThreshold2 = fToleranceThreshold * fToleranceThreshold;
        const int nMaxCircleIterations =
            (int)(OLP_EFFORT_MODIFICATOR * 2000.0f); // 5000 (frueher: 12000)
        const int nCircleIterationsPrep =
            (int)(OLP_EFFORT_MODIFICATOR * (float)nNumCylinderCandidates * 4.0f *
                  sqrtf((float)nNumCylinderCandidates)); // 5.0
        const int nCircleIterations = (nCircleIterationsPrep < nMaxCircleIterations)
                                          ? nCircleIterationsPrep
                                          : nMaxCircleIterations;
        int nMaxSupportForThisAxis = OLP_MIN_NUM_FEATURES - 1;
 
        //      const int nSqrtNumCylinderCandidates = 20;//(nNumCylinderCandidates>225) ? ((int)(sqrtf((float)nNumCylinderCandidates))) : 15;
        //      int* pRandomIndices = new int[nSqrtNumCylinderCandidates];
        //      for (int i=0; i<nSqrtNumCylinderCandidates; i++)
        //      {
        //          pRandomIndices[i] = rand() % nNumCylinderCandidates;
        //      }
 
        //ARMARX_VERBOSE_S << "#CPUs: %d, #threads: %d, max#threads: %d, parallel: %d\n", omp_get_num_procs(), omp_get_num_threads(), omp_get_max_threads(), omp_in_parallel());
 
#pragma omp parallel
        {
            int nFirstIndex, nSecondIndex, nThirdIndex, nSupport;
            float xi, yi, xj, yj, xk, yk, fCenterX, fCenterY, fRadius2, fDist2;
            Vec3d vCylinderCenter, vTemp1, vTemp2;
            int nMaxSupportLocal = OLP_MIN_NUM_FEATURES - 1;
            Vec3d vBestCylinderCenterLocal;
            float fBestRadiusLocal = 0;
 
#pragma omp for schedule(static, 32)
            for (int i = 0; i < nCircleIterations; i++)
            {
                //if (i==42) ARMARX_VERBOSE_S << "#CPUs: %d, #threads: %d, max#threads: %d, parallel: %d\n", omp_get_num_procs(), omp_get_num_threads(), omp_get_max_threads(), omp_in_parallel());
 
                // get 3 random points and calculate the circle that they define
                srand((rand() % 17) * i + i);
                nFirstIndex = rand() % nNumCylinderCandidates;
 
                do
                {
                    nSecondIndex = rand() % nNumCylinderCandidates;
                } while (nSecondIndex == nFirstIndex);
 
                do
                {
                    nThirdIndex = rand() % nNumCylinderCandidates;
                } while (nThirdIndex == nFirstIndex || nThirdIndex == nSecondIndex);
 
                xi = avCylinderCandidatesTransformed[nFirstIndex].y;
                yi = avCylinderCandidatesTransformed[nFirstIndex].z;
                xj = avCylinderCandidatesTransformed[nSecondIndex].y;
                yj = avCylinderCandidatesTransformed[nSecondIndex].z;
                xk = avCylinderCandidatesTransformed[nThirdIndex].y;
                yk = avCylinderCandidatesTransformed[nThirdIndex].z;
                //ARMARX_VERBOSE_S << "%.3f  %.3f  %.3f  %.3f  %.3f  %.3f\n", xi, yi, xj, yj, xk, yk);
 
                const float fDelta = (xi * (yk - yj) + xj * (yi - yk) + xk * (yj - yi));
 
                //ARMARX_VERBOSE_S << "fDelta: %f\n", fDelta);
                if (fabs(fDelta) > 0.001f)
                {
                    fCenterX = 0.5f / fDelta *
                               ((yk - yj) * (xi * xi + yi * yi) + (yi - yk) * (xj * xj + yj * yj) +
                                (yj - yi) * (xk * xk + yk * yk));
                    fCenterY = -0.5f / fDelta *
                               ((xk - xj) * (xi * xi + yi * yi) + (xi - xk) * (xj * xj + yj * yj) +
                                (xj - xi) * (xk * xk + yk * yk));
                    fRadius2 = (fCenterX - xi) * (fCenterX - xi) +
                               (fCenterY - yi) * (fCenterY - yi); // avoid sqrt here
                }
                else
                {
                    continue;
                }
 
 
                // if the radius is realistic
                if (fRadius2 < fMaxRadius2)
                {
                    // restrict the radius further
                    //fRadius2 = (fRadius2 < 0.7*fMaxRadius2) ? fRadius2 : 0.7*fMaxRadius2;
 
                    // calculate the cylinder parameters
                    Math3d::SetVec(vTemp1, 0, fCenterX, fCenterY);
                    Math3d::MulMatVec(mTransformInv, vTemp1, vCylinderCenter);
 
                    // calculate the plane spanned by the cylinder axis and the vector that is
                    // orthogonal to the cylinder axis and the z-axis
                    Vec3d vPlaneNormal;
                    Math3d::CrossProduct(vCylinderAxis, vZAxis, vTemp1);
                    Math3d::CrossProduct(vCylinderAxis, vTemp1, vPlaneNormal);
                    const float fD = -Math3d::ScalarProduct(vPlaneNormal, vCylinderCenter);
                    float fTemp;
 
                    //ARMARX_VERBOSE_S << "center: (%.1f  %.1f  %.1f)   radius: %.2f\n", vCylinderCenter.x, vCylinderCenter.y, vCylinderCenter.z, fRadius);
 
                    //                  // first count the support for this cylinder using a randomized sample (23800ms)
                    //                  bool bDoIntensiveCheck = true;
                    //                  nSupport = 0;
                    //                  for (int j=0; j<nSqrtNumCylinderCandidates; j++)
                    //                  {
                    //                      int k = pRandomIndices[j];
                    //                      // accept only points that are on the side of the cylinder that is turned towards
                    //                      // the camera. If those points are put into the plane equation, the result has the
                    //                      // same sign as the result for (0,0,0), which is = fD
                    //                      fTemp = Math3d::ScalarProduct(vPlaneNormal, avCylinderCandidates[k]) + fD;
                    //                      if (fTemp*fD > 0)
                    //                      {
                    //                          Math3d::SubtractVecVec(avCylinderCandidates[k], vCylinderCenter, vTemp1);
                    //                          Math3d::CrossProduct(vCylinderAxis, vTemp1, vTemp2);
                    //                          fTemp = Math3d::ScalarProduct(vTemp2, vTemp2);
                    //
                    //                          // only here one sqrt is necessary
                    //                          fDist2 = fTemp - 2.0f*sqrtf(fTemp*fRadius2) + fRadius2;
                    //
                    //                          if (fDist2 < fToleranceThreshold2)
                    //                          {
                    //                              nSupport++;
                    //                          }
                    //                      }
                    //                  }
                    //                  if (nSqrtNumCylinderCandidates*nSupport < nMaxSupportLocal)
                    //                  {
                    //                      bDoIntensiveCheck = false;
                    //                  }
                    //
                    //                  // if randomized evaluation indicates a good candidate, do the full support check
                    //                  if (bDoIntensiveCheck)
                    {
                        // count the support for this cylinder
                        nSupport = 0;
 
                        for (int j = 0; j < nNumCylinderCandidates; j++)
                        {
                            // accept only points that are on the side of the cylinder that is turned towards
                            // the camera. If those points are put into the plane equation, the result has the
                            // same sign as the result for (0,0,0), which is = fD
                            fTemp =
                                Math3d::ScalarProduct(vPlaneNormal, avCylinderCandidates[j]) + fD;
 
                            if (fTemp * fD > 0)
                            {
                                Math3d::SubtractVecVec(
                                    avCylinderCandidates[j], vCylinderCenter, vTemp1);
                                Math3d::CrossProduct(vCylinderAxis, vTemp1, vTemp2);
                                fTemp = Math3d::ScalarProduct(vTemp2, vTemp2);
 
                                // only here one sqrt is necessary
                                fDist2 = fTemp - 2.0f * sqrtf(fTemp * fRadius2) + fRadius2;
 
                                if (fDist2 < fToleranceThreshold2)
                                {
                                    nSupport++;
                                }
                            }
                        }
 
                        if (nSupport > nMaxSupportLocal)
                        {
                            nMaxSupportLocal = nSupport;
                            Math3d::SetVec(vBestCylinderCenterLocal, vCylinderCenter);
                            fBestRadiusLocal = sqrtf(fRadius2);
                        }
                    }
                }
            }
 
#pragma omp critical
            if (nMaxSupportLocal > nMaxSupportForThisAxis)
            {
                nMaxSupportForThisAxis = nMaxSupportLocal;
 
                if (nMaxSupportLocal > nMaxSupport)
                {
                    nMaxSupport = nMaxSupportLocal;
                    Math3d::SetVec(vBestCylinderCenter, vBestCylinderCenterLocal);
                    Math3d::SetVec(vBestCylinderAxis, vCylinderAxis);
                    fBestRadius = fBestRadiusLocal;
                }
            }
        }
 
        //if (nMaxSupportForThisAxis == nMaxSupport) ARMARX_VERBOSE_S << "%d  ", n);
 
        //ARMARX_VERBOSE_S << "Max support: %d\n", nMaxSupport);
 
        //      delete[] pRandomIndices;
 
        avAxisBlacklist.AddElement(vCylinderAxis);
 
        //gettimeofday(&tEnd, 0);
        //tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
        //ARMARX_VERBOSE_S << "Time for cylinder radius: %ld ms\n", tTimeDiff);
    }
 
 
    if (nMaxSupport >= OLP_MIN_NUM_FEATURES)
    {
        // return the best cylinder
        float fDist;
        Vec3d vTemp1, vTemp2;
 
        for (int j = 0; j < nOverallNumberOfPoints; j++)
        {
            Math3d::SubtractVecVec(avPoints[j], vBestCylinderCenter, vTemp1);
            Math3d::CrossProduct(vBestCylinderAxis, vTemp1, vTemp2);
            fDist = Math3d::Length(vTemp2);
 
            if (fabsf(fDist - fBestRadius) <= fToleranceThreshold)
            {
                avCylinder.AddElement(avPoints[j]);
            }
        }
 
        //ARMARX_VERBOSE_S << "\nAxis: ( %.2f  %.2f  %.2f )\nRadius: %.1f\n\n", vBestCylinderAxis.x, vBestCylinderAxis.y, vBestCylinderAxis.z, fBestRadius);
 
        Math3d::SetVec(vCylinderAxis, vBestCylinderAxis);
        Math3d::SetVec(vCylinderCenter, vBestCylinderCenter);
        fCylinderRadius = fBestRadius;
 
        //ARMARX_VERBOSE_S << "cylinder: %d p\n", avCylinder.GetSize());
 
        return true;
    }
    else
    {
        return false;
    }
}
 
CObjectHypothesis*
CHypothesisGeneration::CreatePlaneHypothesisDescriptor(const CVec3dArray& avPlanePoints,
                                                       std::vector<CMSERDescriptor3D*>& aMSERs,
                                                       std::vector<Vec3d>& aSigmaPoints,
                                                       const CByteImage* pLeftColorImage,
                                                       const CByteImage* pLeftGreyImage)
{
    CObjectHypothesis* hResult = new CObjectHypothesis();
 
    hResult->eType = CObjectHypothesis::ePlane;
 
    const int nNum3dPoints = avPlanePoints.GetSize();
 
    //#pragma omp parallel
    {
        CHypothesisPoint* pPoint;
 
        //#pragma omp for schedule(static, 5)
        for (int i = 0; i < nNum3dPoints; i++)
        {
            pPoint = new CHypothesisPoint();
 
            if (CreateFeatureDescriptorForPoint(pPoint,
                                                avPlanePoints[i],
                                                aMSERs,
                                                aSigmaPoints,
                                                pLeftColorImage,
                                                pLeftGreyImage))
            {
                //#pragma omp critical
                hResult->aNewPoints.push_back(pPoint);
            }
            else
            {
                delete pPoint;
            }
        }
    }
 
    // calculate the mean of the plane points
    Math3d::SetVec(hResult->vCenter, 0, 0, 0);
 
    for (int i = 0; i < nNum3dPoints; i++)
    {
        hResult->vCenter.x += avPlanePoints[i].x;
        hResult->vCenter.y += avPlanePoints[i].y;
        hResult->vCenter.z += avPlanePoints[i].z;
    }
 
    hResult->vCenter.x /= (float)nNum3dPoints;
    hResult->vCenter.y /= (float)nNum3dPoints;
    hResult->vCenter.z /= (float)nNum3dPoints;
 
    // calculate the plane parameters. this is done by calculation of the eigenvectors of the point cloud.
    // the two largest eigenvectors span the plane, the third one is the plane normal.
    CFloatMatrix mPoints(3, nNum3dPoints);
    CFloatMatrix mEigenVectors(3, 3);
    CFloatMatrix mEigenValues(1, 3);
 
    for (int i = 0; i < nNum3dPoints; i++)
    {
        mPoints(0, i) = avPlanePoints[i].x - hResult->vCenter.x;
        mPoints(1, i) = avPlanePoints[i].y - hResult->vCenter.y;
        mPoints(2, i) = avPlanePoints[i].z - hResult->vCenter.z;
    }
 
    LinearAlgebra::PCA(&mPoints, &mEigenVectors, &mEigenValues);
 
    // make sure the normal points towards the camera
    if (mEigenVectors(2, 2) < 0)
    {
        hResult->vNormal.x = mEigenVectors(2, 0);
        hResult->vNormal.y = mEigenVectors(2, 1);
        hResult->vNormal.z = mEigenVectors(2, 2);
 
        hResult->vSecondAxis.x = mEigenVectors(0, 0);
        hResult->vSecondAxis.y = mEigenVectors(0, 1);
        hResult->vSecondAxis.z = mEigenVectors(0, 2);
    }
    else
    {
        hResult->vNormal.x = -mEigenVectors(2, 0);
        hResult->vNormal.y = -mEigenVectors(2, 1);
        hResult->vNormal.z = -mEigenVectors(2, 2);
 
        hResult->vSecondAxis.x = -mEigenVectors(0, 0);
        hResult->vSecondAxis.y = -mEigenVectors(0, 1);
        hResult->vSecondAxis.z = -mEigenVectors(0, 2);
    }
 
 
    hResult->vThirdAxis.x = mEigenVectors(1, 0);
    hResult->vThirdAxis.y = mEigenVectors(1, 1);
    hResult->vThirdAxis.z = mEigenVectors(1, 2);
 
    Math3d::NormalizeVec(hResult->vNormal);
    Math3d::NormalizeVec(hResult->vSecondAxis);
    Math3d::NormalizeVec(hResult->vThirdAxis);
 
    hResult->fStdDev1 = mEigenValues(0, 0);
    hResult->fStdDev2 = mEigenValues(0, 1);
 
    hResult->fMaxExtent = 0;
    float fDist;
 
    for (int i = 0; i < nNum3dPoints; i++)
    {
        fDist = Math3d::Distance(hResult->vCenter, avPlanePoints[i]);
 
        if (fDist > hResult->fMaxExtent)
        {
            hResult->fMaxExtent = fDist;
        }
    }
 
 
    hResult->mLastRotation = Math3d::unit_mat;
    hResult->vLastTranslation = Math3d::zero_vec;
 
    return hResult;
}
 
CObjectHypothesis*
CHypothesisGeneration::CreateCylinderHypothesisDescriptor(const CVec3dArray& avCylinderPoints,
                                                          const Vec3d vCylinderAxis,
                                                          const Vec3d vCylinderCenter,
                                                          const float fCylinderRadius,
                                                          std::vector<CMSERDescriptor3D*>& aMSERs,
                                                          std::vector<Vec3d>& aSigmaPoints,
                                                          const CByteImage* pLeftColorImage,
                                                          const CByteImage* pLeftGreyImage)
{
    CObjectHypothesis* hResult = new CObjectHypothesis();
 
    hResult->eType = CObjectHypothesis::eCylinder;
 
    const int nNum3dPoints = avCylinderPoints.GetSize();
 
#pragma omp parallel
    {
        CHypothesisPoint* pPoint;
 
#pragma omp for schedule(static, 5)
        for (int i = 0; i < nNum3dPoints; i++)
        {
            pPoint = new CHypothesisPoint();
 
            if (CreateFeatureDescriptorForPoint(pPoint,
                                                avCylinderPoints[i],
                                                aMSERs,
                                                aSigmaPoints,
                                                pLeftColorImage,
                                                pLeftGreyImage))
            {
#pragma omp critical
                hResult->aNewPoints.push_back(pPoint);
            }
            else
            {
                delete pPoint;
            }
        }
    }
 
    // set axis and radius
    Math3d::SetVec(hResult->vCylinderAxis, vCylinderAxis);
    hResult->fRadius = fCylinderRadius;
 
    // calculate mean
    Vec3d vMean;
    const float fVariance = GetVariance(avCylinderPoints, vMean);
 
    // project the mean onto the axis
    // project point P onto the line given by point A and vector AB:
    // P' = A + ( AB*AP / |AB|^2 ) AB   (here: |AB|=1)
    Vec3d vCenterOnAxis = vCylinderCenter;
    Vec3d vTemp1, vTemp2;
    Math3d::SubtractVecVec(vMean, vCylinderCenter, vTemp1);
    Math3d::MulVecScalar(vCylinderAxis, Math3d::ScalarProduct(vCylinderAxis, vTemp1), vTemp2);
    Math3d::AddToVec(vCenterOnAxis, vTemp2);
    Math3d::SetVec(hResult->vCenter, vCenterOnAxis);
 
    // normal is the vector from the center on the axis to the mean of the cylinder points
    Math3d::SubtractVecVec(vMean, vCenterOnAxis, hResult->vNormal);
    Math3d::NormalizeVec(hResult->vNormal);
 
 
    // first axis is cylinder axis, second is orthogonal to cylinder axis and surface normal,
    // third axis is surface normal
    Math3d::CrossProduct(vCylinderAxis, hResult->vNormal, hResult->vSecondAxis);
    Math3d::SetVec(hResult->vThirdAxis, hResult->vNormal);
 
 
    // simple solution
    hResult->fStdDev1 = fVariance;
    hResult->fStdDev2 = fVariance;
 
    hResult->fMaxExtent = hResult->fRadius;
    float fDist;
 
    for (int i = 0; i < nNum3dPoints; i++)
    {
        fDist = Math3d::Distance(hResult->vCenter, avCylinderPoints[i]);
 
        if (fDist > hResult->fMaxExtent)
        {
            hResult->fMaxExtent = fDist;
        }
    }
 
    hResult->mLastRotation = Math3d::unit_mat;
    hResult->vLastTranslation = Math3d::zero_vec;
 
    return hResult;
}
 
CObjectHypothesis*
CHypothesisGeneration::CreateSphereHypothesisDescriptor(const CVec3dArray& avSpherePoints,
                                                        const Vec3d vSphereCenter,
                                                        const float fSphereRadius,
                                                        std::vector<CMSERDescriptor3D*>& aMSERs,
                                                        std::vector<Vec3d>& aSigmaPoints,
                                                        const CByteImage* pLeftColorImage,
                                                        const CByteImage* pLeftGreyImage)
{
    CObjectHypothesis* hResult = new CObjectHypothesis();
 
    hResult->eType = CObjectHypothesis::eSphere;
 
    const int nNum3dPoints = avSpherePoints.GetSize();
 
#pragma omp parallel
    {
        CHypothesisPoint* pPoint;
 
#pragma omp for schedule(static, 5)
        for (int i = 0; i < nNum3dPoints; i++)
        {
            if (CreateFeatureDescriptorForPoint(pPoint,
                                                avSpherePoints[i],
                                                aMSERs,
                                                aSigmaPoints,
                                                pLeftColorImage,
                                                pLeftGreyImage))
            {
#pragma omp critical
                hResult->aNewPoints.push_back(pPoint);
            }
            else
            {
                delete pPoint;
            }
        }
    }
 
    // set center and radius
    Math3d::SetVec(hResult->vCenter, vSphereCenter);
    hResult->fRadius = fSphereRadius;
 
 
    // normal is the vector from the center to the mean of the sphere points
    Vec3d vMean;
    const float fVariance = GetVariance(avSpherePoints, vMean);
    Math3d::SubtractVecVec(vMean, vSphereCenter, hResult->vNormal);
    Math3d::NormalizeVec(hResult->vNormal);
 
 
    // second axis is orthogonal to normal and x axis, third axis orthogonal to normal and second
    Vec3d vXAxis = {1, 0, 0};
    Math3d::CrossProduct(hResult->vNormal, vXAxis, hResult->vSecondAxis);
    Math3d::CrossProduct(hResult->vNormal, hResult->vSecondAxis, hResult->vThirdAxis);
 
 
    // variances dont make much sense here...
    hResult->fStdDev1 = fVariance;
    hResult->fStdDev2 = fVariance;
 
    hResult->fMaxExtent = hResult->fRadius;
    float fDist;
 
    for (int i = 0; i < nNum3dPoints; i++)
    {
        fDist = Math3d::Distance(hResult->vCenter, avSpherePoints[i]);
 
        if (fDist > hResult->fMaxExtent)
        {
            hResult->fMaxExtent = fDist;
        }
    }
 
    hResult->mLastRotation = Math3d::unit_mat;
    hResult->vLastTranslation = Math3d::zero_vec;
 
    return hResult;
}
 
bool
CHypothesisGeneration::CreateFeatureDescriptorForPoint(CHypothesisPoint* pPoint,
                                                       Vec3d vPosition,
                                                       std::vector<CMSERDescriptor3D*>& aMSERs,
                                                       std::vector<Vec3d>& aSigmaPoints,
                                                       const CByteImage* pLeftColorImage,
                                                       const CByteImage* pLeftGreyImage)
{
    pPoint->vPosition = vPosition;
    pPoint->vOldPosition = vPosition;
    pPoint->fMembershipProbability = 0.5f;
 
    // set color
    Vec2d vPos2d;
    calibration->WorldToImageCoordinates(vPosition, vPos2d, false);
    const int nIndexX = round(vPos2d.x);
    const int nIndexY = round(vPos2d.y);
 
    if (nIndexX < 0 || nIndexX > OLP_IMG_WIDTH - 1 || nIndexY < 0 || nIndexY > OLP_IMG_HEIGHT - 1)
    {
        return false;
    }
 
    const float fIntensity = pLeftColorImage->pixels[3 * (nIndexY * OLP_IMG_WIDTH + nIndexX)] +
                             pLeftColorImage->pixels[3 * (nIndexY * OLP_IMG_WIDTH + nIndexX) + 1] +
                             pLeftColorImage->pixels[3 * (nIndexY * OLP_IMG_WIDTH + nIndexX) + 2] +
                             3;
    pPoint->fIntensity = (fIntensity - 3) / (3 * 255);
    pPoint->fColorR =
        (pLeftColorImage->pixels[3 * (nIndexY * OLP_IMG_WIDTH + nIndexX)] + 1) / fIntensity;
    pPoint->fColorG =
        (pLeftColorImage->pixels[3 * (nIndexY * OLP_IMG_WIDTH + nIndexX) + 1] + 1) / fIntensity;
    pPoint->fColorB =
        (pLeftColorImage->pixels[3 * (nIndexY * OLP_IMG_WIDTH + nIndexX) + 2] + 1) / fIntensity;
 
    // check if the point is the center of an MSER
    int nRegionIndex = FindRegionForPoint(vPosition, aMSERs);
 
    if (nRegionIndex >= 0)
    {
        // associate MSER descriptor with that point
        pPoint->ePointType = CHypothesisPoint::eMSER;
        pPoint->pFeatureDescriptors = NULL;
        pPoint->pMSERDescriptor = aMSERs.at(nRegionIndex)->GetCopy();
        return true;
    }
 
    // check if it is a sigma point. if yes, ignore it
    if (PointIsInList(vPosition, aSigmaPoints))
    {
        return false;
    }
 
    // else create SIFT descriptor
    pPoint->ePointType = CHypothesisPoint::eHarrisCornerPoint;
    pPoint->pMSERDescriptor = NULL;
    pPoint->pFeatureDescriptors = new CSIFTFeatureArray();
    AddSIFTFeatureDescriptors(pPoint->pFeatureDescriptors, vPos2d, pLeftGreyImage);
 
    return true;
}
 
bool
CHypothesisGeneration::RANSACPlane(const CVec3dArray& pointCandidates,
                                   const float fRANSACThreshold,
                                   const int nIterations,
                                   CVec3dArray& resultPoints,
                                   Vec3d& vAxis,
                                   float& d)
{
    const int nPointCandidates = pointCandidates.GetSize();
 
    if (nPointCandidates < 3)
    {
        ARMARX_WARNING_S
            << "At least 3 point candidates must be provided for RANSAC::RANSAC3DPlane ("
            << nPointCandidates << " provided)";
        return false;
    }
 
    const int nParallelityFactor = omp_get_num_procs();
    omp_set_num_threads(nParallelityFactor);
 
    int* pMaxPar = new int[nParallelityFactor];
    float* pBestC = new float[nParallelityFactor];
    Vec3d* pBestN = new Vec3d[nParallelityFactor];
 
    for (int i = 0; i < nParallelityFactor; i++)
    {
        pMaxPar[i] = 0;
        pBestC[i] = 0;
        pBestN[i] = Math3d::zero_vec;
    }
 
 
#pragma omp parallel
    {
        const int nProcIndex = omp_get_thread_num();
 
#pragma omp for schedule(dynamic, 200)
        for (int i = 0; i < nIterations; i++)
        {
            // identify 3 different points
            srand((rand() % 17) * i + i);
            const int nFirstIndex = rand() % nPointCandidates;
 
            int nTempIndex;
 
            do
            {
                nTempIndex = rand() % nPointCandidates;
            } while (nTempIndex == nFirstIndex);
 
            const int nSecondIndex = nTempIndex;
 
            do
            {
                nTempIndex = rand() % nPointCandidates;
            } while (nTempIndex == nFirstIndex || nTempIndex == nSecondIndex);
 
            const Vec3d& p1 = pointCandidates[nFirstIndex];
            const Vec3d& p2 = pointCandidates[nSecondIndex];
            const Vec3d& p3 = pointCandidates[nTempIndex];
 
            Vec3d u1, u2, n;
            Math3d::SubtractVecVec(p2, p1, u1);
            Math3d::SubtractVecVec(p3, p1, u2);
            Math3d::CrossProduct(u1, u2, n);
            Math3d::NormalizeVec(n);
            const float c = Math3d::ScalarProduct(n, p1);
 
            // normal sometimes is (0,0,0), probably because of colinear points
            if (Math3d::ScalarProduct(n, n) < 0.9f)
            {
                continue;
            }
 
            // count support
            int nSupport = 0;
 
            for (int j = 0; j < nPointCandidates; j++)
                if (fabsf(Math3d::ScalarProduct(n, pointCandidates[j]) - c) <= fRANSACThreshold)
                {
                    nSupport++;
                }
 
            // store if it is the current maximum
            if (nSupport > pMaxPar[nProcIndex])
            {
                pMaxPar[nProcIndex] = nSupport;
                Math3d::SetVec(pBestN[nProcIndex], n);
                pBestC[nProcIndex] = c;
            }
        }
    }
 
    int nMax = 0;
    float fBestC = 0;
    Vec3d vBestN = {0, 0, 0};
 
    for (int i = 0; i < nParallelityFactor; i++)
    {
        if (pMaxPar[i] > nMax)
        {
            nMax = pMaxPar[i];
            fBestC = pBestC[i];
            Math3d::SetVec(vBestN, pBestN[i]);
        }
    }
 
    // filter points
    resultPoints.Clear();
 
    for (int i = 0; i < nPointCandidates; i++)
    {
        if (fabsf(Math3d::ScalarProduct(pointCandidates[i], vBestN) - fBestC) <= fRANSACThreshold)
        {
            resultPoints.AddElement(pointCandidates[i]);
        }
    }
 
    Math3d::SetVec(vAxis, vBestN);
    d = fBestC;
 
    //ARMARX_VERBOSE_S << "plane: %d p\n", resultPoints.GetSize());
    delete[] pMaxPar;
    delete[] pBestC;
    delete[] pBestN;
 
    return true;
}
 
void
CHypothesisGeneration::AddSIFTFeatureDescriptors(CSIFTFeatureArray* pFeatures,
                                                 Vec2d vPoint2d,
                                                 const CByteImage* pLeftGreyImage)
{
    CDynamicArray afSIFTDescriptors(10);
    m_pSIFTFeatureCalculator->CreateSIFTDescriptors(
        pLeftGreyImage, &afSIFTDescriptors, vPoint2d.x, vPoint2d.y, 0.8f, false, true);
    m_pSIFTFeatureCalculator->CreateSIFTDescriptors(
        pLeftGreyImage, &afSIFTDescriptors, vPoint2d.x, vPoint2d.y, 1.0f, false, true);
    m_pSIFTFeatureCalculator->CreateSIFTDescriptors(
        pLeftGreyImage, &afSIFTDescriptors, vPoint2d.x, vPoint2d.y, 1.25f, false, true);
    const int n = afSIFTDescriptors.GetSize();
    CSIFTFeatureEntry** ppFeatures = new CSIFTFeatureEntry*[n];
 
    for (int i = 0; i < n; i++)
    {
        pFeatures->AddElement(
            (CSIFTFeatureEntry*)((CSIFTFeatureEntry*)afSIFTDescriptors[i])->Clone());
        afSIFTDescriptors[i]->bDelete = false;
        ppFeatures[i] = (CSIFTFeatureEntry*)afSIFTDescriptors[i];
    }
 
    afSIFTDescriptors.Clear();
 
    for (int i = 0; i < n; i++)
    {
        delete ppFeatures[i];
    }
 
    delete[] ppFeatures;
}
 
void
CHypothesisGeneration::ClusterXMeans(const CVec3dArray& aPoints,
                                     const int nMaxNumClusters,
                                     const float fBICFactor,
                                     std::vector<CVec3dArray*>& aaPointClusters)
{
    cv::Mat mSamples;
    cv::Mat mClusterLabels;
    cv::Mat pmClusterCenters; // = NULL;
    const int nNumberOfDifferentInitialisations = 5;
    cv::TermCriteria tTerminationCriteria(
        cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, 0.01);
 
    // copy the points
    const int nNumberOfSamples = aPoints.GetSize();
    mSamples.create(nNumberOfSamples, 3, CV_32FC1);
 
    for (int i = 0; i < nNumberOfSamples; i++)
    {
        mSamples.at<float>(i, 0) = aPoints[i].x;
        mSamples.at<float>(i, 1) = aPoints[i].y;
        mSamples.at<float>(i, 2) = aPoints[i].z;
    }
 
    mClusterLabels.create(nNumberOfSamples, 1, CV_32SC1);
 
    // execute k-means for several values of k and find the value for k that minimises the
    // Bayesian Information Criterion (BIC)
    Vec3d* pvClusterMeans = new Vec3d[nMaxNumClusters];
    double* pdClusterVariances = new double[nMaxNumClusters];
    int* pnClusterSizes = new int[nMaxNumClusters];
    double dMinBIC = 10000000;
    int nOptK = 1;
    double dKMeansCompactness, dLogVar, dBIC;
 
    for (int i = 1; (i <= nMaxNumClusters) && (i < nNumberOfSamples); i++)
    {
#ifdef OLP_USE_NEW_OPENCV
        dKMeansCompactness = cv::kmeans(mSamples,
                                        i,
                                        mClusterLabels,
                                        tTerminationCriteria,
                                        nNumberOfDifferentInitialisations,
                                        cv::KMEANS_RANDOM_CENTERS,
                                        pmClusterCenters);
#else
        //dKMeansCompactness = cv::kmeans(mSamples, i, mClusterLabels, tTerminationCriteria, nNumberOfDifferentInitialisations, cv::KMEANS_RANDOM_CENTERS, &pmClusterCenters);
        dKMeansCompactness = cv::kmeans(mSamples,
                                        i,
                                        mClusterLabels,
                                        tTerminationCriteria,
                                        nNumberOfDifferentInitialisations,
                                        cv::KMEANS_PP_CENTERS,
                                        &pmClusterCenters);
#endif
 
        //const double dMLVariance = 1/(double)(nNumberOfSamples-i) * dKMeansCompactness;
 
        // calculate variances of the clusters
        double dMLVariance = 0;
 
        for (int j = 0; j < i; j++)
        {
            pvClusterMeans[j].x = 0;
            pvClusterMeans[j].y = 0;
            pvClusterMeans[j].z = 0;
            pdClusterVariances[j] = 0;
            pnClusterSizes[j] = 0;
 
            for (int l = 0; l < nNumberOfSamples; l++)
            {
                if (mClusterLabels.at<int>(l, 0) == j)
                {
                    pvClusterMeans[j].x += mSamples.at<float>(l, 0);
                    pvClusterMeans[j].y += mSamples.at<float>(l, 1);
                    pvClusterMeans[j].z += mSamples.at<float>(l, 2);
                    pnClusterSizes[j]++;
                }
            }
 
            pvClusterMeans[j].x /= (float)pnClusterSizes[j];
            pvClusterMeans[j].y /= (float)pnClusterSizes[j];
            pvClusterMeans[j].z /= (float)pnClusterSizes[j];
 
            for (int l = 0; l < nNumberOfSamples; l++)
            {
                if (mClusterLabels.at<int>(l, 0) == j)
                {
                    pdClusterVariances[j] += (pvClusterMeans[j].x - mSamples.at<float>(l, 0)) *
                                                 (pvClusterMeans[j].x - mSamples.at<float>(l, 0)) +
                                             (pvClusterMeans[j].y - mSamples.at<float>(l, 1)) *
                                                 (pvClusterMeans[j].x - mSamples.at<float>(l, 1)) +
                                             (pvClusterMeans[j].z - mSamples.at<float>(l, 2)) *
                                                 (pvClusterMeans[j].x - mSamples.at<float>(l, 2));
                }
            }
 
            if (pnClusterSizes[j] > 1)
            {
                pdClusterVariances[j] /= (float)(pnClusterSizes[j] - 1);
            }
            else
            {
                pdClusterVariances[j] = 0;
            }
 
            dMLVariance += pdClusterVariances[j];
            //ARMARX_VERBOSE_S << "pnClusterSizes[%d] = %d \n", j, pnClusterSizes[j]);
        }
 
        const int nNumberOfFreeParameters = (i - 1) + (3 * i) + i;
        dLogVar = log(dMLVariance);
        dBIC = fBICFactor * 0.35 * dLogVar +
               ((double)nNumberOfFreeParameters / (double)nNumberOfSamples) *
                   log((double)nNumberOfSamples); // 0.4  // 1.5 with manual variance
 
        if (dBIC < dMinBIC)
        {
            dMinBIC = dBIC;
            nOptK = i;
        }
 
        //ARMARX_VERBOSE_S << "k-means with %d clusters. log(var): %.3f   BIC: %.3f\n", i, dLogVar, dBIC);
    }
 
    // execute k-means with optimal k
    if (nOptK > 1)
    {
#ifdef OLP_USE_NEW_OPENCV
        dKMeansCompactness = cv::kmeans(mSamples,
                                        nOptK,
                                        mClusterLabels,
                                        tTerminationCriteria,
                                        nNumberOfDifferentInitialisations,
                                        cv::KMEANS_RANDOM_CENTERS,
                                        pmClusterCenters);
#else
        //dKMeansCompactness = cv::kmeans(mSamples, nOptK, mClusterLabels, tTerminationCriteria, nNumberOfDifferentInitialisations, cv::KMEANS_RANDOM_CENTERS, &pmClusterCenters);
        dKMeansCompactness = cv::kmeans(mSamples,
                                        nOptK,
                                        mClusterLabels,
                                        tTerminationCriteria,
                                        nNumberOfDifferentInitialisations,
                                        cv::KMEANS_PP_CENTERS,
                                        &pmClusterCenters);
#endif
 
        //// get sizes of the clusters
        //int* pnClusterSizes = new int[nOptK];
        //for (int i=0; i<nOptK; i++) pnClusterSizes[i] = 0;
        //for (int i=0; i<nNumberOfSamples; i++)
        //{
        //  if ( (mClusterLabels.at<int>(i,0) >=0) && (mClusterLabels.at<int>(i,0)<nOptK) )
        //  {
        //      pnClusterSizes[mClusterLabels.at<int>(i,0)]++;
        //  }
        //  else
        //  {
        //      ARMARX_VERBOSE_S << "\ninvalid cluster label: %d\n\n", mClusterLabels.at<int>(i,0));
        //      ARMARX_VERBOSE_S << "\n");
        //  }
        //}
 
 
        // copy the points belonging to the clusters
        Vec3d vPoint;
        aaPointClusters.clear();
        CVec3dArray* paDummy;
 
        for (int i = 0; i < nOptK; i++)
        {
            paDummy = new CVec3dArray();
            aaPointClusters.push_back(paDummy);
        }
 
        for (int i = 0; i < nNumberOfSamples; i++)
        {
            if ((mClusterLabels.at<int>(i, 0) >= 0) && (mClusterLabels.at<int>(i, 0) < nOptK))
            {
                vPoint.x = mSamples.at<float>(i, 0);
                vPoint.y = mSamples.at<float>(i, 1);
                vPoint.z = mSamples.at<float>(i, 2);
                aaPointClusters.at(mClusterLabels.at<int>(i, 0))->AddElement(vPoint);
            }
            else
            {
                ARMARX_WARNING_S << "Invalid cluster label: " << mClusterLabels.at<int>(i, 0)
                                 << "nOptK: " << nOptK << ", i: " << i
                                 << ", nNumberOfSamples: " << nNumberOfSamples
                                 << ", dKMeansCompactness: " << dKMeansCompactness;
                break;
            }
        }
    }
    else
    {
        aaPointClusters.clear();
        CVec3dArray* paDummy = new CVec3dArray();
        aaPointClusters.push_back(paDummy);
        Vec3d vPoint;
 
        for (int i = 0; i < nNumberOfSamples; i++)
        {
            vPoint.x = mSamples.at<float>(i, 0);
            vPoint.y = mSamples.at<float>(i, 1);
            vPoint.z = mSamples.at<float>(i, 2);
            aaPointClusters.at(0)->AddElement(vPoint);
        }
    }
 
    //delete pmClusterCenters;
    delete[] pvClusterMeans;
    delete[] pdClusterVariances;
    delete[] pnClusterSizes;
}
 
inline void
CHypothesisGeneration::SortMSERsBySize(std::vector<CMSERDescriptor3D*>& aRegions3D)
{
    // insertion sort
    CMSERDescriptor3D* pTemp;
 
    for (int i = 1; i < (int)aRegions3D.size(); i++)
    {
        pTemp = aRegions3D.at(i);
        int j = i;
 
        while ((j > 0) && (aRegions3D.at(j - 1)->pRegionLeft->nSize < pTemp->pRegionLeft->nSize))
        {
            aRegions3D.at(j) = aRegions3D.at(j - 1);
            j--;
        }
 
        aRegions3D.at(j) = pTemp;
    }
}
 
inline void
CHypothesisGeneration::SortMSERsByX(std::vector<CMSERDescriptor3D*>& aRegions3D)
{
    // insertion sort
    CMSERDescriptor3D* pTemp;
 
    for (int i = 1; i < (int)aRegions3D.size(); i++)
    {
        pTemp = aRegions3D.at(i);
        int j = i;
 
        while ((j > 0) && (aRegions3D.at(j - 1)->vPosition.x > pTemp->vPosition.x))
        {
            aRegions3D.at(j) = aRegions3D.at(j - 1);
            j--;
        }
 
        aRegions3D.at(j) = pTemp;
    }
}
 
inline int
CHypothesisGeneration::FindRegionForPoint(Vec3d vPoint, std::vector<CMSERDescriptor3D*>& aRegions3D)
{
    // binary search
    int nLeft = 0;
    int nRight = (int)aRegions3D.size() - 1;
    int nIndex, i;
 
    while (nRight > nLeft + 1)
    {
        nIndex = nLeft + ((nRight - nLeft) >> 1);
 
        if (vPoint.x < aRegions3D.at(nIndex)->vPosition.x)
        {
            nRight = nIndex;
        }
        else if (vPoint.x > aRegions3D.at(nIndex)->vPosition.x)
        {
            nLeft = nIndex;
        }
        else // we got the right x value
        {
            // search to the left
            i = nIndex;
 
            while ((i >= nLeft) && (vPoint.x == aRegions3D.at(i)->vPosition.x))
            {
                if ((vPoint.y == aRegions3D.at(i)->vPosition.y) &&
                    (vPoint.z == aRegions3D.at(i)->vPosition.z))
                {
                    return i;
                }
                else
                {
                    i--;
                }
            }
 
            // search to the right
            i = nIndex + 1;
 
            while ((i <= nRight) && (vPoint.x == aRegions3D.at(i)->vPosition.x))
            {
                if ((vPoint.y == aRegions3D.at(i)->vPosition.y) &&
                    (vPoint.z == aRegions3D.at(i)->vPosition.z))
                {
                    return i;
                }
                else
                {
                    i++;
                }
            }
 
            return -1;
        }
    }
 
    if (nLeft <= nRight)
    {
        if ((vPoint.x == aRegions3D.at(nLeft)->vPosition.x) &&
            (vPoint.y == aRegions3D.at(nLeft)->vPosition.y) &&
            (vPoint.z == aRegions3D.at(nLeft)->vPosition.z))
        {
            return nLeft;
        }
        else if ((vPoint.x == aRegions3D.at(nRight)->vPosition.x) &&
                 (vPoint.y == aRegions3D.at(nRight)->vPosition.y) &&
                 (vPoint.z == aRegions3D.at(nRight)->vPosition.z))
        {
            return nRight;
        }
    }
 
    return -1;
}
 
inline void
CHypothesisGeneration::SortVec3dsByX(std::vector<Vec3d>& aPoints)
{
    // insertion sort
    Vec3d vTemp;
 
    for (int i = 1; i < (int)aPoints.size(); i++)
    {
        Math3d::SetVec(vTemp, aPoints.at(i));
        int j = i;
 
        while ((j > 0) && (aPoints.at(j - 1).x > vTemp.x))
        {
            Math3d::SetVec(aPoints.at(j), aPoints.at(j - 1));
            j--;
        }
 
        Math3d::SetVec(aPoints.at(j), vTemp);
    }
}
 
inline bool
CHypothesisGeneration::PointIsInList(Vec3d vPoint, std::vector<Vec3d>& aPoints)
{
    // binary search
    int nLeft = 0;
    int nRight = (int)aPoints.size() - 1;
    int nIndex;
 
    while (nRight > nLeft + 1)
    {
        nIndex = nLeft + ((nRight - nLeft) >> 1);
 
        if (vPoint.x < aPoints.at(nIndex).x)
        {
            nRight = nIndex;
        }
        else if (vPoint.x > aPoints.at(nIndex).x)
        {
            nLeft = nIndex;
        }
        else if ((vPoint.y == aPoints.at(nIndex).y) && (vPoint.z == aPoints.at(nIndex).z))
        {
            return true;
        }
        else
        {
            nLeft++; // ugly, incorrect and easy
        }
    }
 
    if (nLeft <= nRight)
    {
        if ((vPoint.x == aPoints.at(nLeft).x) && (vPoint.y == aPoints.at(nLeft).y) &&
            (vPoint.z == aPoints.at(nLeft).z))
        {
            return true;
        }
        else if ((vPoint.x == aPoints.at(nRight).x) && (vPoint.y == aPoints.at(nRight).y) &&
                 (vPoint.z == aPoints.at(nRight).z))
        {
            return true;
        }
    }
 
    return false;
}
 
void
CHypothesisGeneration::CalculateSphere(Vec3d vPoint1,
                                       Vec3d vPoint2,
                                       Vec3d vPoint3,
                                       Vec3d vPoint4,
                                       Vec3d& vCenter,
                                       float& fRadius)
{
 
    /*
        // matrix containing in each row
        // x^2+y^2+z^2  x  y  z  1
        // first row unknown variables, second row first point, third row second point etc
        float pMatrix[25];
 
        // first row is unknown and will not be used
        //pMatrix[0] = 0;
        //pMatrix[1] = 0;
        //pMatrix[2] = 0;
        //pMatrix[3] = 0;
        //pMatrix[4] = 0;
 
        // first point
        pMatrix[5] = vPoint1.x*vPoint1.x + vPoint1.y*vPoint1.y + vPoint1.z*vPoint1.z;
        pMatrix[6] = vPoint1.x;
        pMatrix[7] = vPoint1.y;
        pMatrix[8] = vPoint1.z;
        pMatrix[9] = 1;
 
        // second point
        pMatrix[10] = vPoint2.x*vPoint2.x + vPoint2.y*vPoint2.y + vPoint2.z*vPoint2.z;
        pMatrix[11] = vPoint2.x;
        pMatrix[12] = vPoint2.y;
        pMatrix[13] = vPoint2.z;
        pMatrix[14] = 1;
 
        pMatrix[15] = vPoint3.x*vPoint3.x + vPoint3.y*vPoint3.y + vPoint3.z*vPoint3.z;
        pMatrix[16] = vPoint3.x;
        pMatrix[17] = vPoint3.y;
        pMatrix[18] = vPoint3.z;
        pMatrix[19] = 1;
 
        pMatrix[20] = vPoint4.x*vPoint4.x + vPoint4.y*vPoint4.y + vPoint4.z*vPoint4.z;
        pMatrix[21] = vPoint4.x;
        pMatrix[22] = vPoint4.y;
        pMatrix[23] = vPoint4.z;
        pMatrix[24] = 1;
    */
 
    // submatrix M_ij: matrix that consists of what is left of M when deleting row i and column j
    float pSubmatrix[16];
 
    // determinants of the submatrices
    float fDetM11, fDetM12, fDetM13, fDetM14, fDetM15;
    /*
        int nIgnoreColumn, l;
 
        nIgnoreColumn = 0;
        for (int i=1; i<5; i++)
        {
            l = 0;
            for (int j=0; j<5; j++)
            {
                if (j != nIgnoreColumn)
                {
                    pSubmatrix[4*(i-1)+l] = pMatrix[5*i+j];
                    l++;
                }
            }
        }
        fDetM11 = Determinant4x4(pSubmatrix);
 
        if (fDetM11==0)
        {
            vCenter.x = 0;
            vCenter.y = 0;
            vCenter.z = 0;
            fRadius = 0;
            return;
        }
 
        nIgnoreColumn = 1;
        for (int i=1; i<5; i++)
        {
            l = 0;
            for (int j=0; j<5; j++)
            {
                if (j != nIgnoreColumn)
                {
                    pSubmatrix[4*(i-1)+l] = pMatrix[5*i+j];
                    l++;
                }
            }
        }
        fDetM12 = Determinant4x4(pSubmatrix);
 
        nIgnoreColumn = 2;
        for (int i=1; i<5; i++)
        {
            l = 0;
            for (int j=0; j<5; j++)
            {
                if (j != nIgnoreColumn)
                {
                    pSubmatrix[4*(i-1)+l] = pMatrix[5*i+j];
                    l++;
                }
            }
        }
        fDetM13 = Determinant4x4(pSubmatrix);
 
        nIgnoreColumn = 3;
        for (int i=1; i<5; i++)
        {
            l = 0;
            for (int j=0; j<5; j++)
            {
                if (j != nIgnoreColumn)
                {
                    pSubmatrix[4*(i-1)+l] = pMatrix[5*i+j];
                    l++;
                }
            }
        }
        fDetM14 = Determinant4x4(pSubmatrix);
 
        nIgnoreColumn = 4;
        for (int i=1; i<5; i++)
        {
            l = 0;
            for (int j=0; j<5; j++)
            {
                if (j != nIgnoreColumn)
                {
                    pSubmatrix[4*(i-1)+l] = pMatrix[5*i+j];
                    l++;
                }
            }
        }
        fDetM15 = Determinant4x4(pSubmatrix);
    */
 
 
    Vec3d pPoints[4];
    Math3d::SetVec(pPoints[0], vPoint1);
    Math3d::SetVec(pPoints[1], vPoint2);
    Math3d::SetVec(pPoints[2], vPoint3);
    Math3d::SetVec(pPoints[3], vPoint4);
 
    // Find determinant M11
    for (int i = 0; i < 4; i++)
    {
        pSubmatrix[4 * i + 0] = pPoints[i].x;
        pSubmatrix[4 * i + 1] = pPoints[i].y;
        pSubmatrix[4 * i + 2] = pPoints[i].z;
        pSubmatrix[4 * i + 3] = 1;
    }
 
    fDetM11 = Determinant4x4(pSubmatrix);
 
    // Find determinant M12
    for (int i = 0; i < 4; i++)
    {
        pSubmatrix[4 * i + 0] =
            pPoints[i].x * pPoints[i].x + pPoints[i].y * pPoints[i].y + pPoints[i].z * pPoints[i].z;
        pSubmatrix[4 * i + 1] = pPoints[i].y;
        pSubmatrix[4 * i + 2] = pPoints[i].z;
        pSubmatrix[4 * i + 3] = 1;
    }
 
    fDetM12 = Determinant4x4(pSubmatrix);
 
    // Find determinant M13
    for (int i = 0; i < 4; i++)
    {
        pSubmatrix[4 * i + 0] = pPoints[i].x;
        pSubmatrix[4 * i + 1] =
            pPoints[i].x * pPoints[i].x + pPoints[i].y * pPoints[i].y + pPoints[i].z * pPoints[i].z;
        pSubmatrix[4 * i + 2] = pPoints[i].z;
        pSubmatrix[4 * i + 3] = 1;
    }
 
    fDetM13 = Determinant4x4(pSubmatrix);
 
    // Find determinant M14
    for (int i = 0; i < 4; i++)
    {
        pSubmatrix[4 * i + 0] = pPoints[i].x;
        pSubmatrix[4 * i + 1] = pPoints[i].y;
        pSubmatrix[4 * i + 2] =
            pPoints[i].x * pPoints[i].x + pPoints[i].y * pPoints[i].y + pPoints[i].z * pPoints[i].z;
        pSubmatrix[4 * i + 3] = 1;
    }
 
    fDetM14 = Determinant4x4(pSubmatrix);
 
    // Find determinant M15
    for (int i = 0; i < 4; i++)
    {
        pSubmatrix[4 * i + 0] =
            pPoints[i].x * pPoints[i].x + pPoints[i].y * pPoints[i].y + pPoints[i].z * pPoints[i].z;
        pSubmatrix[4 * i + 1] = pPoints[i].x;
        pSubmatrix[4 * i + 2] = pPoints[i].y;
        pSubmatrix[4 * i + 3] = pPoints[i].z;
    }
 
    fDetM15 = Determinant4x4(pSubmatrix);
 
 
    //ARMARX_VERBOSE_S << "M11: %.2f, M12: %.2f, M13: %.2f, M14: %.2f, M15: %.2f\n", fDetM11, fDetM12, fDetM13, fDetM14, fDetM15);
 
    vCenter.x = 0.5f * fDetM12 / fDetM11;
    vCenter.y = -0.5f * fDetM13 / fDetM11;
    vCenter.z = 0.5f * fDetM14 / fDetM11;
    fRadius = sqrtf(vCenter.x * vCenter.x + vCenter.y * vCenter.y + vCenter.z * vCenter.z -
                    fDetM15 / fDetM11);
}
 
float
CHypothesisGeneration::Determinant4x4(const float* pMatrix)
{
    float fRet = 0;
    float fDet;
    float pSubMat[9];
 
    for (int n = 0; n < 4; n++)
    {
        // calculate the n-th subdeterminant, develop first row
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0, l = 0; j < 4; j++)
            {
                if (j != n)
                {
                    pSubMat[3 * i + l] = pMatrix[4 * (i + 1) + j];
                    l++;
                }
            }
        }
 
        fDet = pSubMat[0] * pSubMat[4] * pSubMat[8] + pSubMat[1] * pSubMat[5] * pSubMat[6] +
               pSubMat[2] * pSubMat[3] * pSubMat[7] - pSubMat[2] * pSubMat[4] * pSubMat[6] -
               pSubMat[1] * pSubMat[3] * pSubMat[8] - pSubMat[0] * pSubMat[5] * pSubMat[7];
 
        if ((n == 0) || (n == 2))
        {
            fRet += pMatrix[n] * fDet;
        }
        else
        {
            fRet -= pMatrix[n] * fDet;
        }
    }
 
    return fRet;
}
 
bool
CHypothesisGeneration::RANSACSphere(const CVec3dArray& aPointCandidates,
                                    const float fRANSACThreshold,
                                    const float fMaxSphereRadius,
                                    const int nMaxIterations,
                                    CVec3dArray& aResultPoints,
                                    Vec3d& vCenter,
                                    float& fRadius)
{
    const int nPointCandidates = aPointCandidates.GetSize();
 
    aResultPoints.Clear();
 
    if (nPointCandidates < 4)
    {
        ARMARX_WARNING_S << "At least 4 point candidates must be provided for RANSAC for sphere ("
                         << nPointCandidates << " provided)";
        return false;
    }
 
    int nMaxSupport = 0;
    Vec3d vBestCenter;
    float fBestRadius;
 
    int nTemp = (int)(OLP_EFFORT_MODIFICATOR * (nPointCandidates * nPointCandidates + 500));
 
    const int nIterations = (nTemp > nMaxIterations) ? nMaxIterations : nTemp;
 
#pragma omp parallel
    {
#pragma omp for schedule(static, 40)
        for (int i = 0; i < nIterations; i++)
        {
            // identify 4 different points
            srand((rand() % 17) * i + i);
            int nTempIndex;
            const int nFirstIndex = rand() % nPointCandidates;
 
            do
            {
                nTempIndex = rand() % nPointCandidates;
            } while (nTempIndex == nFirstIndex);
 
            const int nSecondIndex = nTempIndex;
 
            do
            {
                nTempIndex = rand() % nPointCandidates;
            } while (nTempIndex == nFirstIndex || nTempIndex == nSecondIndex);
 
            const int nThirdIndex = nTempIndex;
 
            do
            {
                nTempIndex = rand() % nPointCandidates;
            } while (nTempIndex == nFirstIndex || nTempIndex == nSecondIndex ||
                     nTempIndex == nThirdIndex);
 
            const Vec3d& p1 = aPointCandidates[nFirstIndex];
            const Vec3d& p2 = aPointCandidates[nSecondIndex];
            const Vec3d& p3 = aPointCandidates[nThirdIndex];
            const Vec3d& p4 = aPointCandidates[nTempIndex];
 
            //Vec3d p1, p2, p3, p4;
            //p1.x = 1;
            //p1.y = 0;
            //p1.z = 0;
            //p2.x = -1;
            //p2.y = 0;
            //p2.z = 0;
            //p3.x = 0;
            //p3.y = 1;
            //p3.z = 0;
            //p4.x = 0;
            //p4.y = 0;
            //p4.z = 1;
 
            Vec3d vTempCenter;
            float fTempRadius;
 
            CalculateSphere(p1, p2, p3, p4, vTempCenter, fTempRadius);
 
            // if points lie on a plane or three of them on a line, they do not define a valid sphere
            if (fTempRadius == 0)
            {
                continue;
            }
 
            // only accept realistic values
            if ((fTempRadius > 100) || (vTempCenter.z < 100))
            {
                continue;
            }
 
            // count support
            int nSupport = 0;
 
            for (int j = 0; j < nPointCandidates; j++)
            {
                if (fabsf(Math3d::Distance(vTempCenter, aPointCandidates[j]) - fTempRadius) <=
                    fRANSACThreshold)
                {
                    nSupport++;
                }
            }
 
            //ARMARX_VERBOSE_S << "\n(%.1f, %.1f, %.1f) \t (%.1f, %.1f, %.1f) \n(%.1f, %.1f, %.1f) \t (%.1f, %.1f, %.1f)\n", p1.x, p1.y, p1.z, p2.x, p2.y, p2.z, p3.x, p3.y, p3.z, p4.x, p4.y, p4.z);
            //ARMARX_VERBOSE_S << "center: (%.1f, %.1f, %.1f)  radius: %.1f    support: %d\n", vTempCenter.x, vTempCenter.y, vTempCenter.z, fTempRadius, nSupport);
 
            // store if it is the current maximum
            if (nSupport > nMaxSupport)
            {
#pragma omp critical
                if (nSupport > nMaxSupport)
                {
                    nMaxSupport = nSupport;
                    Math3d::SetVec(vBestCenter, vTempCenter);
                    fBestRadius = fTempRadius;
                }
            }
        }
    }
 
    if (nMaxSupport < 4)
    {
        return false;
    }
 
    for (int i = 0; i < nPointCandidates; i++)
    {
        if (fabsf(Math3d::Distance(vBestCenter, aPointCandidates[i]) - fBestRadius) <=
            fRANSACThreshold)
        {
            aResultPoints.AddElement(aPointCandidates[i]);
        }
    }
 
    Math3d::SetVec(vCenter, vBestCenter);
    fRadius = fBestRadius;
 
    return true;
}