blobtrackingmsfg.cpp

Using open CV draw color histogram, convert RGB To HSI

/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "_cvaux.h"

// Uncomment to trade flexibility for speed
//#define CONST_HIST_SIZE

// Uncomment to get some performance stats in stderr
//#define REPORT_TICKS

#ifdef CONST_HIST_SIZE
#define m_BinBit 5
#define m_ByteShift 3
#endif

typedef float DefHistType;
#define DefHistTypeMat CV_32F
#define HIST_INDEX(_pData) (((_pData)[0]>>m_ByteShift) + (((_pData)[1]>>(m_ByteShift))<<m_BinBit)+((pImgData[2]>>m_ByteShift)<<(m_BinBit*2)))

class DefHist
{
public:
    CvMat*          m_pHist;
    DefHistType     m_HistVolume;
    DefHist(int BinNum=0)
    {
        m_pHist = NULL;
        m_HistVolume = 0;
        Resize(BinNum);
    }
    ~DefHist()
    {
        if(m_pHist)cvReleaseMat(&m_pHist);
    }
    void Resize(int BinNum)
    {
        if(m_pHist)cvReleaseMat(&m_pHist);
        if(BinNum>0)
        {
            m_pHist = cvCreateMat(1, BinNum, DefHistTypeMat);
            cvZero(m_pHist);
        }
        m_HistVolume = 0;
    }
    void Update(DefHist* pH, float W)
    {/* update hist */
        double  Vol, WM, WC;
        Vol = 0.5*(m_HistVolume + pH->m_HistVolume);
        WM = Vol*(1-W)/m_HistVolume;
        WC = Vol*(W)/pH->m_HistVolume;
        cvAddWeighted(m_pHist, WM, pH->m_pHist,WC,0,m_pHist);
        m_HistVolume = (float)cvSum(m_pHist).val[0];
    }/* update hist */
};

class CvBlobTrackerOneMSFG:public CvBlobTrackerOne
{
protected:
    int             m_BinNumTotal; /* protected only for parralel MSPF */
    CvSize          m_ObjSize;
    void ReAllocKernel(int  w, int h)
    {
        int     x,y;
        float   x0 = 0.5f*(w-1);
        float   y0 = 0.5f*(h-1);
        assert(w>0);
        assert(h>0);
        m_ObjSize = cvSize(w,h);

        if(m_KernelHist) cvReleaseMat(&m_KernelHist);
        if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
        m_KernelHist = cvCreateMat(h, w, DefHistTypeMat);
        m_KernelMeanShift = cvCreateMat(h, w, DefHistTypeMat);
        for(y=0;y<h;++y)for(x=0;x<w;++x)
        {
            double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
//            double r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((y0*y0)+(x0*x0));
            CV_MAT_ELEM(m_KernelHist[0],DefHistType, y, x) = (DefHistType)GetKernelHist(r2);
            CV_MAT_ELEM(m_KernelMeanShift[0],DefHistType, y, x) = (DefHistType)GetKernelMeanShift(r2);
        }
    }
private:
    /* parameters */
    int             m_IterNum;
    float           m_FGWeight;
    float           m_Alpha;
    CvMat*          m_KernelHist;
    CvMat*          m_KernelMeanShift;
#ifndef CONST_HIST_SIZE
    int             m_BinBit;
    int             m_ByteShift;
#endif
    int             m_BinNum;
    int             m_Dim;
    /*
    CvMat*          m_HistModel;
    float           m_HistModelVolume;
    CvMat*          m_HistCandidate;
    float           m_HistCandidateVolume;
    CvMat*          m_HistTemp;
    */
    DefHist         m_HistModel;
    DefHist         m_HistCandidate;
    DefHist         m_HistTemp;

    CvBlob          m_Blob;
    int             m_Collision;

    void ReAllocHist(int Dim, int BinBit)
    {
#ifndef CONST_HIST_SIZE
        m_BinBit = BinBit;
        m_ByteShift = 8-BinBit;
#endif
        m_Dim = Dim;
        m_BinNum = (1<<BinBit);
        m_BinNumTotal = cvRound(pow((double)m_BinNum,(double)m_Dim));
        /*
        if(m_HistModel) cvReleaseMat(&m_HistModel);
        if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
        if(m_HistTemp) cvReleaseMat(&m_HistTemp);
        m_HistCandidate = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
        m_HistModel = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
        m_HistTemp = cvCreateMat(1, m_BinNumTotal, DefHistTypeMat);
        cvZero(m_HistCandidate);
        cvZero(m_HistModel);
        m_HistModelVolume = 0.0f;
        m_HistCandidateVolume = 0.0f;
        */
        m_HistCandidate.Resize(m_BinNumTotal);
        m_HistModel.Resize(m_BinNumTotal);
        m_HistTemp.Resize(m_BinNumTotal);
    }
    double GetKernelHist(double r2)
    {
        return (r2 < 1) ? 1 -  r2 : 0;
    }
    double GetKernelMeanShift(double r2)
    {
        return (r2<1) ? 1 : 0;
    }
//    void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pHist, DefHistType* pHistVolume)
//    void CollectHist(IplImage* pImg, IplImage* pMask, CvPoint Center, DefHist* pHist)
    void CollectHist(IplImage* pImg, IplImage* pMask, CvBlob* pBlob, DefHist* pHist)
    {
        int UsePrecalculatedKernel = 0;
        int BW = cvRound(pBlob->w);
        int BH = cvRound(pBlob->h);
        DefHistType Volume = 0;
        int         x0 = cvRound(pBlob->x - BW*0.5);
        int         y0 = cvRound(pBlob->y - BH*0.5);
        int         x,y;

        UsePrecalculatedKernel = (BW == m_ObjSize.width && BH == m_ObjSize.height ) ;

        //cvZero(pHist);
        cvSet(pHist->m_pHist,cvScalar(1.0/m_BinNumTotal)); /* no zero bins, all bins have very small value*/
        Volume = 1;

        assert(BW < pImg->width);
        assert(BH < pImg->height);
        if((x0+BW)>=pImg->width) BW=pImg->width-x0-1;
        if((y0+BH)>=pImg->height) BH=pImg->height-y0-1;
        if(x0<0){ x0=0;}
        if(y0<0){ y0=0;}

        if(m_Dim == 3)
        {
            for(y=0;y<BH;++y)
            {
                unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
                unsigned char* pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
                DefHistType* pKernelData = NULL;
                if(UsePrecalculatedKernel)
                {
                    pKernelData = ((DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelHist[0],y,0,sizeof(DefHistType)));
                }
                for(x=0;x<BW;++x,pImgData+=3)
                {
                    DefHistType K;
                    int index = HIST_INDEX(pImgData);
                    assert(index >= 0 && index < pHist->m_pHist->cols);
                    if(UsePrecalculatedKernel)
                    {
                        K = pKernelData[x];
                    }
                    else
                    {
                        float dx = (x+x0-pBlob->x)/(pBlob->w*0.5f);
                        float dy = (y+y0-pBlob->y)/(pBlob->h*0.5f);
                        double r2 = dx*dx+dy*dy;
                        K = (float)GetKernelHist(r2);
                    }

                    if(pMaskData)
                    {
                        K *= pMaskData[x]*0.003921568627450980392156862745098f;
                    }
                    Volume += K;
                    ((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;
                }/* next column */
            }/* next row */
        }/* if m_Dim == 3 */
        pHist->m_HistVolume = Volume;
    };/*CollectHist*/
    double calcBhattacharyya(DefHist* pHM = NULL, DefHist* pHC = NULL, DefHist* pHT = NULL)
    {
        if(pHM==NULL) pHM = &m_HistModel;
        if(pHC==NULL) pHC = &m_HistCandidate;
        if(pHT==NULL) pHT = &m_HistTemp;
        if(pHC->m_HistVolume*pHM->m_HistVolume > 0)
        {
#if 0
            // use CV functions
            cvMul(pHM->m_pHist,pHC->m_pHist,pHT->m_pHist);
            cvPow(pHT->m_pHist,pHT->m_pHist,0.5);
            return cvSum(pHT->m_pHist).val[0] / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
#else
            // Do computations manually and let autovectorizer do the job
            DefHistType *hm, *hc, *ht;
            double sum;
            int     size;
            hm=(DefHistType *)(pHM->m_pHist->data.ptr);
            hc=(DefHistType *)(pHC->m_pHist->data.ptr);
            ht=(DefHistType *)(pHT->m_pHist->data.ptr);
            size = pHM->m_pHist->width*pHM->m_pHist->height;
            sum = 0.;
            for(int i = 0; i < size; i++ )
            {
                sum += sqrt(hm[i]*hc[i]);
            }
            return sum / sqrt(pHC->m_HistVolume*pHM->m_HistVolume);
#endif
        }
        return 0;
    } /* calcBhattacharyyaCoefficient */
protected:
    //double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, float x, float y, DefHist* pHist=NULL)
    double GetBhattacharyya(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob, DefHist* pHist=NULL, int /*thread_number*/ = 0)
    {
        if(pHist==NULL)pHist = &m_HistTemp;
        CollectHist(pImg, pImgFG, pBlob, pHist);
        return calcBhattacharyya(&m_HistModel, pHist, pHist);
    }
    void UpdateModelHist(IplImage* pImg, IplImage* pImgFG, CvBlob* pBlob)
    {
        if(m_Alpha>0 && !m_Collision)
        {/* update hist */
            CollectHist(pImg, pImgFG, pBlob, &m_HistCandidate);
            m_HistModel.Update(&m_HistCandidate, m_Alpha);
        }/* update hist */
    }/* UpdateModelHist*/

public:
    CvBlobTrackerOneMSFG()
    {

        /* add several parameters for external use */
        m_FGWeight = 2;
        AddParam("FGWeight", &m_FGWeight);
        CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");

        m_Alpha = 0.01f;
        AddParam("Alpha", &m_Alpha);
        CommentParam("Alpha","Coefficient for model histogramm updating (0 - hist is not upated)");

        m_IterNum = 10;
        AddParam("IterNum", &m_IterNum);
        CommentParam("IterNum","Maximal number of iteration in meanshift operation");

        /* init internal data */
        m_Collision = 0;
//        m_BinBit=0;
        m_Dim = 0;
        /*
        m_HistModel = NULL;
        m_HistCandidate = NULL;
        m_HistTemp = NULL;
        */
        m_KernelHist = NULL;
        m_KernelMeanShift = NULL;
        ReAllocHist(3,5);   /* 3D hist, each dim has 2^5 bins*/
    }
    ~CvBlobTrackerOneMSFG()
    {
        /*
        if(m_HistModel) cvReleaseMat(&m_HistModel);
        if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
        if(m_HistTemp) cvReleaseMat(&m_HistTemp);
        */
        if(m_KernelHist) cvReleaseMat(&m_KernelHist);
        if(m_KernelMeanShift) cvReleaseMat(&m_KernelMeanShift);
    }
    /* interface */
    virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int w = cvRound(CV_BLOB_WX(pBlobInit));
        int h = cvRound(CV_BLOB_WY(pBlobInit));
        if(w<CV_BLOB_MINW)w=CV_BLOB_MINW;
        if(h<CV_BLOB_MINH)h=CV_BLOB_MINH;
        if(pImg)
        {
            if(w>pImg->width)w=pImg->width;
            if(h>pImg->height)h=pImg->height;
        }
        ReAllocKernel(w,h);
        if(pImg)
            CollectHist(pImg, pImgFG, pBlobInit, &m_HistModel);
        m_Blob = pBlobInit[0];
    };
    virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int     iter;

        if(pBlobPrev)
        {
            m_Blob = pBlobPrev[0];
        }

        {/* check blob size and realloc kernels if it is necessary */
            int w = cvRound(m_Blob.w);
            int h = cvRound(m_Blob.h);
            if( w != m_ObjSize.width || h!=m_ObjSize.height)
            {
                ReAllocKernel(w,h);
                /* after this ( w != m_ObjSize.width || h!=m_ObjSize.height) shoiuld be false */
            }
        }/* check blob size and realloc kernels if it is necessary */