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/Calibration.h>
#include <Calibration/StereoCalibration.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());
    }
 
} // namespace COLPTools