OLPTools.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 "OLPTools.h"
#include "ObjectHypothesis.h"
 
 
// IVT
#include <Calibration/StereoCalibration.h>
#include <Calibration/Calibration.h>
#include <Image/ByteImage.h>
#include <Image/ImageProcessor.h>
 
// OpenCV
#include <opencv2/opencv.hpp>
 
 
#include <ArmarXCore/core/logging/Logging.h>
 
 
 
namespace COLPTools
{
    void InterpolateRotation(const Mat3d m1, const Mat3d m2, const float fWeight, Mat3d& mResult)
    {
        Mat3d mTemp1, mTemp2;
        Vec3d vTemp;
        float fRotationAngle;
        Math3d::Transpose(m1, mTemp1);
        Math3d::MulMatMat(m2, mTemp1, mTemp2);
        Math3d::GetAxisAndAngle(mTemp2, vTemp, fRotationAngle);
 
        if (Math3d::ScalarProduct(vTemp, vTemp) < 0.9f) // rotation matrices are identical
        {
            Math3d::SetMat(mResult, m1);
        }
        else
        {
            Math3d::SetRotationMatAxis(mTemp1, vTemp, fWeight * fRotationAngle);
            Math3d::MulMatMat(mTemp1, m1, mResult);
        }
    }
 
 
 
 
    void InterpolateTransformation(const Mat3d m1, const Vec3d v1, const Mat3d m2, const Vec3d v2, const float fWeight, Mat3d& mResult, Vec3d& vResult)
    {
        InterpolateRotation(m1, m2, fWeight, mResult);
        Math3d::AddVecVec(v1, v2, vResult);
        Math3d::MulVecScalar(vResult, fWeight, vResult);
    }
 
 
 
    void ClusterXMeans(const std::vector<CHypothesisPoint*>& aPoints, const int nMinNumClusters, const int nMaxNumClusters, const float fBICFactor, std::vector<std::vector<CHypothesisPoint*> >& aaPointClusters)
    {
        std::vector<Vec3d> aPointsVec3d;
        std::vector<std::vector<Vec3d> > aaPointClustersVec3d;
        std::vector<std::vector<int> > aaOldIndices;
 
        for (size_t i = 0; i < aPoints.size(); i++)
        {
            aPointsVec3d.push_back(aPoints.at(i)->vPosition);
        }
 
        // call pure 3D point clustering
        ClusterXMeans(aPointsVec3d, nMinNumClusters, nMaxNumClusters, fBICFactor, aaPointClustersVec3d, aaOldIndices);
 
        //ARMARX_VERBOSE_S << "%ld clusters\n", aaPointClustersVec3d.size());
 
        // put hypothesis points into clusters according to the indices obtained from the pure 3D point clustering
        aaPointClusters.clear();
 
        for (size_t i = 0; i < aaOldIndices.size(); i++)
        {
            std::vector<CHypothesisPoint*> aNewCluster;
            aaPointClusters.push_back(aNewCluster);
        }
 
        for (size_t i = 0; i < aaOldIndices.size(); i++)
        {
            for (size_t j = 0; j < aaOldIndices.at(i).size(); j++)
            {
                aaPointClusters.at(i).push_back(aPoints.at(aaOldIndices.at(i).at(j))->GetCopy());
            }
        }
    }
 
 
 
 
    void ClusterXMeans(const std::vector<Vec3d>& aPoints, const int nMinNumClusters, const int nMaxNumClusters, const float fBICFactor, std::vector<std::vector<Vec3d> >& aaPointClusters, std::vector<std::vector<int> >& aaOldIndices)
    {
        cv::Mat mSamples;
        cv::Mat mClusterLabels;
        const int nNumberOfDifferentInitialisations = 7; // 5
        cv::TermCriteria tTerminationCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, 0.01);
 
        // copy the points
        const int nNumberOfSamples = aPoints.size();
        mSamples.create(nNumberOfSamples, 3, CV_32FC1);
 
        for (int i = 0; i < nNumberOfSamples; i++)
        {
            mSamples.at<float>(i, 0) = aPoints.at(i).x;
            mSamples.at<float>(i, 1) = aPoints.at(i).y;
            mSamples.at<float>(i, 2) = aPoints.at(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)
        double dMinBIC = FLT_MAX;
        int nOptK = nMinNumClusters;
        const int nMaxNumClustersSafe = (10 * nMaxNumClusters < nNumberOfSamples) ? nMaxNumClusters : nNumberOfSamples / 10;
 
        #pragma omp parallel for schedule(dynamic, 1)
        for (int i = nMaxNumClustersSafe; i >= nMinNumClusters; i--)
        {
            // maybe we need to make parallel instances of mClusterLabels
#ifdef OLP_USE_NEW_OPENCV
            cv::kmeans(mSamples, i, mClusterLabels, tTerminationCriteria, nNumberOfDifferentInitialisations, cv::KMEANS_RANDOM_CENTERS);
#else
            cv::Mat mClusterCenters;
            cv::kmeans(mSamples, i, mClusterLabels, tTerminationCriteria, nNumberOfDifferentInitialisations, cv::KMEANS_PP_CENTERS, &mClusterCenters);
#endif
            double dLogVar, dBIC;
            Vec3d* pvClusterMeans = new Vec3d[nMaxNumClusters];
            double* pdClusterVariances = new double[nMaxNumClusters];
            int* pnClusterSizes =  new int[nMaxNumClusters];
 
            // 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];
            }
 
            const int nNumberOfFreeParameters = (i - 1) + (3 * i) + i;
            //dLogVar = log(dMLVariance);
            dLogVar = log(dMLVariance / i);
            dBIC = fBICFactor * 0.35 * dLogVar + ((double)nNumberOfFreeParameters / (double)nNumberOfSamples) * log((double)nNumberOfSamples); // 0.4  // 1.5 with manual variance
 
            #pragma omp critical
            {
                if (dBIC < dMinBIC)
                {
                    dMinBIC = dBIC;
                    nOptK = i;
                }
            }
 
            delete[] pvClusterMeans;
            delete[] pdClusterVariances;
            delete[] pnClusterSizes;
 
            //ARMARX_VERBOSE_S << "k-means with " << i << " clusters. log(var): " << dLogVar << "   BIC: " << dBIC;
        }
 
        // execute k-means with optimal k
        if (nOptK > 1)
        {
#ifdef OLP_USE_NEW_OPENCV
            double dKMeansCompactness = cv::kmeans(mSamples, nOptK, mClusterLabels, tTerminationCriteria, nNumberOfDifferentInitialisations, cv::KMEANS_RANDOM_CENTERS);
#else
            cv::Mat mClusterCenters;
            double dKMeansCompactness = cv::kmeans(mSamples, nOptK, mClusterLabels, tTerminationCriteria, nNumberOfDifferentInitialisations, cv::KMEANS_PP_CENTERS, &mClusterCenters);
#endif
 
            // copy the points belonging to the clusters
            Vec3d vPoint;
            aaPointClusters.clear();
            aaOldIndices.clear();
 
            for (int i = 0; i < nOptK; i++)
            {
                std::vector<Vec3d> aNewCluster;
                aaPointClusters.push_back(aNewCluster);
                std::vector<int> aClusterIndices;
                aaOldIndices.push_back(aClusterIndices);
            }
 
            for (int i = 0; i < nNumberOfSamples; i++)
            {
                const int nLabel = mClusterLabels.at<int>(i, 0);
 
                if ((nLabel >= 0) && (nLabel < nOptK))
                {
                    vPoint.x = mSamples.at<float>(i, 0);
                    vPoint.y = mSamples.at<float>(i, 1);
                    vPoint.z = mSamples.at<float>(i, 2);
                    aaPointClusters.at(nLabel).push_back(vPoint);
                    aaOldIndices.at(nLabel).push_back(i);
                }
                else
                {
                    ARMARX_WARNING_S << "Invalid cluster label: " <<  nLabel << "nOptK: " << nOptK << ", i: " << i << ", nNumberOfSamples: " << nNumberOfSamples << ", dKMeansCompactness: " << dKMeansCompactness;
                    break;
                }
            }
        }
        else
        {
            aaPointClusters.clear();
            std::vector<Vec3d> aNewCluster;
            aaPointClusters.push_back(aNewCluster);
            aaOldIndices.clear();
            std::vector<int> aClusterIndices;
            aaOldIndices.push_back(aClusterIndices);
            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).push_back(vPoint);
                aaOldIndices.at(0).push_back(i);
            }
        }
    }
 
 
 
 
    void FilterForegroundPoints(const std::vector<CHypothesisPoint*>& aAllPoints, const CByteImage* pForegroundImage, const CCalibration* calibration, std::vector<CHypothesisPoint*>& aForegroundPoints)
    {
        Vec2d vImagePoint;
 
        for (size_t i = 0; i < aAllPoints.size(); i++)
        {
            calibration->WorldToImageCoordinates(aAllPoints.at(i)->vPosition, vImagePoint, false);
            const int nIndex = ((int)vImagePoint.y) * OLP_IMG_WIDTH + (int)vImagePoint.x;
 
            if ((nIndex >= 0) && (nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT))
            {
                if (pForegroundImage->pixels[nIndex] > 0)
                {
                    aForegroundPoints.push_back(aAllPoints.at(i)->GetCopy());
                }
            }
        }
    }
 
 
 
    bool PointIsInForeground(const Vec3d vPoint, const CByteImage* pForegroundImage, const CCalibration* calibration)
    {
        Vec2d vImagePoint;
        calibration->WorldToImageCoordinates(vPoint, vImagePoint, false);
        const int nIndex = ((int)vImagePoint.y) * OLP_IMG_WIDTH + (int)vImagePoint.x;
 
        if ((nIndex >= 0) && (nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT))
        {
            return (pForegroundImage->pixels[nIndex] > 0);
        }
        else
        {
            return false;
        }
    }
 
 
    bool PointIsInForeground(const CHypothesisPoint* pPoint, const CByteImage* pForegroundImage, const CCalibration* calibration)
    {
        return PointIsInForeground(pPoint->vPosition, pForegroundImage, calibration);
    }
 
 
 
 
 
    void CalculateForegroundRatioOfHypothesis(const CObjectHypothesis* pHypothesis, const CByteImage* pForegroundImage, const CCalibration* calibration, float& fForegroundRatio, int& nNumForegroundPixels)
    {
        // get the 2D points
        int nNumPoints;
        Vec2d* pPoints2D;
 
        if (pHypothesis->aVisibleConfirmedPoints.size() > OLP_MIN_NUM_FEATURES)
        {
            nNumPoints = pHypothesis->aVisibleConfirmedPoints.size();
            pPoints2D = new Vec2d[nNumPoints];
 
            for (int i = 0; i < nNumPoints; i++)
            {
                calibration->WorldToImageCoordinates(pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition, pPoints2D[i], false);
            }
        }
        else
        {
            nNumPoints = pHypothesis->aVisibleConfirmedPoints.size() + pHypothesis->aNewPoints.size();
            pPoints2D = new Vec2d[nNumPoints];
 
            for (size_t i = 0; i < pHypothesis->aNewPoints.size(); i++)
            {
                calibration->WorldToImageCoordinates(pHypothesis->aNewPoints.at(i)->vPosition, pPoints2D[i], false);
            }
 
            for (size_t i = 0; i < pHypothesis->aVisibleConfirmedPoints.size(); i++)
            {
                calibration->WorldToImageCoordinates(pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition, pPoints2D[pHypothesis->aNewPoints.size() + i], false);
            }
        }
 
        int nNumForegroundPoints = 0;
        long nForegroundSum = 0;
 
        for (int i = 0; i < nNumPoints; i++)
        {
            int nIndex = (OLP_IMG_WIDTH * (int)pPoints2D[i].y) + (int)pPoints2D[i].x;
 
            if ((nIndex > 0) && (nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT))
            {
                nForegroundSum += pForegroundImage->pixels[nIndex];
 
                if (pForegroundImage->pixels[nIndex] > 0)
                {
                    nNumForegroundPoints++;
                }
            }
        }
 
        nNumForegroundPixels = nNumForegroundPoints;
        fForegroundRatio = (double)nForegroundSum / ((double)nNumPoints * 255.0);
 
 
        /*
        // calculate foreground ratio of enclosing rectangle
 
        // get enclosing rectangle
        Vec2d p1, p2, p3, p4;
        bool bRotated;
        GetEnclosingRectangle(pPoints2d, nNumPoints, true, p1, p2, p3, p4, bRotated);
 
        // make sure points are within image
        p1.x = (p1.x < 0) ? 0 : p1.x;
        p1.x = (p1.x > OLP_IMG_WIDTH-1) ? OLP_IMG_WIDTH-1 : p1.x;
        p1.y = (p1.y < 0) ? 0 : p1.y;
        p1.y = (p1.y > OLP_IMG_HEIGHT-1) ? OLP_IMG_HEIGHT-1 : p1.y;
        p2.x = (p2.x < 0) ? 0 : p2.x;
        p2.x = (p2.x > OLP_IMG_WIDTH-1) ? OLP_IMG_WIDTH-1 : p2.x;
        p2.y = (p2.y < 0) ? 0 : p2.y;
        p2.y = (p2.y > OLP_IMG_HEIGHT-1) ? OLP_IMG_HEIGHT-1 : p2.y;
        p3.x = (p3.x < 0) ? 0 : p3.x;
        p3.x = (p3.x > OLP_IMG_WIDTH-1) ? OLP_IMG_WIDTH-1 : p3.x;
        p3.y = (p3.y < 0) ? 0 : p3.y;
        p3.y = (p3.y > OLP_IMG_HEIGHT-1) ? OLP_IMG_HEIGHT-1 : p3.y;
        p4.x = (p4.x < 0) ? 0 : p4.x;
        p4.x = (p4.x > OLP_IMG_WIDTH-1) ? OLP_IMG_WIDTH-1 : p4.x;
        p4.y = (p4.y < 0) ? 0 : p4.y;
        p4.y = (p4.y > OLP_IMG_HEIGHT-1) ? OLP_IMG_HEIGHT-1 : p4.y;
 
        // count the foreground pixels in the rectangle
        if (!bRotated)
        {
            const int nMinX = (int)p1.x;
            const int nMaxX = (int)p2.x;
            const int nMinY = (int)p1.y;
            const int nMaxY = (int)p3.y;
 
            int nForegroundSum = 0;
            for (int i=nMinY; i<=nMaxY; i++)
            {
                for (int j=nMinX; j<nMaxX; j++)
                {
                    nForegroundSum += pForegroundImage->pixels[i*OLP_IMG_WIDTH+j];
                }
            }
            nNumForegroundPixels = nForegroundSum / 255;
            fForegroundRatio = (float)nNumForegroundPixels / (float)((nMaxX-nMinX)*(nMaxY-nMinY));
        }
        else
        {
            // x ~ (x+y), y ~ (-x+y)  =>
            // p1 is (min, middle), p2 is (middle, min), p3 is (max, middle), p4 is (middle, max)
            nNumForegroundPixels = CountForegroundPixelsInRectangle(p2, p1, p3, p4, pForegroundImage);
            fForegroundRatio = (float)nNumForegroundPixels / ((p3.x-p1.x)*(p4.y-p2.y));
        }
        */
 
        delete[] pPoints2D;
    }
 
 
 
 
    void GetEnclosingRectangle(const Vec2d* pPoints, const int nNumPoints, const bool bUseSecondMaxPoints, Vec2d& p1, Vec2d& p2, Vec2d& p3, Vec2d& p4, bool& bRotated)
    {
        // calculate axis-parallel enclosing rectangle
        float fMinX = pPoints[0].x;
        float fMaxX = pPoints[0].x;
        float fMinY = pPoints[0].y;
        float fMaxY = pPoints[0].y;
        float fSecondMinX = pPoints[0].x;
        float fSecondMaxX = pPoints[0].x;
        float fSecondMinY = pPoints[0].y;
        float fSecondMaxY = pPoints[0].y;
 
        for (int i = 0; i < nNumPoints; i++)
        {
            if (pPoints[i].x < fMinX)
            {
                fSecondMinX = fMinX;
                fMinX = pPoints[i].x;
            }
            else if (pPoints[i].x < fSecondMinX)
            {
                fSecondMinX = pPoints[i].x;
            }
 
            if (pPoints[i].x > fMaxX)
            {
                fSecondMaxX = fMaxX;
                fMaxX = pPoints[i].x;
            }
            else if (pPoints[i].x > fSecondMaxX)
            {
                fSecondMaxX = pPoints[i].x;
            }
 
            if (pPoints[i].y < fMinY)
            {
                fSecondMinY = fMinY;
                fMinY = pPoints[i].y;
            }
            else if (pPoints[i].y < fSecondMinY)
            {
                fSecondMinY = pPoints[i].y;
            }
 
            if (pPoints[i].y > fMaxY)
            {
                fSecondMaxY = fMaxY;
                fMaxY = pPoints[i].y;
            }
            else if (pPoints[i].y > fSecondMaxY)
            {
                fSecondMaxY = pPoints[i].y;
            }
        }
 
        // calculate rectangle that is rotated by 45 degrees
        Vec2d* pRotatedPoints = new Vec2d[nNumPoints];
 
        for (int i = 0; i < nNumPoints; i++)
        {
            pRotatedPoints[i].x = pPoints[i].x - pPoints[i].y;
            pRotatedPoints[i].y = pPoints[i].x + pPoints[i].y;
        }
 
        float fMinXr = pRotatedPoints[0].x;
        float fMaxXr = pRotatedPoints[0].x;
        float fMinYr = pRotatedPoints[0].y;
        float fMaxYr = pRotatedPoints[0].y;
        float fSecondMinXr = pRotatedPoints[0].x;
        float fSecondMaxXr = pRotatedPoints[0].x;
        float fSecondMinYr = pRotatedPoints[0].y;
        float fSecondMaxYr = pRotatedPoints[0].y;
 
        for (int i = 0; i < nNumPoints; i++)
        {
            if (pRotatedPoints[i].x < fMinXr)
            {
                fSecondMinXr = fMinXr;
                fMinXr = pRotatedPoints[i].x;
            }
            else if (pRotatedPoints[i].x < fSecondMinXr)
            {
                fSecondMinXr = pRotatedPoints[i].x;
            }
 
            if (pRotatedPoints[i].x > fMaxXr)
            {
                fSecondMaxXr = fMaxXr;
                fMaxXr = pRotatedPoints[i].x;
            }
            else if (pRotatedPoints[i].x > fSecondMaxXr)
            {
                fSecondMaxXr = pRotatedPoints[i].x;
            }
 
            if (pRotatedPoints[i].y < fMinYr)
            {
                fSecondMinYr = fMinYr;
                fMinYr = pRotatedPoints[i].y;
            }
            else if (pRotatedPoints[i].y < fSecondMinYr)
            {
                fSecondMinYr = pRotatedPoints[i].y;
            }
 
            if (pRotatedPoints[i].y > fMaxYr)
            {
                fSecondMaxYr = fMaxYr;
                fMaxYr = pRotatedPoints[i].y;
            }
            else if (pRotatedPoints[i].y > fSecondMaxYr)
            {
                fSecondMaxYr = pRotatedPoints[i].y;
            }
        }
 
        const float fSqrt2Fact = 0.5f * sqrtf(2.0f);
        fMinXr *= fSqrt2Fact;
        fMaxXr *= fSqrt2Fact;
        fMinYr *= fSqrt2Fact;
        fMaxYr *= fSqrt2Fact;
        fSecondMinXr *= fSqrt2Fact;
        fSecondMaxXr *= fSqrt2Fact;
        fSecondMinYr *= fSqrt2Fact;
        fSecondMaxYr *= fSqrt2Fact;
 
        if (bUseSecondMaxPoints)
        {
            // choose the rectangle with the smaller volume
            if ((fSecondMaxX - fSecondMinX) * (fSecondMaxY - fSecondMinY) < (fSecondMaxXr - fSecondMinXr) * (fSecondMaxYr - fSecondMinYr))
            {
                bRotated = false;
                Math2d::SetVec(p1, fSecondMinX, fSecondMinY);
                Math2d::SetVec(p2, fSecondMaxX, fSecondMinY);
                Math2d::SetVec(p3, fSecondMaxX, fSecondMaxY);
                Math2d::SetVec(p4, fSecondMinX, fSecondMaxY);
            }
            else
            {
                bRotated = true;
                Math2d::SetVec(p1, fSqrt2Fact * (fSecondMinXr + fSecondMinYr), fSqrt2Fact * (-fSecondMinXr + fSecondMinYr));
                Math2d::SetVec(p2, fSqrt2Fact * (fSecondMaxXr + fSecondMinYr), fSqrt2Fact * (-fSecondMaxXr + fSecondMinYr));
                Math2d::SetVec(p3, fSqrt2Fact * (fSecondMaxXr + fSecondMaxYr), fSqrt2Fact * (-fSecondMaxXr + fSecondMaxYr));
                Math2d::SetVec(p4, fSqrt2Fact * (fSecondMinXr + fSecondMaxYr), fSqrt2Fact * (-fSecondMinXr + fSecondMaxYr));
            }
        }
        else
        {
            // choose the rectangle with the smaller volume
            if ((fMaxX - fMinX) * (fMaxY - fMinY) < (fMaxXr - fMinXr) * (fMaxYr - fMinYr))
            {
                bRotated = false;
                Math2d::SetVec(p1, fMinX, fMinY);
                Math2d::SetVec(p2, fMaxX, fMinY);
                Math2d::SetVec(p3, fMaxX, fMaxY);
                Math2d::SetVec(p4, fMinX, fMaxY);
            }
            else
            {
                bRotated = true;
                Math2d::SetVec(p1, fSqrt2Fact * (fMinXr + fMinYr), fSqrt2Fact * (-fMinXr + fMinYr));
                Math2d::SetVec(p2, fSqrt2Fact * (fMaxXr + fMinYr), fSqrt2Fact * (-fMaxXr + fMinYr));
                Math2d::SetVec(p3, fSqrt2Fact * (fMaxXr + fMaxYr), fSqrt2Fact * (-fMaxXr + fMaxYr));
                Math2d::SetVec(p4, fSqrt2Fact * (fMinXr + fMaxYr), fSqrt2Fact * (-fMinXr + fMaxYr));
            }
        }
 
        delete[] pRotatedPoints;
    }
 
 
    int CountForegroundPixelsInRectangle(const Vec2d vMiddleMin, const Vec2d vMinMiddle, const Vec2d vMaxMiddle, const Vec2d vMiddleMax, const CByteImage* pForegroundImage)
    {
        int nStartY = (int)vMiddleMin.y;
        int nEndY = (int)vMiddleMax.y;
        int nMiddleY1, nMiddleY2;
        float fDeltaLeft1, fDeltaLeft2, fDeltaLeft3, fDeltaRight1, fDeltaRight2, fDeltaRight3;
 
        int nSum = 0;
 
        // check if left or right corner comes first
        if (vMinMiddle.y < vMaxMiddle.y)
        {
            nMiddleY1 = (int)vMinMiddle.y;
            nMiddleY2 = (int)vMaxMiddle.y;
            fDeltaLeft1 = (vMinMiddle.x - vMiddleMin.x) / (vMinMiddle.y - vMiddleMin.y);
            fDeltaLeft2 = (vMiddleMax.x - vMinMiddle.x) / (vMiddleMax.y - vMinMiddle.y);
            fDeltaLeft3 = fDeltaLeft2;
            fDeltaRight1 = (vMaxMiddle.x - vMiddleMin.x) / (vMaxMiddle.y - vMiddleMin.y);
            fDeltaRight2 = fDeltaRight1;
            fDeltaRight3 = (vMiddleMax.x - vMaxMiddle.x) / (vMiddleMax.y - vMaxMiddle.y);
        }
        else
        {
            nMiddleY1 = (int)vMaxMiddle.y;
            nMiddleY2 = (int)vMinMiddle.y;
            fDeltaLeft1 = (vMinMiddle.x - vMiddleMin.x) / (vMinMiddle.y - vMiddleMin.y);
            fDeltaLeft2 = fDeltaLeft1;
            fDeltaLeft3 = (vMiddleMax.x - vMinMiddle.x) / (vMiddleMax.y - vMinMiddle.y);
            fDeltaRight1 = (vMaxMiddle.x - vMiddleMin.x) / (vMaxMiddle.y - vMiddleMin.y);
            fDeltaRight2 = (vMiddleMax.x - vMaxMiddle.x) / (vMiddleMax.y - vMaxMiddle.y);
            fDeltaRight3 = fDeltaRight2;
        }
 
        float fLeft = vMiddleMin.x;
        float fRight = vMiddleMin.x;
 
        // go from start to first corner
        for (int i = nStartY; i < nMiddleY1; i++)
        {
            for (int j = (int)fLeft; j <= (int)fRight; j++)
            {
                nSum += pForegroundImage->pixels[i * OLP_IMG_WIDTH + j];
            }
 
            fLeft += fDeltaLeft1;
            fRight += fDeltaRight1;
        }
 
        // first corner to second corner
        for (int i = nMiddleY1; i < nMiddleY2; i++)
        {
            for (int j = (int)fLeft; j <= (int)fRight; j++)
            {
                nSum += pForegroundImage->pixels[i * OLP_IMG_WIDTH + j];
            }
 
            fLeft += fDeltaLeft2;
            fRight += fDeltaRight2;
        }
 
        // second corner to end
        for (int i = nMiddleY2; i < nEndY; i++)
        {
            for (int j = (int)fLeft; j <= (int)fRight; j++)
            {
                nSum += pForegroundImage->pixels[i * OLP_IMG_WIDTH + j];
            }
 
            fLeft += fDeltaLeft3;
            fRight += fDeltaRight3;
        }
 
        return nSum / 255;
    }
 
 
 
 
    void GetMeanAndVariance(const std::vector<CHypothesisPoint*>& pHypothesisPoints, Vec3d& vMean, float& fVariance)
    {
        std::vector<Vec3d> aPoints3d;
 
        for (size_t i = 0; i < pHypothesisPoints.size(); i++)
        {
            aPoints3d.push_back(pHypothesisPoints.at(i)->vPosition);
        }
 
        GetMeanAndVariance(aPoints3d, vMean, fVariance);
    }
 
 
 
 
    void GetMeanAndVariance(const std::vector<Vec3d>& aPoints, Vec3d& vMean, float& fVariance)
    {
        vMean.x = 0;
        vMean.y = 0;
        vMean.z = 0;
        fVariance = 0;
 
        const int nNumPoints = aPoints.size();
 
        if (nNumPoints > 0)
        {
            const float fNumPointsInv = 1.0f / (float)nNumPoints;
 
            // calculate mean
            for (int i = 0; i < nNumPoints; i++)
            {
                vMean.x += aPoints.at(i).x;
                vMean.y += aPoints.at(i).y;
                vMean.z += aPoints.at(i).z;
            }
 
            vMean.x *= fNumPointsInv;
            vMean.y *= fNumPointsInv;
            vMean.z *= fNumPointsInv;
 
            // calculate variance
            for (int i = 0; i < nNumPoints; i++)
            {
                fVariance += (vMean.x - aPoints.at(i).x) * (vMean.x - aPoints.at(i).x)
                             + (vMean.y - aPoints.at(i).y) * (vMean.y - aPoints.at(i).y)
                             + (vMean.z - aPoints.at(i).z) * (vMean.z - aPoints.at(i).z);
            }
 
            fVariance *= fNumPointsInv;
        }
        else
        {
            ARMARX_VERBOSE_S << "GetMeanAndVariance: no points in array!";
        }
    }
 
 
 
 
 
    void RemoveOutliers(std::vector<Vec3d>& aPoints, const float fStdDevFactor, std::vector<int>* pOldIndices)
    {
        if (aPoints.size() < 9)
        {
            if (pOldIndices != NULL)
            {
                pOldIndices->clear();
 
                for (size_t i = 0; i < aPoints.size(); i++)
                {
                    pOldIndices->push_back(i);
                }
            }
 
            return;
        }
 
        Vec3d vMean;
        float fVariance;
        GetMeanAndVariance(aPoints, vMean, fVariance);
        const float fThreshold2 = fStdDevFactor * fStdDevFactor * fVariance;
 
        ARMARX_VERBOSE_S << "Mean: (" << vMean.x << ", " << vMean.y << ", " << vMean.z << "), std dev: " << sqrtf(fVariance);
 
        // remove all points that are further away from the mean than fStdDevFactor x standard deviation
        float fDist2;
 
        if (pOldIndices != NULL)
        {
            pOldIndices->clear();
 
            for (size_t i = 0; i < aPoints.size(); i++)
            {
                pOldIndices->push_back(i);
            }
 
            for (size_t i = 0; i < aPoints.size(); i++)
            {
                fDist2 = (vMean.x - aPoints.at(i).x) * (vMean.x - aPoints.at(i).x)
                         + (vMean.y - aPoints.at(i).y) * (vMean.y - aPoints.at(i).y)
                         + (vMean.z - aPoints.at(i).z) * (vMean.z - aPoints.at(i).z);
 
                if (fDist2 > fThreshold2)
                {
                    Math3d::SetVec(aPoints.at(i), aPoints.at(aPoints.size() - 1));
                    pOldIndices->at(i) = pOldIndices->at(aPoints.size() - 1);
                    aPoints.pop_back();
                    pOldIndices->pop_back();
                    i--;
                }
            }
        }
        else
        {
            for (size_t i = 0; i < aPoints.size(); i++)
            {
                fDist2 = (vMean.x - aPoints.at(i).x) * (vMean.x - aPoints.at(i).x)
                         + (vMean.y - aPoints.at(i).y) * (vMean.y - aPoints.at(i).y)
                         + (vMean.z - aPoints.at(i).z) * (vMean.z - aPoints.at(i).z);
 
                if (fDist2 > fThreshold2)
                {
                    aPoints[i] = aPoints[aPoints.size() - 1];
                    aPoints.pop_back();
                    i--;
                }
            }
        }
    }
 
 
 
    void RemoveOutliers(std::vector<CHypothesisPoint*>& aPoints, const float fStdDevFactor)
    {
        std::vector<Vec3d> aPointsVec3d;
        std::vector<int> pOldIndices;
 
        for (size_t i = 0; i < aPoints.size(); i++)
        {
            aPointsVec3d.push_back(aPoints.at(i)->vPosition);
        }
 
        RemoveOutliers(aPointsVec3d, fStdDevFactor, &pOldIndices);
 
        ARMARX_VERBOSE_S << "RemoveOutliers: " << pOldIndices.size() << " of " << aPoints.size() << " points left\n";
 
        // keep only the points with index contained in pOldIndices
 
        std::vector<CHypothesisPoint*> aPointsCopy;
        std::vector<bool> aPointStillIncluded;
 
        for (size_t i = 0; i < aPoints.size(); i++)
        {
            aPointsCopy.push_back(aPoints.at(i));
            aPointStillIncluded.push_back(false);
        }
 
        for (size_t i = 0; i < pOldIndices.size(); i++)
        {
            aPointStillIncluded.at(pOldIndices.at(i)) = true;
        }
 
        aPoints.clear();
 
        for (size_t i = 0; i < aPointsCopy.size(); i++)
        {
            if (aPointStillIncluded.at(i))
            {
                aPoints.push_back(aPointsCopy.at(i));
            }
            else
            {
                delete aPointsCopy.at(i);
            }
        }
    }
 
 
 
    bool CheckIfPointIsAlreadyInHypothesis(const Vec3d vPoint, const CObjectHypothesis* pHypothesis)
    {
        float fMinDist = FLT_MAX;
        float fDist;
 
        for (size_t i = 0; i < pHypothesis->aConfirmedPoints.size(); i++)
        {
            fDist = Math3d::Distance(vPoint, pHypothesis->aConfirmedPoints.at(i)->vPosition);
 
            if (fDist < fMinDist)
            {
                fMinDist = fDist;
            }
        }
 
        for (size_t i = 0; i < pHypothesis->aDoubtablePoints.size(); i++)
        {
            fDist = Math3d::Distance(vPoint, pHypothesis->aDoubtablePoints.at(i)->vPosition);
 
            if (fDist < fMinDist)
            {
                fMinDist = fDist;
            }
        }
 
        return (fMinDist < 0.1f * OLP_TOLERANCE_CONCURRENT_MOTION);
    }
 
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ConfirmedPointsToPCL(const CObjectHypothesis* pHypothesis)
    {
        pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
        cloud->resize(pHypothesis->aConfirmedPoints.size());
 
        Vec3d vCenter = pHypothesis->vCenter;
 
        for (size_t i = 0; i < pHypothesis->aConfirmedPoints.size(); i++)
        {
            const CHypothesisPoint* p = pHypothesis->aConfirmedPoints.at(i);
 
            cloud->points[i].x = p->vPosition.x - vCenter.x;
            cloud->points[i].y = p->vPosition.y - vCenter.y;
            cloud->points[i].z = p->vPosition.z - vCenter.z;
            cloud->points[i].r = (int)(255.0f * p->fIntensity * p->fColorR);
            cloud->points[i].g = (int)(255.0f * p->fIntensity * p->fColorG);
            cloud->points[i].b = (int)(255.0f * p->fIntensity * p->fColorB);
            cloud->points[i].a = 1;
        }
 
        return cloud;
    }
 
    // stolen from IVT: ImageProcessor
    // (1 << 20) / i
    static const int division_table[] =
    {
        0, 1048576, 524288, 349525, 262144, 209715, 174762, 149796,
        131072, 116508, 104857, 95325, 87381, 80659, 74898, 69905,
        65536, 61680, 58254, 55188, 52428, 49932, 47662, 45590,
        43690, 41943, 40329, 38836, 37449, 36157, 34952, 33825,
        32768, 31775, 30840, 29959, 29127, 28339, 27594, 26886,
        26214, 25575, 24966, 24385, 23831, 23301, 22795, 22310,
        21845, 21399, 20971, 20560, 20164, 19784, 19418, 19065,
        18724, 18396, 18078, 17772, 17476, 17189, 16912, 16644,
        16384, 16131, 15887, 15650, 15420, 15196, 14979, 14768,
        14563, 14364, 14169, 13981, 13797, 13617, 13443, 13273,
        13107, 12945, 12787, 12633, 12483, 12336, 12192, 12052,
        11915, 11781, 11650, 11522, 11397, 11275, 11155, 11037,
        10922, 10810, 10699, 10591, 10485, 10381, 10280, 10180,
        10082, 9986, 9892, 9799, 9709, 9619, 9532, 9446,
        9362, 9279, 9198, 9118, 9039, 8962, 8886, 8811,
        8738, 8665, 8594, 8525, 8456, 8388, 8322, 8256,
        8192, 8128, 8065, 8004, 7943, 7884, 7825, 7767,
        7710, 7653, 7598, 7543, 7489, 7436, 7384, 7332,
        7281, 7231, 7182, 7133, 7084, 7037, 6990, 6944,
        6898, 6853, 6808, 6765, 6721, 6678, 6636, 6594,
        6553, 6512, 6472, 6432, 6393, 6355, 6316, 6278,
        6241, 6204, 6168, 6132, 6096, 6061, 6026, 5991,
        5957, 5924, 5890, 5857, 5825, 5793, 5761, 5729,
        5698, 5667, 5637, 5607, 5577, 5548, 5518, 5489,
        5461, 5433, 5405, 5377, 5349, 5322, 5295, 5269,
        5242, 5216, 5190, 5165, 5140, 5115, 5090, 5065,
        5041, 5017, 4993, 4969, 4946, 4922, 4899, 4877,
        4854, 4832, 4809, 4788, 4766, 4744, 4723, 4702,
        4681, 4660, 4639, 4619, 4599, 4578, 4559, 4539,
        4519, 4500, 4481, 4462, 4443, 4424, 4405, 4387,
        4369, 4350, 4332, 4315, 4297, 4279, 4262, 4245,
        4228, 4211, 4194, 4177, 4161, 4144, 4128, 4112
    };
 
 
 
    void ConvertRGB2HSV(const unsigned char r, const unsigned char g, const unsigned char b, unsigned char& h, unsigned char& s, unsigned char& v)
    {
        // stolen from IVT: ImageProcessor
        const int max = MY_MAX(MY_MAX(r, g), b);
        const int min = MY_MIN(MY_MIN(r, g), b);
        const int delta = max - min;
 
        // unoptimized: 30 * (g - b) / delta (etc.)
        int hInt;
 
        if (r == max)
        {
            hInt = g > b ? 180 + ((30 * (g - b) * division_table[delta]) >> 20) : 180 - ((30 * (b - g) * division_table[delta]) >> 20);
        }
        else if (g == max)
        {
            hInt = b > r ? 60 + ((30 * (b - r) * division_table[delta]) >> 20) : 60 - ((30 * (r - b) * division_table[delta]) >> 20);
        }
        else
        {
            hInt = r > g ? 120 + ((30 * (r - g) * division_table[delta]) >> 20) : 120 - ((30 * (g - r) * division_table[delta]) >> 20);
        }
 
        h = (hInt >= 180) ? hInt - 180 : hInt;
 
        // unoptimized: delta * 255 / max;
        s = (255 * delta * division_table[max]) >> 20;
 
        v = max;
    }
 
 
 
 
    void CreateHueAndSaturationHistogram(const CObjectHypothesis* pHypothesis, std::vector<float>& aHueHistogram, std::vector<float>& aSaturationHistogram)
    {
        if ((pHypothesis->eType != CObjectHypothesis::eRGBD) && (pHypothesis->eType != CObjectHypothesis::eSingleColored) && false)
        {
            ARMARX_WARNING_S << "CreateHueHistogram: wrong hypothesis type (not RGBD or SingleColored)!";
            return;
        }
 
        aHueHistogram.resize(OLP_SIZE_OBJECT_HUE_HISTOGRAM);
        aSaturationHistogram.resize(OLP_SIZE_OBJECT_HUE_HISTOGRAM);
        const int nBucketSize = (int)(ceil(256.0f / (float)OLP_SIZE_OBJECT_HUE_HISTOGRAM));
 
        unsigned char r, g, b, h, s, v;
 
        // add the values for the confirmed points
        for (size_t i = 0; i < pHypothesis->aConfirmedPoints.size(); i++)
        {
            CHypothesisPoint* pPoint = pHypothesis->aConfirmedPoints.at(i);
            r = (unsigned char)(255.0f * pPoint->fColorR * pPoint->fIntensity);
            g = (unsigned char)(255.0f * pPoint->fColorG * pPoint->fIntensity);
            b = (unsigned char)(255.0f * pPoint->fColorB * pPoint->fIntensity);
            COLPTools::ConvertRGB2HSV(r, g, b, h, s, v);
 
            // saturation-weighted hue values
            const float fWeight = 1.0f - 1.0f / (1.0f + 0.0025f * (s * s)); // 0.5 when s=20
            const int nHistogramIndexHue = h / nBucketSize;
            aHueHistogram.at(nHistogramIndexHue) += fWeight;
            const int nHistogramIndexSaturation = s / nBucketSize;
            aSaturationHistogram.at(nHistogramIndexSaturation) += 1;
        }
 
        // add the values for the candidate points
        const float fCandidatePointWeightFactor = 0.01f; // smaller weight for candidate points
 
        for (size_t i = 0; i < pHypothesis->aNewPoints.size(); i++)
        {
            CHypothesisPoint* pPoint = pHypothesis->aNewPoints.at(i);
            r = (unsigned char)(255.0f * pPoint->fColorR * pPoint->fIntensity);
            g = (unsigned char)(255.0f * pPoint->fColorG * pPoint->fIntensity);
            b = (unsigned char)(255.0f * pPoint->fColorB * pPoint->fIntensity);
            COLPTools::ConvertRGB2HSV(r, g, b, h, s, v);
 
            // saturation-weighted hue values
            const float fWeight = 1.0f - 1.0f / (1.0f + 0.0025f * (s * s));
            const int nHistogramIndexHue = h / nBucketSize;
            aHueHistogram.at(nHistogramIndexHue) += fCandidatePointWeightFactor * fWeight;
            const int nHistogramIndexSaturation = s / nBucketSize;
            aSaturationHistogram.at(nHistogramIndexSaturation) += fCandidatePointWeightFactor;
        }
 
        SmoothHistogram(aHueHistogram);
        NormalizeHistogram(aHueHistogram);
        SmoothHistogram(aSaturationHistogram);
        NormalizeHistogram(aSaturationHistogram);
    }
 
 
 
    void CreateHueAndSaturationHistogramInWindow(const CByteImage* pHSVImage, const int nMinX, const int nMinY, const int nMaxX, const int nMaxY,
            std::vector<float>& aHueHistogram, std::vector<float>& aSaturationHistogram)
    {
        aHueHistogram.resize(OLP_SIZE_OBJECT_HUE_HISTOGRAM);
        aSaturationHistogram.resize(OLP_SIZE_OBJECT_HUE_HISTOGRAM);
        const int nBucketSize = (int)(ceil(256.0f / (float)OLP_SIZE_OBJECT_HUE_HISTOGRAM));
 
        // accumulate histogram
        unsigned char h, s;
 
        for (int i = nMinY; i < nMaxY; i++)
        {
            for (int j = nMinX; j < nMaxX; j++)
            {
                h = pHSVImage->pixels[3 * (i * OLP_IMG_WIDTH + j)];
                s = pHSVImage->pixels[3 * (i * OLP_IMG_WIDTH + j) + 1];
                // saturation-weighted hue values
                const float fWeight = 1.0f - 1.0f / (1.0f + 0.0025f * (s * s)); // 0.5 when s=20
                const int nHistogramIndexHue = h / nBucketSize;
                aHueHistogram.at(nHistogramIndexHue) += fWeight;
                const int nHistogramIndexSaturation = s / nBucketSize;
                aSaturationHistogram.at(nHistogramIndexSaturation) += 1;
            }
        }
 
        SmoothHistogram(aHueHistogram);
        NormalizeHistogram(aHueHistogram);
        SmoothHistogram(aSaturationHistogram);
        NormalizeHistogram(aSaturationHistogram);
    }
 
 
 
 
    void SmoothHistogram(std::vector<float>& aHistogram)
    {
        std::vector<float> aHistogramTemp;
        const int nHistogramSize = aHistogram.size();
        aHistogramTemp.resize(nHistogramSize);
 
        for (int i = 0; i < nHistogramSize; i++)
        {
            aHistogramTemp.at(i) = aHistogram.at(i);
        }
 
        // smooth the histogram
        for (int i = 0; i < nHistogramSize; i++)
        {
            //            aHistogram.at(i) =  0.4f * aHistogramTemp.at(i)
            //                              + 0.2f * aHistogramTemp.at((i-1+nHistogramSize)%nHistogramSize)
            //                              + 0.2f * aHistogramTemp.at((i+1)%nHistogramSize)
            //                              + 0.1f * aHistogramTemp.at((i-2+nHistogramSize)%nHistogramSize)
            //                              + 0.1f * aHistogramTemp.at((i+2)%nHistogramSize);
            aHistogram.at(i) =  0.7f * aHistogramTemp.at(i)
                                + 0.15f * aHistogramTemp.at((i - 1 + nHistogramSize) % nHistogramSize)
                                + 0.15f * aHistogramTemp.at((i + 1) % nHistogramSize);
        }
    }
 
 
 
    void NormalizeHistogram(std::vector<float>& aHistogram)
    {
        float fSum = 0;
 
        for (size_t i = 0; i < aHistogram.size(); i++)
        {
            fSum += aHistogram.at(i);
        }
 
        const float fInverseSum = 1.0f / fSum;
 
        for (size_t i = 0; i < aHistogram.size(); i++)
        {
            aHistogram.at(i) *= fInverseSum;
        }
    }
 
 
 
 
    float GetHistogramDistanceL1(const std::vector<float>& aHistogram1, const std::vector<float>& aHistogram2)
    {
        const int n = (int)aHistogram1.size();
 
        float fDist = 0;
 
        for (int i = 0; i < n; i++)
        {
            fDist += fabsf(aHistogram1.at(i) - aHistogram2.at(i));
        }
 
        return fDist;
    }
 
 
    float GetHistogramDistanceL2(const std::vector<float>& aHistogram1, const std::vector<float>& aHistogram2)
    {
        const int n = (int)aHistogram1.size();
 
        float fDist = 0;
 
        for (int i = 0; i < n; i++)
        {
            fDist += (aHistogram1.at(i) - aHistogram2.at(i)) * (aHistogram1.at(i) - aHistogram2.at(i));
        }
 
        return sqrtf(fDist);
    }
 
 
    float GetHistogramDistanceX2(const std::vector<float>& aHistogram1, const std::vector<float>& aHistogram2)
    {
        const int n = (int)aHistogram1.size();
 
        float fDist = 0;
 
        for (int i = 0; i < n; i++)
        {
            fDist += (aHistogram1.at(i) - aHistogram2.at(i)) * (aHistogram1.at(i) - aHistogram2.at(i)) / (aHistogram1.at(i) + aHistogram2.at(i) + 0.00001f);
        }
 
        return sqrtf(fDist);
    }
 
 
 
 
    void DrawCross(CByteImage* pGreyImage, int x, int y, int nBrightness)
    {
        if (x > 2 && y > 2 && x < OLP_IMG_WIDTH - 3 && y < OLP_IMG_HEIGHT - 3)
        {
            for (int j = -3; j <= 3; j++)
            {
                pGreyImage->pixels[(y + j)*OLP_IMG_WIDTH + x] = nBrightness;
                pGreyImage->pixels[y * OLP_IMG_WIDTH + x + j] = nBrightness;
            }
        }
    }
 
 
    void DrawCross(CByteImage* pColorImage, int x, int y, int r, int g, int b)
    {
        if (x > 2 && y > 2 && x < OLP_IMG_WIDTH - 3 && y < OLP_IMG_HEIGHT - 3)
        {
            for (int j = -3; j <= 3; j++)
            {
                pColorImage->pixels[3 * ((y + j)*OLP_IMG_WIDTH + x)] = r;
                pColorImage->pixels[3 * ((y + j)*OLP_IMG_WIDTH + x) + 1] = g;
                pColorImage->pixels[3 * ((y + j)*OLP_IMG_WIDTH + x) + 2] = b;
 
                pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j)] = r;
                pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j) + 1] = g;
                pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j) + 2] = b;
            }
        }
    }
 
 
    void DrawFatCross(CByteImage* pColorImage, int x, int y, int r, int g, int b)
    {
        if (x > 2 && y > 2 && x < OLP_IMG_WIDTH - 5 && y < OLP_IMG_HEIGHT - 5)
        {
            for (int j = -3; j <= 4; j++)
            {
                pColorImage->pixels[3 * ((y + j)*OLP_IMG_WIDTH + x)] = r;
                pColorImage->pixels[3 * ((y + j)*OLP_IMG_WIDTH + x) + 1] = g;
                pColorImage->pixels[3 * ((y + j)*OLP_IMG_WIDTH + x) + 2] = b;
 
                pColorImage->pixels[3 * ((y + j)*OLP_IMG_WIDTH + x + 1)] = r;
                pColorImage->pixels[3 * ((y + j)*OLP_IMG_WIDTH + x + 1) + 1] = g;
                pColorImage->pixels[3 * ((y + j)*OLP_IMG_WIDTH + x + 1) + 2] = b;
 
 
                pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j)] = r;
                pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j) + 1] = g;
                pColorImage->pixels[3 * (y * OLP_IMG_WIDTH + x + j) + 2] = b;
 
                pColorImage->pixels[3 * ((y + 1)*OLP_IMG_WIDTH + x + j)] = r;
                pColorImage->pixels[3 * ((y + 1)*OLP_IMG_WIDTH + x + j) + 1] = g;
                pColorImage->pixels[3 * ((y + 1)*OLP_IMG_WIDTH + x + j) + 2] = b;
            }
        }
    }
 
 
 
 
    void CreateObjectSegmentationMask(const CObjectHypothesis* pHypothesis, const CCalibration* calibration, CByteImage*& pForegroundImage)
    {
        if (pForegroundImage == NULL)
        {
            pForegroundImage = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
        }
        else if (pForegroundImage->width != OLP_IMG_WIDTH || pForegroundImage->height != OLP_IMG_HEIGHT || pForegroundImage->type != CByteImage::eGrayScale)
        {
            delete pForegroundImage;
            pForegroundImage = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
        }
 
        // reset pixels
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            pForegroundImage->pixels[i] = 0;
        }
 
 
        // pixels belonging to the hypothesis are foreground
        for (size_t i = 0; i < pHypothesis->aVisibleConfirmedPoints.size(); i++)
        {
            Vec2d vPoint2D;
            calibration->WorldToImageCoordinates(pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition, vPoint2D, false);
            int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
 
            if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
            {
                pForegroundImage->pixels[nIndex] = 255;
            }
        }
 
        if (pHypothesis->aVisibleConfirmedPoints.size() < OLP_MIN_NUM_FEATURES)
        {
            for (size_t i = 0; i < pHypothesis->aNewPoints.size(); i++)
            {
                Vec2d vPoint2D;
                calibration->WorldToImageCoordinates(pHypothesis->aNewPoints.at(i)->vPosition, vPoint2D, false);
                int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
 
                if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
                {
                    pForegroundImage->pixels[nIndex] = 255;
                }
            }
        }
 
 
        // apply morphological operators to cover the free space between the points and close holes
        CByteImage* pTempImage = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
 
        for (int i = 0; i < OLP_DEPTH_MAP_PIXEL_DISTANCE + 1; i++)
        {
            ImageProcessor::Dilate(pForegroundImage, pTempImage);
            ImageProcessor::Dilate(pTempImage, pForegroundImage);
        }
        //ImageProcessor::Dilate(pForegroundImage, pTempImage);
        for (int i = 0; i < OLP_DEPTH_MAP_PIXEL_DISTANCE + 1; i++)
        {
            ImageProcessor::Erode(pForegroundImage, pTempImage);
            ImageProcessor::Erode(pTempImage, pForegroundImage);
        }
        //ImageProcessor::CopyImage(pTempImage, pForegroundImage);
 
        delete pTempImage;
 
        //pForegroundImage->SaveToFile("/home/staff/schieben/datalog/objseg.bmp");
    }
 
 
    void CreateSegmentationProbabilityMap(const CObjectHypothesis* pHypothesis, const CCalibration* calibration, CByteImage*& pProbabilityImage)
    {
        if (pProbabilityImage == NULL)
        {
            pProbabilityImage = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
 
        }
 
        // reset pixels
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            pProbabilityImage->pixels[i] = 0;
        }
 
        // pixels belonging to the hypothesis are foreground
        const int probValueConfirmedPoints = 255;
        const int probValueCandidatePoints = 51;
        Vec2d vPoint2D;
 
        for (size_t i = 0; i < pHypothesis->aNewPoints.size(); i++)
        {
            calibration->WorldToImageCoordinates(pHypothesis->aNewPoints.at(i)->vPosition, vPoint2D, false);
            int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
 
            if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
            {
                pProbabilityImage->pixels[nIndex] = probValueCandidatePoints;
            }
        }
 
        for (size_t i = 0; i < pHypothesis->aVisibleConfirmedPoints.size(); i++)
        {
            calibration->WorldToImageCoordinates(pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition, vPoint2D, false);
            int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
 
            if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
            {
                pProbabilityImage->pixels[nIndex] = probValueConfirmedPoints;
            }
        }
 
        CByteImage* tempImage = new CByteImage(pProbabilityImage);
        const float sigma = OLP_DEPTH_MAP_PIXEL_DISTANCE + 1;
        const int nKernelSize = 4 * sigma + 1; // >= 4*sigma+1
        ImageProcessor::GaussianSmooth(pProbabilityImage, tempImage, sigma * sigma, nKernelSize);
        ImageProcessor::HistogramStretching(tempImage, pProbabilityImage, 0.0f, 1.0f);
        ImageProcessor::GaussianSmooth(pProbabilityImage, tempImage, sigma * sigma, nKernelSize);
        ImageProcessor::HistogramStretching(tempImage, pProbabilityImage, 0.0f, 1.0f);
 
        //        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        //        {
        //            pTempImage->pixels[i] = (pTempImage->pixels[i] > 50) ? 255 : 5 * pTempImage->pixels[i];
        //        }
        //        ImageProcessor::GaussianSmooth(pTempImage, pProbabilityImage, sigma * sigma, nKernelSize);
        delete tempImage;
    }
 
 
 
    void SetNumberInFileName(std::string& sFileName, int nNumber, int nNumDigits)
    {
        for (int i = 0; i < nNumDigits; i++)
        {
            int nDecimalDivisor = 1;
 
            for (int j = 0; j < i; j++)
            {
                nDecimalDivisor *= 10;
            }
 
            sFileName.at(sFileName.length() - (5 + i)) = '0' + (nNumber / nDecimalDivisor) % 10;
        }
    }
 
 
 
    float GetHypothesesIntersectionRatio(const CObjectHypothesis* pHypothesis1, const CObjectHypothesis* pHypothesis2, const CCalibration* calibration)
    {
        CByteImage* pHypothesisRegionImage1 = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
        CByteImage* pHypothesisRegionImage2 = new CByteImage(OLP_IMG_WIDTH, OLP_IMG_HEIGHT, CByteImage::eGrayScale);
        CreateObjectSegmentationMask(pHypothesis1, calibration, pHypothesisRegionImage1);
        CreateObjectSegmentationMask(pHypothesis2, calibration, pHypothesisRegionImage2);
 
        int nIntersectingPoints = 0;
        int nUnionPoints = 0;
 
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            nIntersectingPoints += (pHypothesisRegionImage1->pixels[i] * pHypothesisRegionImage2->pixels[i] != 0);
            nUnionPoints += (pHypothesisRegionImage1->pixels[i] + pHypothesisRegionImage2->pixels[i] != 0);
        }
 
        delete pHypothesisRegionImage1;
        delete pHypothesisRegionImage2;
 
        if (nUnionPoints > 0)
        {
            return (float)nIntersectingPoints / (float)nUnionPoints;
        }
        else
        {
            return 0;
        }
    }
 
 
    void DrawHypothesis(CObjectHypothesis* pHypothesis, const CCalibration* calibration, CByteImage* pImage, const int nNumFusedHypotheses)
    {
        // reorder points to draw closest ones last
        SortByPositionZ(pHypothesis->aVisibleConfirmedPoints);
        // invert order to decreasing
        CHypothesisPoint* pTempPoint;
        const int nSize = pHypothesis->aVisibleConfirmedPoints.size();
 
        for (int i = 0; 2 * i < nSize; i++)
        {
            pTempPoint = pHypothesis->aVisibleConfirmedPoints.at(i);
            pHypothesis->aVisibleConfirmedPoints.at(i) = pHypothesis->aVisibleConfirmedPoints.at(nSize - i - 1);
            pHypothesis->aVisibleConfirmedPoints.at(nSize - i - 1) = pTempPoint;
        }
 
        ImageProcessor::Zero(pImage);
 
        for (size_t i = 0; i < pHypothesis->aVisibleConfirmedPoints.size(); i++)
        {
            Vec2d vPoint2D;
            calibration->WorldToImageCoordinates(pHypothesis->aVisibleConfirmedPoints.at(i)->vPosition, vPoint2D, false);
            int nIndex = (int)vPoint2D.y * OLP_IMG_WIDTH + (int)vPoint2D.x;
 
            if (0 <= nIndex && nIndex < OLP_IMG_WIDTH * OLP_IMG_HEIGHT)
            {
                pImage->pixels[3 * nIndex] = 255 * pHypothesis->aVisibleConfirmedPoints.at(i)->fIntensity * pHypothesis->aVisibleConfirmedPoints.at(i)->fColorR;
                pImage->pixels[3 * nIndex + 1] = 255 * pHypothesis->aVisibleConfirmedPoints.at(i)->fIntensity * pHypothesis->aVisibleConfirmedPoints.at(i)->fColorG;
                pImage->pixels[3 * nIndex + 2] = 255 * pHypothesis->aVisibleConfirmedPoints.at(i)->fIntensity * pHypothesis->aVisibleConfirmedPoints.at(i)->fColorB;
            }
        }
 
        // fill holes
        CByteImage* pTempImg = new CByteImage(pImage);
        FillHolesRGB(pImage, pTempImg, 2);
        FillHolesRGB(pTempImg, pImage, 2);
        delete pTempImg;
 
        // remove regions with low foreground probability
        CByteImage* pSegmentationProbability = NULL;
        CreateSegmentationProbabilityMap(pHypothesis, calibration, pSegmentationProbability);
        std::string sFileName = OLP_OBJECT_LEARNING_DIR;
        sFileName += "SegmProb.bmp";
        pSegmentationProbability->SaveToFile(sFileName.c_str());
 
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            if (pSegmentationProbability->pixels[i] < 1 + nNumFusedHypotheses / 3)
            {
                pImage->pixels[3 * i] *= 0.3;
                pImage->pixels[3 * i + 1] *= 0.3;
                pImage->pixels[3 * i + 2] *= 0.3;
            }
        }
    }
 
 
    void FillHolesRGB(const CByteImage* pInputImage, CByteImage* pOutputImage, const int nRadius)
    {
        for (int i = 0; i < 3 * OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            pOutputImage->pixels[i] = pInputImage->pixels[i];
        }
 
        for (int i = nRadius; i < OLP_IMG_HEIGHT - nRadius; i++)
        {
            for (int j = nRadius; j < OLP_IMG_WIDTH - nRadius; j++)
            {
                int nIndex = i * OLP_IMG_WIDTH + j;
 
                if (pInputImage->pixels[3 * nIndex] + pInputImage->pixels[3 * nIndex + 1] + pInputImage->pixels[3 * nIndex + 2] == 0)
                {
                    int r, g, b, nNumPixels;
                    r = g = b = nNumPixels = 0;
 
                    for (int l = -nRadius; l <= nRadius; l++)
                    {
                        for (int k = -nRadius; k <= nRadius; k++)
                        {
                            int nTempIndex = 3 * (nIndex + l * OLP_IMG_WIDTH + k);
 
                            if (pInputImage->pixels[nTempIndex] + pInputImage->pixels[nTempIndex + 1] + pInputImage->pixels[nTempIndex + 2] != 0)
                            {
                                r += pInputImage->pixels[nTempIndex];
                                g += pInputImage->pixels[nTempIndex + 1];
                                b += pInputImage->pixels[nTempIndex + 2];
                                nNumPixels++;
                            }
                        }
                    }
 
                    if (nNumPixels > 0)
                    {
                        pOutputImage->pixels[3 * nIndex] = r / nNumPixels;
                        pOutputImage->pixels[3 * nIndex + 1] = g / nNumPixels;
                        pOutputImage->pixels[3 * nIndex + 2] = b / nNumPixels;
                    }
                }
            }
        }
    }
 
 
    void FillHolesGray(const CByteImage* pInputImage, CByteImage* pOutputImage, const int nRadius)
    {
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            pOutputImage->pixels[i] = pInputImage->pixels[i];
        }
 
        for (int i = nRadius; i < OLP_IMG_HEIGHT - nRadius; i++)
        {
            for (int j = nRadius; j < OLP_IMG_WIDTH - nRadius; j++)
            {
                int nIndex = i * OLP_IMG_WIDTH + j;
 
                if (pInputImage->pixels[nIndex] == 0)
                {
                    int nSum = 0;
                    int nNumPixels = 0;
 
                    for (int l = -nRadius; l <= nRadius; l++)
                    {
                        for (int k = -nRadius; k <= nRadius; k++)
                        {
                            int nTempIndex = nIndex + l * OLP_IMG_WIDTH + k;
 
                            if (pInputImage->pixels[nTempIndex] != 0)
                            {
                                nSum += pInputImage->pixels[nTempIndex];
                                nNumPixels++;
                            }
                        }
                    }
 
                    if (nNumPixels > 0)
                    {
                        pOutputImage->pixels[nIndex] = nSum / nNumPixels;
                    }
                }
            }
        }
    }
 
 
    // Quicksort from slide of Prof. Sanders
    void SortByPositionZ(std::vector<CHypothesisPoint*>& aHypothesisPoints, const int nLeftLimit, const int nRightLimit)
    {
        if (aHypothesisPoints.size() <= 1 || nRightLimit - nLeftLimit <= 0)
        {
            return;
        }
 
        float fPivot = aHypothesisPoints.at((nLeftLimit + nRightLimit) / 2)->vPosition.z;
 
        int nLeftIndex = nLeftLimit;
        int nRightIndex = nRightLimit;
 
        while (nLeftIndex <= nRightIndex)
        {
            while (aHypothesisPoints.at(nLeftIndex)->vPosition.z < fPivot && (nLeftIndex <= nRightIndex))
            {
                nLeftIndex++;
            }
 
            while (aHypothesisPoints.at(nRightIndex)->vPosition.z > fPivot && (nLeftIndex <= nRightIndex))
            {
                nRightIndex--;
            }
 
            if (nLeftIndex <= nRightIndex)
            {
                CHypothesisPoint* pTemp = aHypothesisPoints.at(nLeftIndex);
                aHypothesisPoints.at(nLeftIndex) = aHypothesisPoints.at(nRightIndex);
                aHypothesisPoints.at(nRightIndex) = pTemp;
                nLeftIndex++;
                nRightIndex--;
            }
        }
 
        SortByPositionZ(aHypothesisPoints, nLeftLimit, nRightIndex);
        SortByPositionZ(aHypothesisPoints, nLeftIndex, nRightLimit);
    }
 
    void SortByPositionZ(std::vector<CHypothesisPoint*>& aHypothesisPoints)
    {
        SortByPositionZ(aHypothesisPoints, 0, aHypothesisPoints.size() - 1);
    }
 
 
 
    void CalculateEdgeImageFromImageAndDisparity(CByteImage* pImageGray, CByteImage* pDisparity, CByteImage* pEdgeImage)
    {
        // edges from the image
        CByteImage* pTempImage = new CByteImage(pImageGray);
        float pEdgesFromImage[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
        ImageProcessor::GaussianSmooth5x5(pImageGray, pTempImage);
        ImageProcessor::CalculateGradientImageSobel(pTempImage, pEdgeImage);
 
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            pEdgesFromImage[i] = pEdgeImage->pixels[i];
        }
 
        std::string sScreenshotFile = OLP_SCREENSHOT_PATH;
        sScreenshotFile += "edge-img.bmp";
        pEdgeImage->SaveToFile(sScreenshotFile.c_str());
 
 
        // edges from disparity
        float pEdgesFromDisparity[OLP_IMG_WIDTH * OLP_IMG_HEIGHT];
        // count number of disparity pixels that are zero to allow histogram stretching
        int nNumZeroDisparityPixels = 0;
 
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            if (pDisparity->pixels[i] == 0)
            {
                nNumZeroDisparityPixels++;
            }
        }
 
        float fZeroDisparityPixelRatio = (float)nNumZeroDisparityPixels / (float)(OLP_IMG_WIDTH * OLP_IMG_HEIGHT);
        ImageProcessor::HistogramStretching(pDisparity, pTempImage, fZeroDisparityPixelRatio + 0.1f, 0.9f);
        sScreenshotFile = OLP_SCREENSHOT_PATH;
        sScreenshotFile += "disp.bmp";
        pTempImage->SaveToFile(sScreenshotFile.c_str());
 
        const float fSigma = 7;
        const int nKernelSize = 4 * fSigma + 1; // >= 4*fSigma+1
        ImageProcessor::GaussianSmooth(pTempImage, pEdgeImage, fSigma * fSigma, nKernelSize);
 
        ImageProcessor::CalculateGradientImageSobel(pEdgeImage, pTempImage);
 
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            pEdgesFromDisparity[i] = pTempImage->pixels[i];
        }
 
        sScreenshotFile = OLP_SCREENSHOT_PATH;
        sScreenshotFile += "edge-disp.bmp";
        pTempImage->SaveToFile(sScreenshotFile.c_str());
 
 
        // combine edges
        float fValue;
 
        for (int i = 0; i < OLP_IMG_WIDTH * OLP_IMG_HEIGHT; i++)
        {
            fValue = sqrtf(pEdgesFromImage[i] * pEdgesFromDisparity[i] * sqrtf(pEdgesFromDisparity[i]));
            pEdgeImage->pixels[i] = (fValue > 255) ? 255 : fValue;
        }
 
        sScreenshotFile = OLP_SCREENSHOT_PATH;
        sScreenshotFile += "edge-full.bmp";
        pEdgeImage->SaveToFile(sScreenshotFile.c_str());
    }
 
}