GaussBackground.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 "GaussBackground.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include "ObjectLearningByPushingDefinitions.h"
#include <Image/ImageProcessor.h>
 
CGaussBackground::CGaussBackground(const int nWidth, const int nHeight)
{
    m_pPixelProbabilityDistributions = new CPixelProbabilityDistribution[nWidth * nHeight];
    m_nImageWidth = nWidth;
    m_nImageHeight = nHeight;
 
    m_pBackgroundRGB = new CByteImage(nWidth, nHeight, CByteImage::eRGB24);
}
 
CGaussBackground::~CGaussBackground()
{
    delete m_pPixelProbabilityDistributions;
    delete m_pBackgroundRGB;
}
 
inline double
DetMat2d(const Mat2d m)
{
    return m.r1 * m.r4 - m.r3 * m.r2;
}
 
void
CGaussBackground::LearnBackgroundRGB(CByteImage** pRGBImages, const int nNumImages)
{
    CByteImage** pHSVImages = new CByteImage*[nNumImages];
    CByteImage* pSmoothedImage = new CByteImage(m_nImageWidth, m_nImageHeight, CByteImage::eRGB24);
 
    for (int i = 0; i < nNumImages; i++)
    {
        pHSVImages[i] = new CByteImage(m_nImageWidth, m_nImageHeight, CByteImage::eRGB24);
        ImageProcessor::GaussianSmooth3x3(pRGBImages[i], pSmoothedImage);
        ImageProcessor::CalculateHSVImage(pSmoothedImage, pHSVImages[i]);
    }
 
    LearnBackground(pHSVImages, nNumImages);
 
    ImageProcessor::CopyImage(pRGBImages[0], m_pBackgroundRGB);
 
    for (int i = 0; i < nNumImages; i++)
    {
        delete pHSVImages[i];
    }
 
    delete[] pHSVImages;
    delete pSmoothedImage;
}
 
void
CGaussBackground::LearnBackground(CByteImage** pHSVImages, const int nNumImages)
{
    if ((pHSVImages[0]->width != m_nImageWidth) || (pHSVImages[0]->height != m_nImageHeight))
    {
        ARMARX_WARNING_S << "CGaussBackground::LearnBackground: Image dimensions do not fit!";
        return;
    }
 
    if (nNumImages < 1)
    {
        ARMARX_WARNING_S << "CGaussBackground::LearnBackground: Need at least one image!";
        return;
    }
 
    CPixelProbabilityDistribution* pPPD = m_pPixelProbabilityDistributions;
    CPixelProbabilityDistribution* pShPPD =
        new CPixelProbabilityDistribution[m_nImageWidth * m_nImageHeight];
 
    // reset all pixels
    for (int j = 0; j < m_nImageWidth * m_nImageHeight; j++)
    {
        Math2d::SetVec(pPPD[j].vMean, 0, 0);
        pPPD[j].mCovariance.r1 = 0;
        pPPD[j].mCovariance.r2 = 0;
        pPPD[j].mCovariance.r3 = 0;
        pPPD[j].mCovariance.r4 = 0;
 
        Math2d::SetVec(pShPPD[j].vMean, 0, 0);
        pShPPD[j].mCovariance.r1 = 0;
        pShPPD[j].mCovariance.r2 = 0;
        pShPPD[j].mCovariance.r3 = 0;
        pShPPD[j].mCovariance.r4 = 0;
    }
 
 
    // calculate means
    //ARMARX_VERBOSE_S << "Calculating means";
    for (int i = 0; i < nNumImages; i++)
    {
        //if (i%10==0) ARMARX_VERBOSE_S << "      Image %d of %d\n", i, nNumImages);
        for (int j = 0; j < m_nImageWidth * m_nImageHeight; j++)
        {
            pPPD[j].vMean.x += pHSVImages[i]->pixels[3 * j];
            pPPD[j].vMean.y += pHSVImages[i]->pixels[3 * j + 1];
 
            pShPPD[j].vMean.x += (pHSVImages[i]->pixels[3 * j] + 128) % 256;
            pShPPD[j].vMean.y += pHSVImages[i]->pixels[3 * j + 1];
        }
    }
 
    for (int j = 0; j < m_nImageWidth * m_nImageHeight; j++)
    {
        pPPD[j].vMean.x /= nNumImages;
        pPPD[j].vMean.y /= nNumImages;
 
        pShPPD[j].vMean.x /= nNumImages;
        pShPPD[j].vMean.y /= nNumImages;
    }
 
 
    // calculate covariance matrices
    if (nNumImages > 1)
    {
        //ARMARX_VERBOSE_S << " -- Calculating covariance matrices --\n");
        for (int i = 0; i < nNumImages; i++)
        {
            //if (i%10==0) ARMARX_VERBOSE_S << "      Image %d of %d\n", i, nNumImages);
            for (int j = 0; j < m_nImageWidth * m_nImageHeight; j++)
            {
                pPPD[j].mCovariance.r1 += (pHSVImages[i]->pixels[3 * j] - pPPD[j].vMean.x) *
                                          (pHSVImages[i]->pixels[3 * j] - pPPD[j].vMean.x);
                pPPD[j].mCovariance.r2 += (pHSVImages[i]->pixels[3 * j] - pPPD[j].vMean.x) *
                                          (pHSVImages[i]->pixels[3 * j + 1] - pPPD[j].vMean.y);
                pPPD[j].mCovariance.r3 += (pHSVImages[i]->pixels[3 * j + 1] - pPPD[j].vMean.y) *
                                          (pHSVImages[i]->pixels[3 * j] - pPPD[j].vMean.x);
                pPPD[j].mCovariance.r4 += (pHSVImages[i]->pixels[3 * j + 1] - pPPD[j].vMean.y) *
                                          (pHSVImages[i]->pixels[3 * j + 1] - pPPD[j].vMean.y);
 
                pShPPD[j].mCovariance.r1 +=
                    (((pHSVImages[i]->pixels[3 * j] + 128) % 256) - pShPPD[j].vMean.x) *
                    (((pHSVImages[i]->pixels[3 * j] + 128) % 256) - pShPPD[j].vMean.x);
                pShPPD[j].mCovariance.r2 +=
                    (((pHSVImages[i]->pixels[3 * j] + 128) % 256) - pShPPD[j].vMean.x) *
                    (pHSVImages[i]->pixels[3 * j + 1] - pShPPD[j].vMean.y);
                pShPPD[j].mCovariance.r3 +=
                    (pHSVImages[i]->pixels[3 * j + 1] - pShPPD[j].vMean.y) *
                    (((pHSVImages[i]->pixels[3 * j] + 128) % 256) - pShPPD[j].vMean.x);
                pShPPD[j].mCovariance.r4 += (pHSVImages[i]->pixels[3 * j + 1] - pShPPD[j].vMean.y) *
                                            (pHSVImages[i]->pixels[3 * j + 1] - pShPPD[j].vMean.y);
            }
        }
 
        const float fFactor = 1.0f / (float)(nNumImages - 1);
 
        for (int j = 0; j < m_nImageWidth * m_nImageHeight; j++)
        {
            pPPD[j].mCovariance.r1 *= fFactor;
            pPPD[j].mCovariance.r2 *= fFactor;
            pPPD[j].mCovariance.r3 *= fFactor;
            pPPD[j].mCovariance.r4 *= fFactor;
 
            pShPPD[j].mCovariance.r1 *= fFactor;
            pShPPD[j].mCovariance.r2 *= fFactor;
            pShPPD[j].mCovariance.r3 *= fFactor;
            pShPPD[j].mCovariance.r4 *= fFactor;
 
 
            // if shifted version has smaller variance, use it
            if (DetMat2d(pPPD[j].mCovariance) > DetMat2d(pShPPD[j].mCovariance))
            {
                pPPD[j].vMean.x = pShPPD[j].vMean.x;
                pPPD[j].vMean.y = pShPPD[j].vMean.y;
 
                pPPD[j].mCovariance.r1 = pShPPD[j].mCovariance.r1;
                pPPD[j].mCovariance.r2 = pShPPD[j].mCovariance.r2;
                pPPD[j].mCovariance.r3 = pShPPD[j].mCovariance.r3;
                pPPD[j].mCovariance.r4 = pShPPD[j].mCovariance.r4;
 
                pPPD[j].bShifted = true;
            }
            else
            {
                pPPD[j].bShifted = false;
            }
 
            // smooth variance
            pPPD[j].mCovariance.r1 += 1;
            pPPD[j].mCovariance.r4 += 1;
 
            //if (j%10000==0) ARMARX_VERBOSE_S << "Matrix: (%.2f %.2f   %.2f %.2f)\n", pPPD[j].mCovariance.r1, pPPD[j].mCovariance.r2, pPPD[j].mCovariance.r3, pPPD[j].mCovariance.r4);
 
            // invert the covariance matrix for faster evaluation
            Math2d::Invert(pPPD[j].mCovariance, pPPD[j].mCovariance);
 
            pPPD[j].fSatWeight = exp(-pPPD[j].vMean.y * pPPD[j].vMean.y / 6400);
        }
    }
    else
    {
        for (int j = 0; j < m_nImageWidth * m_nImageHeight; j++)
        {
            pPPD[j].mCovariance.r1 = 1.0f / 264.0f;
            pPPD[j].mCovariance.r2 = 0;
            pPPD[j].mCovariance.r3 = 0;
            pPPD[j].mCovariance.r4 = 1.0f / 264.0f;
 
            pPPD[j].fSatWeight = exp(-pPPD[j].vMean.y * pPPD[j].vMean.y / 6400);
 
            if (pPPD[j].vMean.x < 64 || pPPD[j].vMean.x > 192)
            {
                pPPD[j].vMean.x = ((int)pPPD[j].vMean.x + 128) % 256;
                pPPD[j].bShifted = true;
            }
            else
            {
                pPPD[j].bShifted = false;
            }
        }
    }
 
    delete[] pShPPD;
}
 
inline double
CGaussBackground::CalcProbOfPixel(const int nIndex, const Vec2d vHS_Value)
{
    double dTemp0;
    Vec2d vTemp1, vTemp2;
 
    // v = pixel value - mean
    Math2d::SubtractVecVec(vHS_Value, m_pPixelProbabilityDistributions[nIndex].vMean, vTemp1);
    // d = v^T*Sigma^-1*v
    Math2d::MulMatVec(m_pPixelProbabilityDistributions[nIndex].mCovariance, vTemp1, vTemp2);
    dTemp0 = Math2d::ScalarProduct(vTemp1, vTemp2);
 
    return exp(-0.03 *
               (m_pPixelProbabilityDistributions[nIndex].fSatWeight * (vTemp1.y * vTemp1.y) *
                    m_pPixelProbabilityDistributions[nIndex].mCovariance.r4 +
                (1 - m_pPixelProbabilityDistributions[nIndex].fSatWeight) * dTemp0));
    //return exp(-0.05 * dTemp0);
}
 
void
CGaussBackground::BinarizeAndFillHoles(CByteImage* pProbabilityImage)
{
    //char pFileName[] = "/home/staff/schieben/datalog/bg0.bmp";
    //pProbabilityImage->SaveToFile(pFileName);
 
    // binarize
    for (int i = 0; i < m_nImageWidth * m_nImageHeight; i++)
    {
        pProbabilityImage->pixels[i] =
            (pProbabilityImage->pixels[i] > OLP_FOREGROUND_THRESHOLD) ? 255 : 0;
    }
 
 
    // remove noise
    CByteImage* pTempImage = new CByteImage(m_nImageWidth, m_nImageHeight, CByteImage::eGrayScale);
    ImageProcessor::Dilate(pProbabilityImage, pTempImage);
    ImageProcessor::Erode(pTempImage, pProbabilityImage);
    ImageProcessor::Erode(pProbabilityImage, pTempImage);
    ImageProcessor::Erode(pTempImage, pProbabilityImage);
    ImageProcessor::Erode(pProbabilityImage, pTempImage);
    ImageProcessor::Dilate(pTempImage, pProbabilityImage);
    ImageProcessor::Dilate(pProbabilityImage, pTempImage);
    ImageProcessor::Dilate(pTempImage, pProbabilityImage);
    ImageProcessor::Dilate(pProbabilityImage, pTempImage);
    ImageProcessor::Dilate(pTempImage, pProbabilityImage);
 
 
    // fill holes
    for (int i = 0; i < 1; i++) // 1
    {
        FillHolesHorVertDiag(pProbabilityImage, pTempImage);
        FillHolesHorVertDiag(pTempImage, pProbabilityImage);
        //pFileName[31] = '1'+i;
        //pProbabilityImage->SaveToFile(pFileName);
    }
 
    for (int i = 0; i < 2; i++) // 2
    {
        FillHolesHorVert(pProbabilityImage, pTempImage);
        FillHolesDiag(pTempImage, pProbabilityImage);
        //pFileName[31] = '2'+i;
        //pProbabilityImage->SaveToFile(pFileName);
    }
 
    // extend foreground regions
    for (int i = 0; i < 0; i++) // 1
    {
        ImageProcessor::Dilate(pProbabilityImage, pTempImage);
        ImageProcessor::Dilate(pTempImage, pProbabilityImage);
    }
 
    delete pTempImage;
}
 
inline void
CGaussBackground::FillHolesHorVert(const CByteImage* pInputImage,
                                   CByteImage* pOutputImage,
                                   const int nRadius)
{
    for (int i = 0; i < m_nImageWidth * m_nImageHeight; i++)
    {
        pOutputImage->pixels[i] = pInputImage->pixels[i];
    }
 
#pragma omp parallel for schedule(static, 100)
    for (int i = 0; i < m_nImageWidth * m_nImageHeight; i++)
    {
        if (pOutputImage->pixels[i] == 0)
        {
            // check for white pixels in all directions
            int nPixelSumsInDirections[4] = {0, 0, 0, 0};
            int nIndex;
 
            for (int j = 1; j <= nRadius; j++)
            {
                // to the left
                nIndex = i - j;
 
                if (nIndex >= 0)
                {
                    nPixelSumsInDirections[0] += pInputImage->pixels[nIndex];
                }
 
                // to the right
                nIndex = i + j;
 
                if (nIndex < m_nImageWidth * m_nImageHeight)
                {
                    nPixelSumsInDirections[1] += pInputImage->pixels[nIndex];
                }
 
                // upwards
                nIndex = i - j * m_nImageWidth;
 
                if (nIndex >= 0)
                {
                    nPixelSumsInDirections[2] += pInputImage->pixels[nIndex];
                }
 
                // downwards
                nIndex = i + j * m_nImageWidth;
 
                if (nIndex < m_nImageWidth * m_nImageHeight)
                {
                    nPixelSumsInDirections[3] += pInputImage->pixels[nIndex];
                }
            }
 
            if (nPixelSumsInDirections[0] * nPixelSumsInDirections[1] * nPixelSumsInDirections[2] *
                    nPixelSumsInDirections[3] !=
                0)
            {
                pOutputImage->pixels[i] = 255;
            }
        }
    }
}
 
inline void
CGaussBackground::FillHolesDiag(const CByteImage* pInputImage,
                                CByteImage* pOutputImage,
                                const int nRadius)
{
    for (int i = 0; i < m_nImageWidth * m_nImageHeight; i++)
    {
        pOutputImage->pixels[i] = pInputImage->pixels[i];
    }
 
#pragma omp parallel for schedule(static, 100)
    for (int i = 0; i < m_nImageWidth * m_nImageHeight; i++)
    {
        if (pOutputImage->pixels[i] == 0)
        {
            // check for white pixels in all directions
            int nPixelSumsInDirections[4] = {0, 0, 0, 0};
            int nIndex;
 
            for (int j = 1; j <= nRadius; j++)
            {
                // to the left and up
                nIndex = i - j * m_nImageWidth - j;
 
                if (nIndex >= 0)
                {
                    nPixelSumsInDirections[0] += pInputImage->pixels[nIndex];
                }
 
                // to the right and up
                nIndex = i - j * m_nImageWidth + j;
 
                if (nIndex >= 0)
                {
                    nPixelSumsInDirections[1] += pInputImage->pixels[nIndex];
                }
 
                // to the left and down
                nIndex = i + j * m_nImageWidth - j;
 
                if (nIndex < m_nImageWidth * m_nImageHeight)
                {
                    nPixelSumsInDirections[2] += pInputImage->pixels[nIndex];
                }
 
                // to the right and down
                nIndex = i + j * m_nImageWidth + j;
 
                if (nIndex < m_nImageWidth * m_nImageHeight)
                {
                    nPixelSumsInDirections[3] += pInputImage->pixels[nIndex];
                }
            }
 
            if (nPixelSumsInDirections[0] * nPixelSumsInDirections[1] * nPixelSumsInDirections[2] *
                    nPixelSumsInDirections[3] !=
                0)
            {
                pOutputImage->pixels[i] = 255;
            }
        }
    }
}
 
inline void
CGaussBackground::FillHolesHorVertDiag(const CByteImage* pInputImage,
                                       CByteImage* pOutputImage,
                                       const int nRadius)
{
    for (int i = 0; i < m_nImageWidth * m_nImageHeight; i++)
    {
        pOutputImage->pixels[i] = pInputImage->pixels[i];
    }
 
#pragma omp parallel for schedule(static, 100)
    for (int i = 0; i < m_nImageWidth * m_nImageHeight; i++)
    {
        if (pOutputImage->pixels[i] == 0)
        {
            // check for white pixels in all directions
            int nPixelSumsInDirections[8] = {0, 0, 0, 0, 0, 0, 0, 0};
            int nIndex;
 
            for (int j = 1; j <= nRadius; j++)
            {
                // to the left
                nIndex = i - j;
 
                if (nIndex >= 0)
                {
                    nPixelSumsInDirections[0] += pInputImage->pixels[nIndex];
                }
 
                // to the right
                nIndex = i + j;
 
                if (nIndex < m_nImageWidth * m_nImageHeight)
                {
                    nPixelSumsInDirections[1] += pInputImage->pixels[nIndex];
                }
 
                // upwards
                nIndex = i - j * m_nImageWidth;
 
                if (nIndex >= 0)
                {
                    nPixelSumsInDirections[2] += pInputImage->pixels[nIndex];
                }
 
                // downwards
                nIndex = i + j * m_nImageWidth;
 
                if (nIndex < m_nImageWidth * m_nImageHeight)
                {
                    nPixelSumsInDirections[3] += pInputImage->pixels[nIndex];
                }
 
                // to the left and up
                nIndex = i - j * m_nImageWidth - j;
 
                if (nIndex >= 0)
                {
                    nPixelSumsInDirections[4] += pInputImage->pixels[nIndex];
                }
 
                // to the right and up
                nIndex = i - j * m_nImageWidth + j;
 
                if (nIndex >= 0)
                {
                    nPixelSumsInDirections[5] += pInputImage->pixels[nIndex];
                }
 
                // to the left and down
                nIndex = i + j * m_nImageWidth - j;
 
                if (nIndex < m_nImageWidth * m_nImageHeight)
                {
                    nPixelSumsInDirections[6] += pInputImage->pixels[nIndex];
                }
 
                // to the right and down
                nIndex = i + j * m_nImageWidth + j;
 
                if (nIndex < m_nImageWidth * m_nImageHeight)
                {
                    nPixelSumsInDirections[7] += pInputImage->pixels[nIndex];
                }
            }
 
            if (nPixelSumsInDirections[0] * nPixelSumsInDirections[1] * nPixelSumsInDirections[2] *
                    nPixelSumsInDirections[3] * nPixelSumsInDirections[4] *
                    nPixelSumsInDirections[5] * nPixelSumsInDirections[6] *
                    nPixelSumsInDirections[7] !=
                0)
            {
                pOutputImage->pixels[i] = 255;
            }
        }
    }
}
 
void
CGaussBackground::GetBinaryForegroundImageRGB(const CByteImage* pInputImageRGB,
                                              CByteImage* pForegroundImage)
{
    SegmentImageRGB(pInputImageRGB, pForegroundImage);
 
    BinarizeAndFillHoles(pForegroundImage);
}
 
void
CGaussBackground::SegmentImageRGB(const CByteImage* pInputImageRGB, CByteImage* pProbabilityImage)
{
    CByteImage* pHSVImage = new CByteImage(m_nImageWidth, m_nImageHeight, CByteImage::eRGB24);
    CByteImage* pSmoothedImage = new CByteImage(m_nImageWidth, m_nImageHeight, CByteImage::eRGB24);
 
    ImageProcessor::GaussianSmooth3x3(pInputImageRGB, pSmoothedImage);
    ImageProcessor::CalculateHSVImage(pSmoothedImage, pHSVImage);
 
    SegmentImage(pInputImageRGB, pHSVImage, pProbabilityImage);
 
    delete pHSVImage;
    delete pSmoothedImage;
}
 
void
CGaussBackground::SegmentImageRGB(const CByteImage* pInputImageRGB, float* pProbabilityImage)
{
    CByteImage* pHSVImage = new CByteImage(m_nImageWidth, m_nImageHeight, CByteImage::eRGB24);
    CByteImage* pSmoothedImage = new CByteImage(m_nImageWidth, m_nImageHeight, CByteImage::eRGB24);
 
    ImageProcessor::GaussianSmooth3x3(pInputImageRGB, pSmoothedImage);
    ImageProcessor::CalculateHSVImage(pSmoothedImage, pHSVImage);
 
    SegmentImage(pInputImageRGB, pHSVImage, pProbabilityImage);
 
    delete pHSVImage;
    delete pSmoothedImage;
}
 
void
CGaussBackground::SegmentImage(const CByteImage* pInputImageRGB,
                               const CByteImage* pInputImageHSV,
                               CByteImage* pProbabilityImage)
{
    if ((pInputImageHSV->width != m_nImageWidth) || (pInputImageHSV->height != m_nImageHeight))
    {
        ARMARX_WARNING_S << "CGaussBackground::SegmentImage: Image dimensions do not fit!";
        return;
    }
 
    float* pProbabilities = new float[m_nImageWidth * m_nImageHeight];
    SegmentImage(pInputImageRGB, pInputImageHSV, pProbabilities);
 
    for (int i = 0; i < m_nImageWidth * m_nImageHeight; i++)
    {
        pProbabilityImage->pixels[i] = (int)(255 * pProbabilities[i]);
    }
 
    delete[] pProbabilities;
}
 
void
CGaussBackground::SegmentImage(const CByteImage* pInputImageRGB,
                               const CByteImage* pInputImageHSV,
                               float* pProbabilityImage)
{
    if ((pInputImageHSV->width != m_nImageWidth) || (pInputImageHSV->height != m_nImageHeight))
    {
        ARMARX_WARNING_S << "CGaussBackground::SegmentImage: Image dimensions do not fit!";
        return;
    }
 
    const int nNumShifts = 21;
    int nImageShiftValuesX[nNumShifts] = {-1, 1,  -2, 0, 2, -3, -1, 1, 3,  -2, 0,
                                          2,  -3, -1, 1, 3, -2, 0,  2, -1, 1};
    int nImageShiftValuesY[nNumShifts] = {-3, -3, -2, -2, -2, -1, -1, -1, -1, 0, 0,
                                          0,  1,  1,  1,  1,  2,  2,  2,  3,  3};
    float fProbSums[nNumShifts] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    float* pProbabilityImages = new float[nNumShifts * m_nImageWidth * m_nImageHeight];
 
    // set to 0
    for (int i = 0; i < nNumShifts * m_nImageWidth * m_nImageHeight; i++)
    {
        pProbabilityImages[i] = 0;
    }
 
// calc background probability for every pixel
#pragma omp parallel for schedule(static, 1)
    for (int n = 0; n < nNumShifts; n++)
    {
        for (int i = 3; i < m_nImageHeight - 3; i++)
        {
            for (int j = 3; j < m_nImageWidth - 3; j++)
            {
                const int nIndex = i * m_nImageWidth + j;
                const int nIndexSh =
                    (i + nImageShiftValuesY[n]) * m_nImageWidth + j + nImageShiftValuesX[n];
 
                // calculate probability using HS-distribution
                Vec2d vTemp1;
 
                if (m_pPixelProbabilityDistributions[nIndexSh].bShifted)
                {
                    vTemp1.x = (pInputImageHSV->pixels[3 * nIndex] + 128) %
                               256; // adapt pixel value if the mean is shifted
                }
                else
                {
                    vTemp1.x = pInputImageHSV->pixels[3 * nIndex];
                }
 
                vTemp1.y = pInputImageHSV->pixels[3 * nIndex + 1];
                const float fProbHSV = 1 - CalcProbOfPixel(nIndexSh, vTemp1);
 
                // calculate probability using the RGB image
                const Vec3d vRGBfromImage = {(float)pInputImageRGB->pixels[3 * nIndex],
                                             (float)pInputImageRGB->pixels[3 * nIndex + 1],
                                             (float)pInputImageRGB->pixels[3 * nIndex + 2]};
                const Vec3d vRGBfromBackground = {
                    (float)m_pBackgroundRGB->pixels[3 * nIndexSh],
                    (float)m_pBackgroundRGB->pixels[3 * nIndexSh + 1],
                    (float)m_pBackgroundRGB->pixels[3 * nIndexSh + 2]};
                const float fDistance = Math3d::Distance(vRGBfromImage, vRGBfromBackground);
                const float fProbRGB = 1 - exp(-0.01f * fDistance);
 
 
                const float fProb =
                    0.33f *
                    (fProbRGB +
                     2 * fProbHSV); // (fProbRGB*fProbHSV) / (fProbRGB*fProbHSV + (1-fProbRGB)*(1-fProbHSV) + 0.00001f);
                pProbabilityImages[n * m_nImageWidth * m_nImageHeight + nIndex] = fProb;
                fProbSums[n] += fProb;
            }
        }
    }
 
    float fMinProb = fProbSums[0];
    int nBestIndex = 0;
 
    for (int i = 1; i < nNumShifts; i++)
    {
        if (fProbSums[i] < fMinProb)
        {
            fMinProb = fProbSums[i];
            nBestIndex = i;
        }
    }
 
    ARMARX_VERBOSE_S << "Best background shift: (" << nImageShiftValuesX[nBestIndex] << ", "
                     << nImageShiftValuesY[nBestIndex] << ")";
 
    for (int i = 0; i < m_nImageWidth * m_nImageHeight; i++)
    {
        pProbabilityImage[i] = pProbabilityImages[nBestIndex * m_nImageWidth * m_nImageHeight + i];
    }
 
    delete[] pProbabilityImages;
}