ParticleFilterRobotHandLocalisation.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 "ParticleFilterRobotHandLocalisation.h"
 
#include <cfloat>
 
namespace visionx
{
    //******************************************************************************************************************
    //      Constructor and destructor
    //******************************************************************************************************************
 
    // constructor
    CParticleFilterRobotHandLocalisation::CParticleFilterRobotHandLocalisation(
        int nParticles,
        int nDimension,
        int nNumParallelThreads,
        CStereoCalibration* pCalibration,
        std::string sHandModelFileName) :
        CParticleFilterFrameworkParallelized(nParticles, nDimension, nNumParallelThreads)
    {
        // set hand model
        m_pHandModels = new CHandModelV2*[m_nNumParallelThreads];
        for (int i = 0; i < m_nNumParallelThreads; i++)
        {
#ifdef DSHT_USE_ICUB
            m_pHandModels[i] = new HandModeliCub(sHandModelFileName, pCalibration);
#else
            m_pHandModels[i] = new CHandModelV2(sHandModelFileName, pCalibration);
#endif
        }
 
        // init array for the sensor values and the allowed deviation
        sensor_config = new double[nDimension];
        allowed_deviation = new double[nDimension];
 
 
        SetAllowedDeviation((double*)DSHT_default_allowed_deviation);
 
 
        // init array of preliminary probabilities
        m_ppProbabilities = new double*[NUMBER_OF_CUES];
        for (int i = 0; i < NUMBER_OF_CUES; i++)
        {
            m_ppProbabilities[i] = new double[m_nParticles];
        }
 
 
        c_total = 1; // c_total is sum of all pi. c[i] is the sum of all p[j] with j<=i.
        // It is used to pick the random samples for the recalculation of the particles
 
 
        // init spaces for temporary stuff
        m_pTempIntersectionPolygons =
            new ConvexPolygonCalculations::Polygon*[m_nNumParallelThreads];
        m_pTempVecArrays = new Vec3d*[m_nNumParallelThreads];
        m_pTempClockwiseHullPolys1 = new Vec3d*[m_nNumParallelThreads];
        m_pTempClockwiseHullPolys2 = new Vec3d*[m_nNumParallelThreads];
        m_pTempBoolArrays = new bool*[m_nNumParallelThreads];
        for (int i = 0; i < m_nNumParallelThreads; i++)
        {
            m_pTempIntersectionPolygons[i] = new ConvexPolygonCalculations::Polygon;
            m_pTempVecArrays[i] = new Vec3d[2 * DSHT_MAX_POLYGON_CORNERS];
            m_pTempClockwiseHullPolys1[i] = new Vec3d[DSHT_MAX_POLYGON_CORNERS + 1];
            m_pTempClockwiseHullPolys2[i] = new Vec3d[DSHT_MAX_POLYGON_CORNERS + 1];
            m_pTempBoolArrays[i] = new bool[2 * DSHT_MAX_POLYGON_CORNERS];
        }
    }
 
    // Destructor
    CParticleFilterRobotHandLocalisation::~CParticleFilterRobotHandLocalisation()
    {
        for (int i = 0; i < NUMBER_OF_CUES; i++)
        {
            delete[] m_ppProbabilities[i];
        }
        delete[] m_ppProbabilities;
 
        for (int i = 0; i < m_nNumParallelThreads; i++)
        {
            delete m_pTempIntersectionPolygons[i];
            delete m_pTempVecArrays[i];
            delete m_pTempClockwiseHullPolys1[i];
            delete m_pTempClockwiseHullPolys2[i];
            delete m_pTempBoolArrays[i];
        }
        delete[] m_pTempIntersectionPolygons;
        delete[] m_pTempVecArrays;
        delete[] m_pTempClockwiseHullPolys1;
        delete[] m_pTempClockwiseHullPolys2;
        delete[] m_pTempBoolArrays;
 
        delete[] sensor_config;
        delete[] allowed_deviation;
    }
 
    //******************************************************************************************************************
    //          Set and change configurations of the particle filter
    //******************************************************************************************************************
 
 
    void
    CParticleFilterRobotHandLocalisation::SetImages(CByteImage* pRegionImageLeft,
                                                    CByteImage* pRegionImageRight,
                                                    CByteImage* pSobelImageLeft,
                                                    CByteImage* pSobelImageRight,
                                                    CByteImage* pSobelXImageLeft,
                                                    CByteImage* pSobelXImageRight,
                                                    CByteImage* pSobelYImageLeft,
                                                    CByteImage* pSobelYImageRight)
    {
        // set images
        m_pRegionImageLeft = pRegionImageLeft;
        m_pRegionImageRight = pRegionImageRight;
        m_pSobelImageLeft = pSobelImageLeft;
        m_pSobelImageRight = pSobelImageRight;
        m_pSobelXImageLeft = pSobelXImageLeft;
        m_pSobelXImageRight = pSobelXImageRight;
        m_pSobelYImageLeft = pSobelYImageLeft;
        m_pSobelYImageRight = pSobelYImageRight;
    }
 
    // set the configuration of the particles
    void
    CParticleFilterRobotHandLocalisation::SetParticleConfig(double* pConfig)
    {
        for (int i = 0; i < m_nParticles; i++)
            for (int j = 0; j < m_nDimension; j++)
            {
                s[i][j] = pConfig[j];
            }
    }
 
    // set the configuration of the first half of the particles, the rest is not changed
    void
    CParticleFilterRobotHandLocalisation::SetParticleConfigHalf(double* pConfig)
    {
        for (int i = 0; i < m_nParticles / 2; i++)
            for (int j = 0; j < m_nDimension; j++)
            {
                s[i][j] = pConfig[j];
            }
    }
 
    // set the configuration of ten percent of the particles, the rest is not changed
    void
    CParticleFilterRobotHandLocalisation::SetConfigOfATenthOfTheParticles(int nTenthIndex,
                                                                          double* pConfig)
    {
        if (nTenthIndex >= 0 && nTenthIndex <= 9)
        {
            for (int i = m_nParticles / 10 * nTenthIndex; i < m_nParticles / 10 * (nTenthIndex + 1);
                 i++)
                for (int j = 0; j < m_nDimension; j++)
                {
                    s[i][j] = pConfig[j];
                }
        }
    }
 
    // set the array containing the sensor values
    void
    CParticleFilterRobotHandLocalisation::SetSensorConfig(double* sconfig)
    {
        for (int i = 0; i < m_nDimension; i++)
        {
            sensor_config[i] = sconfig[i];
            lower_limit[i] = sensor_config[i] - allowed_deviation[i];
            upper_limit[i] = sensor_config[i] + allowed_deviation[i];
        }
    }
 
    // set the allowed deviation of the particle configs from the sensor values
    void
    CParticleFilterRobotHandLocalisation::SetAllowedDeviation(double* adeviation)
    {
        for (int i = 0; i < m_nDimension; i++)
        {
            allowed_deviation[i] = adeviation[i];
            sigma[i] = allowed_deviation[i] / 2;
        }
    }
 
    // set the tracking ball position and size
    void
    CParticleFilterRobotHandLocalisation::SetTrackingBallPositions(double* dPosX,
                                                                   double* dPosY,
                                                                   double* dRadius,
                                                                   int nNumTrBallRegions,
                                                                   bool bLeftCamImage)
    {
        if (bLeftCamImage)
        {
            m_pdTrackingBallPosXLeft = dPosX;
            m_pdTrackingBallPosYLeft = dPosY;
            m_pdTrackingBallRadiusLeft = dRadius;
            m_nNumTrackingBallRegionsLeft = nNumTrBallRegions;
        }
        else
        {
            m_pdTrackingBallPosXRight = dPosX;
            m_pdTrackingBallPosYRight = dPosY;
            m_pdTrackingBallRadiusRight = dRadius;
            m_nNumTrackingBallRegionsRight = nNumTrBallRegions;
        }
    }
 
    //******************************************************************************************************************
    //      Update the hand model according to the configuration nParticleIndex
    //******************************************************************************************************************
 
    void
    CParticleFilterRobotHandLocalisation::UpdateModel(int nParticleIndex, int nModelIndex)
    {
        //printf("UpdateModel(%d)\n", nParticleIndex);
        m_pHandModels[nModelIndex]->UpdateHand(s[nParticleIndex]);
    }
 
    //******************************************************************************************************************
    //  Predict new bases from the old ones. dSigmaFactor will be multiplied with all sigma[i] values
    //******************************************************************************************************************
 
    void
    CParticleFilterRobotHandLocalisation::PredictNewBases(double dSigmaFactor)
    {
        //printf("PredictNewBases\n");
 
#pragma omp parallel for
        for (int nNewIndex = 0; nNewIndex < m_nParticles; nNewIndex++)
        {
            int nOldIndex = PickBaseSample();
 
            for (int i = 0; i < m_nDimension; i++)
            {
                double dNewValue = s[nOldIndex][i] + dSigmaFactor * sigma[i] * gaussian_random();
                s_temp[nNewIndex][i] =
                    (dNewValue >= lower_limit[i] && dNewValue <= upper_limit[i])
                        ? dNewValue
                        : (0.7 * 0.5 * (lower_limit[i] + upper_limit[i]) + 0.3 * s[nOldIndex][i] +
                           0.5 * dSigmaFactor * sigma[i] * gaussian_random());
            }
        }
 
        // switch old/new
        double** temp = s_temp;
        s_temp = s;
        s = temp;
    }
 
    //******************************************************************************************************************
    //          Functions for the region cue
    //******************************************************************************************************************
 
    // sum up all pixel values of m_pRegionImageLeft that are inside the polygon pol, and count the number
    // of pixels inside the polygon
    inline void
    CParticleFilterRobotHandLocalisation::CalculatePixelSumInPolygon(
        ConvexPolygonCalculations::Polygon* pol,
        int& region_sum,
        int& region_length,
        bool bLeftCamImage)
    {
        CByteImage* pImage = (bLeftCamImage) ? m_pRegionImageLeft : m_pRegionImageRight;
        const int width = pImage->width;
 
        int leftBorderInt, rightBorderInt, up, down, indexLeft, indexRight, indexMain, nTempOffset;
        float leftBorder = 0, rightBorder = 0, leftOffset = 0, rightOffset = 0;
 
        region_sum = 0;
        region_length = 0;
 
        indexLeft = 1;
        indexRight = 1;
        for (indexMain = 0; indexMain < pol->nCorners - 1; indexMain++)
        {
            if (pol->hull[indexMain].z != 2) // if this is not a right border point, we have
            {
                // to change left border and offset
                leftBorder = pol->hullLeft[indexLeft].x;
                leftOffset = (pol->hullLeft[indexLeft + 1].x - pol->hullLeft[indexLeft].x) /
                             (pol->hullLeft[indexLeft + 1].y - pol->hullLeft[indexLeft].y);
            }
 
            if (pol->hull[indexMain].z != 1) // if this is not a left border point, we have
            {
                // to change right border and offset
                rightBorder = pol->hullRight[indexRight].x;
                rightOffset = (pol->hullRight[indexRight + 1].x - pol->hullRight[indexRight].x) /
                              (pol->hullRight[indexRight + 1].y - pol->hullRight[indexRight].y);
            }
 
            up = (int)pol->hull[indexMain].y;
            down = (int)pol->hull[indexMain + 1].y;
 
            if (pol->hull[indexMain + 1].z != 2)
            {
                indexLeft++;
            }
            if (pol->hull[indexMain + 1].z != 1)
            {
                indexRight++;
            }
 
            if ((down > DSHT_IMAGE_HEIGHT - 1) || (up < 0))
            {
                goto skip_updown; // not in the image
            }
 
            // stay inside image
            if (down < 0)
            {
                down = 0;
            }
            if (up > DSHT_IMAGE_HEIGHT - 1)
            {
                up = DSHT_IMAGE_HEIGHT - 1;
            }
 
            for (int j = up; j > down; j--, leftBorder -= leftOffset, rightBorder -= rightOffset)
            {
                // optim: width/2 sofort dazuaddieren
                leftBorderInt = (int)leftBorder;
                rightBorderInt = (int)rightBorder;
 
                // stay inside image
                if ((leftBorderInt > DSHT_IMAGE_WIDTH - 1) || (rightBorderInt < 0))
                {
                    goto skip_leftright;
                }
 
                if (leftBorderInt < 0)
                {
                    leftBorderInt = 0;
                }
                if (rightBorderInt > DSHT_IMAGE_WIDTH - 1)
                {
                    rightBorderInt = DSHT_IMAGE_WIDTH - 1;
                }
 
                nTempOffset = j * width;
 
                for (int k = leftBorderInt; k < rightBorderInt; k++)
                {
                    region_sum += pImage->pixels[nTempOffset + k];
                    region_length++;
                }
            skip_leftright:;
            }
        skip_updown:;
        }
    }
 
    // calculate the region cue
    void
    CParticleFilterRobotHandLocalisation::CalculateRegionCue(int& region_sum,
                                                             int& region_length,
                                                             int nModelIndex,
                                                             bool bLeftCamImage)
    {
        const int num_polygons = 5;
        ConvexPolygonCalculations::Polygon* polygons[num_polygons];
 
        if (bLeftCamImage)
        {
            for (int i = 0; i < num_polygons; i++)
            {
                polygons[i] = &m_pHandModels[nModelIndex]->m_aFingerTipPolygonsLeftCam.at(i);
            }
        }
        else
        {
            for (int i = 0; i < num_polygons; i++)
            {
                polygons[i] = &m_pHandModels[nModelIndex]->m_aFingerTipPolygonsRightCam.at(i);
            }
        }
 
        region_sum = 0;
        region_length = 0;
 
        int polygon_interior_sum, polygon_interior_size;
 
 
        // sum up the pixels of correct color inside the fingertip polygons
 
        for (int i = 0; i < num_polygons; i++)
        {
            CalculatePixelSumInPolygon(
                polygons[i], polygon_interior_sum, polygon_interior_size, bLeftCamImage);
            region_sum += polygon_interior_sum;
            region_length += polygon_interior_size;
        }
 
 
        // subtract intersection regions which may have been counted two times
 
        for (int i = 1; i < num_polygons; i++) // thumb with all other fingers
        {
            if (PolygonsMightIntersect(polygons[0], polygons[i]))
            {
                ConvexPolygonCalculations::GetPolygonIntersection(
                    polygons[0],
                    polygons[i],
                    m_pTempIntersectionPolygons[nModelIndex],
                    m_pTempVecArrays[nModelIndex],
                    m_pTempBoolArrays[nModelIndex],
                    m_pTempClockwiseHullPolys1[nModelIndex],
                    m_pTempClockwiseHullPolys2[nModelIndex]);
                CalculatePixelSumInPolygon(m_pTempIntersectionPolygons[nModelIndex],
                                           polygon_interior_sum,
                                           polygon_interior_size,
                                           bLeftCamImage);
                region_sum -= polygon_interior_sum;
                region_length -= polygon_interior_size;
            }
        }
 
        for (int i = 1; i < num_polygons - 1;
             i++) // every other fingers only with its direct neighbour, to reduce
        {
            // multiple subtraction of overlapping intersection regions
            if (PolygonsMightIntersect(polygons[i], polygons[i + 1]))
            {
                ConvexPolygonCalculations::GetPolygonIntersection(
                    polygons[i],
                    polygons[i + 1],
                    m_pTempIntersectionPolygons[nModelIndex],
                    m_pTempVecArrays[nModelIndex],
                    m_pTempBoolArrays[nModelIndex],
                    m_pTempClockwiseHullPolys1[nModelIndex],
                    m_pTempClockwiseHullPolys2[nModelIndex]);
                CalculatePixelSumInPolygon(m_pTempIntersectionPolygons[nModelIndex],
                                           polygon_interior_sum,
                                           polygon_interior_size,
                                           bLeftCamImage);
                region_sum -= polygon_interior_sum;
                region_length -= polygon_interior_size;
            }
        }
 
        // keep index and ringfinger from fusioning
        if (PolygonsMightIntersect(polygons[1], polygons[3]))
        {
            ConvexPolygonCalculations::GetPolygonIntersection(
                polygons[1],
                polygons[3],
                m_pTempIntersectionPolygons[nModelIndex],
                m_pTempVecArrays[nModelIndex],
                m_pTempBoolArrays[nModelIndex],
                m_pTempClockwiseHullPolys1[nModelIndex],
                m_pTempClockwiseHullPolys2[nModelIndex]);
            CalculatePixelSumInPolygon(m_pTempIntersectionPolygons[nModelIndex],
                                       polygon_interior_sum,
                                       polygon_interior_size,
                                       bLeftCamImage);
            region_sum -= polygon_interior_sum;
            region_length -= polygon_interior_size;
        }
    }
 
    //******************************************************************************************************************
    //          Functions for the edge cue
    //******************************************************************************************************************
 
    // sum up the pixel values on a sequence of lines
    inline void
    CParticleFilterRobotHandLocalisation::CalculatePixelSumOnLineSequence(Vec3d linePoints[],
                                                                          int nPoints,
                                                                          int& edge_sum,
                                                                          int& edge_length,
                                                                          double& angle_diffs,
                                                                          bool bLeftCamImage)
    {
        int up, down, left, right, lineSum;
        double xOffset, xDiff, yOffset, yDiff, lineLength;
 
#ifdef DSHT_USE_EDGE_DIRECTION
        int absVal;
        double angleSum, normalX, normalY;
#endif // DSHT_USE_EDGE_DIRECTION
 
        edge_sum = 0;
        edge_length = 0;
        angle_diffs = 0;
 
        CByteImage* pImage = (bLeftCamImage) ? m_pSobelImageLeft : m_pSobelImageRight;
        const int width = pImage->width;
 
        for (int i = 0; i < nPoints - 1; i++)
        {
            lineSum = 0;
 
            lineLength = sqrt(
                (linePoints[i + 1].x - linePoints[i].x) * (linePoints[i + 1].x - linePoints[i].x) +
                (linePoints[i + 1].y - linePoints[i].y) * (linePoints[i + 1].y - linePoints[i].y));
 
#ifdef DSHT_USE_EDGE_DIRECTION
            angleSum = 0;
            normalX = (linePoints[i + 1].y -
                       linePoints[i].y); // / lineLength is not done, because at the end the sum
            normalY =
                -(linePoints[i + 1].x - linePoints[i].x); // has to be multiplied with lineLength
#endif // DSHT_USE_EDGE_DIRECTION
 
            if (linePoints[i].y > linePoints[i + 1].y)
            {
                up = (int)linePoints[i].y;
                down = (int)linePoints[i + 1].y;
                xOffset = linePoints[i + 1].x;
            }
            else
            {
                up = (int)linePoints[i + 1].y;
                down = (int)linePoints[i].y;
                xOffset = linePoints[i].x;
            }
 
            if (linePoints[i].x > linePoints[i + 1].x)
            {
                right = (int)linePoints[i].x;
                left = (int)linePoints[i + 1].x;
                yOffset = linePoints[i + 1].y;
            }
            else
            {
                right = (int)linePoints[i + 1].x;
                left = (int)linePoints[i].x;
                yOffset = linePoints[i].y;
            }
 
            if (up < DSHT_IMAGE_HEIGHT && down >= 0 && left >= 0 && right < DSHT_IMAGE_WIDTH)
            {
                xDiff = (linePoints[i + 1].x - linePoints[i].x) /
                        (linePoints[i + 1].y - linePoints[i].y);
                yDiff = (linePoints[i + 1].y - linePoints[i].y) /
                        (linePoints[i + 1].x - linePoints[i].x);
 
#ifdef DSHT_USE_EDGE_DIRECTION
                for (int j = down; j < up; j++, xOffset += xDiff)
                {
                    absVal = pImage->pixels[j * width + (int)xOffset];
                    lineSum += absVal;
                    angleSum += abs(((double)pImage->pixels[j * width + (int)xOffset] * normalX +
                                     (double)pImage->pixels[j * width + (int)xOffset] * normalY) /
                                    ((double)absVal + 0.001));
                }
 
 
                for (int j = left; j < right; j++, yOffset += yDiff)
                {
                    absVal = pImage->pixels[((int)yOffset) * width + j];
                    lineSum += absVal;
                    angleSum += abs(((double)pImage->pixels[((int)yOffset) * width + j] * normalX +
                                     (double)pImage->pixels[((int)yOffset) * width + j] * normalY) /
                                    ((double)absVal + 0.001));
                }
 
                edge_length += (int)lineLength;
                // set weight congruent to line length
                edge_sum += (int)((double)lineSum * lineLength /
                                  ((double)(up - down + right - left) + 0.001));
                angle_diffs += angleSum / ((double)(up - down + right - left) +
                                           0.001); // no *lineLength, because no
                // /linelength done at normal calculation
#else
                for (int j = down; j < up; j++, xOffset += xDiff)
                {
                    lineSum += pImage->pixels[j * width + (int)xOffset];
                }
 
                for (int j = left; j < right; j++, yOffset += yDiff)
                {
                    lineSum += pImage->pixels[((int)yOffset) * width + j];
                }
 
                edge_length += (int)lineLength;
                // set weight congruent to line length
                edge_sum += (int)((double)lineSum * lineLength /
                                  ((double)(up - down + right - left) + 0.001));
#endif // DSHT_USE_EDGE_DIRECTION (else)
            }
        }
    }
 
    // calculate the edge cue
    void
    CParticleFilterRobotHandLocalisation::CalculateEdgeCue(int& edge_sum,
                                                           int& edge_length,
                                                           double& angle_diffs,
                                                           int nModelIndex,
                                                           bool bLeftCamImage)
    {
        const int num_polygons = 5;
        ConvexPolygonCalculations::Polygon* polygons[num_polygons];
 
        if (bLeftCamImage)
        {
            for (int i = 0; i < num_polygons; i++)
            {
                polygons[i] = &m_pHandModels[nModelIndex]->m_aFingerTipPolygonsLeftCam.at(i);
            }
        }
        else
        {
            for (int i = 0; i < num_polygons; i++)
            {
                polygons[i] = &m_pHandModels[nModelIndex]->m_aFingerTipPolygonsRightCam.at(i);
            }
        }
 
        int temp_edge_sum, temp_edge_length;
        double temp_angle_diffs;
        edge_sum = 0;
        edge_length = 0;
        angle_diffs = 0;
 
        for (int i = 0; i < num_polygons; i++)
        {
            CalculatePixelSumOnLineSequence(polygons[i]->hullCircle,
                                            polygons[i]->nCorners + 1,
                                            temp_edge_sum,
                                            temp_edge_length,
                                            temp_angle_diffs,
                                            bLeftCamImage);
            edge_sum += temp_edge_sum;
            edge_length += temp_edge_length;
            angle_diffs += temp_angle_diffs;
        }
    }
 
    //******************************************************************************************************************
    //          Calculate error distance to tracking ball
    //******************************************************************************************************************
 
 
    // calculate error distance to tracking ball
    void
    CParticleFilterRobotHandLocalisation::CalculateTrackingBallCue(double& dDistanceXY,
                                                                   double& dDistanceZ,
                                                                   int nModelIndex)
    {
        double dPosX, dPosY, dRadius;
 
        double dMinDistance;
        int nMinIndex;
        double dTempDist;
 
 
        // left image
 
        dPosX = m_pHandModels[nModelIndex]->m_vTrackingBallPosLeftCam.x;
        dPosY = m_pHandModels[nModelIndex]->m_vTrackingBallPosLeftCam.y;
        dRadius = m_pHandModels[nModelIndex]->m_fTrackingBallRadiusLeftCam;
 
        nMinIndex = 0;
        dMinDistance = 1000000000;
 
        for (int i = 0; i < m_nNumTrackingBallRegionsLeft; i++)
        {
            dTempDist =
                (m_pdTrackingBallPosXLeft[i] - dPosX) * (m_pdTrackingBallPosXLeft[i] - dPosX) +
                (m_pdTrackingBallPosYLeft[i] - dPosY) * (m_pdTrackingBallPosYLeft[i] - dPosY);
            if (dTempDist < dMinDistance)
            {
                dMinDistance = dTempDist;
                nMinIndex = i;
            }
        }
 
        dDistanceXY = sqrt((m_pdTrackingBallPosXLeft[nMinIndex] - dPosX) *
                               (m_pdTrackingBallPosXLeft[nMinIndex] - dPosX) +
                           (m_pdTrackingBallPosYLeft[nMinIndex] - dPosY) *
                               (m_pdTrackingBallPosYLeft[nMinIndex] - dPosY));
 
        dDistanceZ = fabs(1 / m_pdTrackingBallRadiusLeft[nMinIndex] - 1 / dRadius);
        if (std::isnan(dDistanceZ))
        {
            dDistanceZ = FLT_MAX;
        }
 
 
        // right image
 
        dPosX = m_pHandModels[nModelIndex]->m_vTrackingBallPosRightCam.x;
        dPosY = m_pHandModels[nModelIndex]->m_vTrackingBallPosRightCam.y;
        dRadius = m_pHandModels[nModelIndex]->m_fTrackingBallRadiusRightCam;
 
        nMinIndex = 0;
        dMinDistance = 1000000000;
 
        for (int i = 0; i < m_nNumTrackingBallRegionsRight; i++)
        {
            dTempDist =
                (m_pdTrackingBallPosXRight[i] - dPosX) * (m_pdTrackingBallPosXRight[i] - dPosX) +
                (m_pdTrackingBallPosYRight[i] - dPosY) * (m_pdTrackingBallPosYRight[i] - dPosY);
            if (dTempDist < dMinDistance)
            {
                dMinDistance = dTempDist;
                nMinIndex = i;
            }
        }
 
        //    dDistanceXY += sqrt   (   (m_pdTrackingBallPosXRight[nMinIndex]-dPosX)*(m_pdTrackingBallPosXRight[nMinIndex]-dPosX)
        //                          + (m_pdTrackingBallPosYRight[nMinIndex]-dPosY)*(m_pdTrackingBallPosYRight[nMinIndex]-dPosY)
        //                        );
 
        //    dDistanceZ += fabs(1/m_pdTrackingBallRadiusRight[nMinIndex]-1/dRadius);
    }
 
    //******************************************************************************************************************
    //          Evaluation and rating of the particles
    //******************************************************************************************************************
 
 
    // calculate the preliminary probabilities for the different cues
    double
    CParticleFilterRobotHandLocalisation::CalculateProbability(int nParticleIndex, int nModelIndex)
    {
        //printf("CalculateProbability 1\n");
 
        int nRegionSumLeftImg, nRegionLengthLeftImg, nEdgeSumLeftImg, nEdgeLengthLeftImg;
        int nRegionSumRightImg, nRegionLengthRightImg, nEdgeSumRightImg, nEdgeLengthRightImg;
        double fAngleDiffsLeftImg, fAngleDiffsRightImg;
 
        CalculateRegionCue(nRegionSumLeftImg, nRegionLengthLeftImg, nModelIndex, true);
        CalculateRegionCue(nRegionSumRightImg, nRegionLengthRightImg, nModelIndex, false);
 
        //printf("CalculateProbability 2\n");
 
        CalculateEdgeCue(
            nEdgeSumLeftImg, nEdgeLengthLeftImg, fAngleDiffsLeftImg, nModelIndex, true);
        CalculateEdgeCue(
            nEdgeSumRightImg, nEdgeLengthRightImg, fAngleDiffsRightImg, nModelIndex, false);
 
        double dTrBallDistXY, dTrBallDistZ;
        CalculateTrackingBallCue(dTrBallDistXY, dTrBallDistZ, nModelIndex);
 
        //printf("CalculateProbability 3\n");
 
        m_ppProbabilities[0][nParticleIndex] = 0.5 * (nRegionSumLeftImg + nRegionSumRightImg);
        m_ppProbabilities[1][nParticleIndex] =
            (double)(nRegionSumLeftImg + nRegionSumRightImg) /
            ((double)(nRegionLengthLeftImg + nRegionLengthRightImg) + 0.001);
 
        m_ppProbabilities[2][nParticleIndex] = 0.5 * (nEdgeSumLeftImg + nEdgeSumRightImg);
        m_ppProbabilities[3][nParticleIndex] =
            (double)(nEdgeSumLeftImg + nEdgeSumRightImg) /
            ((double)(nEdgeLengthLeftImg + nEdgeLengthRightImg) + 0.001);
#ifdef DSHT_USE_EDGE_DIRECTION
        m_ppProbabilities[4][nParticleIndex] = -(fAngleDiffsLeftImg + fAngleDiffsRightImg) /
                                               ((double)(nEdgeLengthLeftImg + nEdgeLengthRightImg) +
                                                0.001); // TODO: find out if sign is correct
#else
        m_ppProbabilities[4][nParticleIndex] = 0;
#endif // DSHT_USE_EDGE_DIRECTION
 
        m_ppProbabilities[5][nParticleIndex] = -dTrBallDistXY;
        m_ppProbabilities[6][nParticleIndex] = 0; //- dTrBallDistZ;
 
        return (double)(nRegionSumLeftImg / 255) +
               ((double)nRegionSumLeftImg / ((double)(nRegionLengthLeftImg * 255) + 0.001));
    }
 
    // calculate the real probabilities for the particles by normalizing the preliminary ones and giving
    // specific weights to the different cues
    void
    CParticleFilterRobotHandLocalisation::CalculateFinalProbabilities()
    {
        //printf("CalculateFinalProbabilities 1\n");
 
        // normalisation of the probabilities to the interval [0,1]
        for (int i = 0; i < NUMBER_OF_CUES; i++)
        {
            double min = 99999999;
            double max = -99999999;
 
            for (int j = 0; j < m_nParticles; j++)
            {
                if (m_ppProbabilities[i][j] != m_ppProbabilities[i][j])
                {
                    m_ppProbabilities[i][j] = 0;
                }
 
                if (m_ppProbabilities[i][j] < min)
                {
                    min = m_ppProbabilities[i][j];
                }
 
                if (m_ppProbabilities[i][j] > max)
                {
                    max = m_ppProbabilities[i][j];
                }
            }
 
            if (max != min)
            {
                for (int j = 0; j < m_nParticles; j++)
                {
                    m_ppProbabilities[i][j] = (m_ppProbabilities[i][j] - min) / (max - min);
                }
            }
 
            m_pProbMin[i] = min;
            m_pProbMax[i] = max;
        }
 
 
//debug
//double xSum=0, ySum=0, fDistSum=0;
 
// weightet influence of the different cues on the final probability
#pragma omp parallel for
        for (int j = 0; j < m_nParticles; j++)
        {
#ifdef DSHT_USE_EDGE_DIRECTION
            pi[j] = exp(15.0 / 31.0 *
                        (6 * m_ppProbabilities[0][j] + 6 * m_ppProbabilities[1][j] +
                         1 * m_ppProbabilities[2][j] + 1 * m_ppProbabilities[3][j] +
                         2 * m_ppProbabilities[4][j] + 10 * m_ppProbabilities[5][j] +
                         5 * m_ppProbabilities[6][j])) -
                    0.95;
#else
            pi[j] = exp(15.0 / 31.0 *
                        (6 * m_ppProbabilities[0][j] + 6 * m_ppProbabilities[1][j] +
                         2 * m_ppProbabilities[2][j] + 2 * m_ppProbabilities[3][j] +
                         10 * m_ppProbabilities[5][j] + 5 * m_ppProbabilities[6][j])) -
                    0.95;
 
#endif // DSHT_USE_EDGE_DIRECTION
 
            //        pi[j] = exp( 15.0/29.0 * (
            //                                          5 * m_ppProbabilities[0][j]
            //                                        + 5 * m_ppProbabilities[1][j]
            //                                        + 2 * m_ppProbabilities[2][j]
            //                                        + 2 * m_ppProbabilities[3][j]
            //                                        + 2 * m_ppProbabilities[4][j]
            //                                        + 10 * m_ppProbabilities[5][j]
            //                                        + 5 * m_ppProbabilities[6][j]
            //                                 )
            //                    ) - 0.95;
 
            //        m_pHandModels[0]->UpdateHand(s[j]);
            //        double x,y,r;
            //        m_pHandModels[0]->GetTrackingBallPositionLeftCam(x, y, r);
            //        //printf("Particle %d: (%.1f, %.1f)  %.3f\n", j, x, y, pi[j]);
            //        xSum += x;
            //        ySum += y;
            //        fDistSum += sqrt((x-m_pdTrackingBallPosXLeft[0])*(x-m_pdTrackingBallPosXLeft[0]) + (y-m_pdTrackingBallPosYLeft[0])*(y-m_pdTrackingBallPosYLeft[0]));
 
            if (pi[j] != pi[j])
            {
                if (j % 100 == 0)
                    printf("pi[i] is nan! m_ppProbabilities[0][j] = %f, m_ppProbabilities[1][j] = "
                           "%f, m_ppProbabilities[2][j] = %f, m_ppProbabilities[3][j] = %f, "
                           "m_ppProbabilities[4][j] = %f, m_ppProbabilities[5][j] = %f, "
                           "m_ppProbabilities[6][j] = %f\n",
                           m_ppProbabilities[0][j],
                           m_ppProbabilities[1][j],
                           m_ppProbabilities[2][j],
                           m_ppProbabilities[3][j],
                           m_ppProbabilities[4][j],
                           m_ppProbabilities[5][j],
                           m_ppProbabilities[6][j]);
            }
        }
 
        // debug
        //printf("TrBallPos: (%.1f, %.1f)\n", m_pdTrackingBallPosXLeft[0], m_pdTrackingBallPosYLeft[0]);
        //printf("Avg: (%.1f, %.1f)     Dist: %.1f\n", xSum/m_nParticles, ySum/m_nParticles, fDistSum/m_nParticles);
    }
 
    void
    CParticleFilterRobotHandLocalisation::GetDetailedRating(double* pConf,
                                                            DetailedPFRating* pRating)
    {
        //printf("GetDetailedRating 1\n");
        int i;
 
        // copy config
        for (i = 0; i < m_nDimension; i++)
        {
            pRating->dResultConfig[i] = pConf[i];
        }
 
        // count number of white pixels in color segmented image
        pRating->nNumberOfWhitePixelsInImage = 0;
        for (i = 0; i < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT; i++)
        {
            pRating->nNumberOfWhitePixelsInImage += m_pRegionImageLeft->pixels[i] == 255;
        }
 
        // calculate the rating for the config
        //ISO C++ forbids variable length array [-Werror=vla] => use a vector (the array will be on the heap anyways)
        std::vector<double> temp(m_nDimension);
        for (i = 0; i < m_nDimension; i++)
        {
            temp[i] = s[0][i];
            s[0][i] = pConf[i];
        }
 
        UpdateModel(0);
 
        for (i = 0; i < m_nDimension; i++)
        {
            s[0][i] = temp[i];
        }
 
        // color regions
        int nRegionSum = 0;
        int nRegionLength = 0;
        CalculateRegionCue(nRegionSum, nRegionLength, 0, true);
        pRating->nOverallNumberOfFoundPixels = nRegionSum / 255;
        pRating->dOverallFoundPercentageOfExpectedPixels =
            (double)nRegionSum / ((double)(nRegionLength * 255) + 0.001);
 
        for (i = 0; i < DSHT_NUM_FINGERS; i++)
        {
            nRegionSum = 0;
            nRegionLength = 0;
            CalculatePixelSumInPolygon(&(m_pHandModels[0]->m_aFingerTipPolygonsLeftCam.at(i)),
                                       nRegionSum,
                                       nRegionLength,
                                       true);
            pRating->nNumberOfFoundPixelsOfFinger[i] = nRegionSum / 255;
            pRating->dFoundPercentageOfExpectedPixelsOfFinger[i] =
                (double)nRegionSum / ((double)(nRegionLength * 255) + 0.001);
        }
 
        // edges
        nRegionSum = 0;
        nRegionLength = 0;
        double dAngleDiffs = 0;
        CalculateEdgeCue(nRegionSum, nRegionLength, dAngleDiffs, 0, true);
        pRating->nEdgeSum = nRegionSum;
        pRating->dEdgePercentage = (double)nRegionSum / ((double)nRegionLength + 0.001);
        pRating->dEdgeDirection = dAngleDiffs / ((double)nRegionLength + 0.001);
 
        // PF cue rating
        CalculateProbability(0);
        for (i = 0; i < NUMBER_OF_CUES; i++)
        {
            if (m_pProbMax[i] != m_pProbMin[i])
            {
                m_ppProbabilities[i][0] =
                    (m_ppProbabilities[i][0] - m_pProbMin[i]) / (m_pProbMax[i] - m_pProbMin[i]);
            }
        }
#ifdef DSHT_USE_EDGE_DIRECTION
        pRating->dPFCueRating = 1.0 / 29.0 *
                                (5 * m_ppProbabilities[0][0] + 5 * m_ppProbabilities[1][0] +
                                 1 * m_ppProbabilities[2][0] + 1 * m_ppProbabilities[3][0] +
                                 2 * m_ppProbabilities[4][0] + 10 * m_ppProbabilities[5][0] +
                                 5 * m_ppProbabilities[6][0]);
#else
        pRating->dPFCueRating = 1.0 / 29.0 *
                                (5 * m_ppProbabilities[0][0] + 5 * m_ppProbabilities[1][0] +
                                 2 * m_ppProbabilities[2][0] + 2 * m_ppProbabilities[3][0] +
                                 10 * m_ppProbabilities[5][0] + 5 * m_ppProbabilities[6][0]);
#endif // DSHT_USE_EDGE_DIRECTION
 
        // final rating
 
        UpdateModel(0);
 
        // index is closer than pinky
        if (m_pHandModels[0]->m_aFingerTipPolygonsLeftCam.at(1).center3d.z <
            m_pHandModels[0]->m_aFingerTipPolygonsLeftCam.at(4).center3d.z)
        {
            // thumb and index should be visible
            if (abs(m_pHandModels[0]->m_aFingerTipPolygonsLeftCam.at(0).center3d.z -
                    m_pHandModels[0]->m_aFingerTipPolygonsLeftCam.at(1).center3d.z) <= 30)
            {
                pRating->dRating = 0.3 * pRating->dFoundPercentageOfExpectedPixelsOfFinger[0] +
                                   0.3 * pRating->dFoundPercentageOfExpectedPixelsOfFinger[1] +
                                   0.3 * pRating->dOverallFoundPercentageOfExpectedPixels +
                                   0.1 * 1.0;
            }
            // thumb is in front
            else if (m_pHandModels[0]->m_aFingerTipPolygonsLeftCam.at(0).center3d.z <
                     m_pHandModels[0]->m_aFingerTipPolygonsLeftCam.at(1).center3d.z + 30)
            {
                pRating->dRating = 0.4 * pRating->dFoundPercentageOfExpectedPixelsOfFinger[0] +
                                   0.5 * pRating->dOverallFoundPercentageOfExpectedPixels +
                                   0.1 * 0.0;
            }
            else // other fingers are in front
            {
                pRating->dRating = 0.2 * pRating->dFoundPercentageOfExpectedPixelsOfFinger[1] +
                                   0.7 * pRating->dOverallFoundPercentageOfExpectedPixels +
                                   0.1 * 0.0;
            }
        }
        else // pinky is closer than index
        {
            // thumb is in front
            if (m_pHandModels[0]->m_aFingerTipPolygonsLeftCam.at(0).center3d.z <
                m_pHandModels[0]->m_aFingerTipPolygonsLeftCam.at(4).center3d.z + 30)
            {
                pRating->dRating = 0.4 * pRating->dFoundPercentageOfExpectedPixelsOfFinger[0] +
                                   0.5 * pRating->dOverallFoundPercentageOfExpectedPixels +
                                   0.1 * 0.0;
            }
            else // other fingers are in front
            {
                pRating->dRating =
                    0.8 * pRating->dOverallFoundPercentageOfExpectedPixels + 0.2 * 0.0;
            }
        }
 
 
        // enough pixels found for a good estimation? -> confidence of the rating
        if (pRating->nNumberOfWhitePixelsInImage > 400)
        {
            pRating->dConfidenceOfRating = (double)pRating->nOverallNumberOfFoundPixels /
                                           (double)pRating->nNumberOfWhitePixelsInImage;
            if (pRating->nNumberOfWhitePixelsInImage < 1200)
            {
                pRating->dConfidenceOfRating *=
                    (double)pRating->nNumberOfWhitePixelsInImage / 1200.0;
            }
        }
        else
        {
            pRating->dConfidenceOfRating = 0;
        }
    }
 
    double
    CParticleFilterRobotHandLocalisation::DistanceBetweenConfigs(double* pOldConf, double* pNewConf)
    {
        double result = 0;
        for (int i = 0; i < 6; i++)
        {
            result += (pNewConf[i] - pOldConf[i]) * (pNewConf[i] - pOldConf[i]) /
                      (allowed_deviation[i] * allowed_deviation[i]);
        }
        for (int i = 6; i < m_nDimension; i++)
        {
            result += 0.1 * (pNewConf[i] - pOldConf[i]) * (pNewConf[i] - pOldConf[i]) /
                      (allowed_deviation[i] * allowed_deviation[i]);
        }
        return sqrt(result);
    }
} // namespace visionx