blobtrackingmsfgs.cpp

Program use openCV to demo edge detection (algorithm Gradient).

/*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"

#define SCALE_BASE 1.1
#define SCALE_RANGE 2
#define SCALE_NUM (2*SCALE_RANGE+1)
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)))

void calcKernelEpanechnikov(CvMat* pK)
{/* allocate kernel for histogramm creation */
    int     x,y;
    int     w = pK->width;
    int     h = pK->height;
    float   x0 = 0.5f*(w-1);
    float   y0 = 0.5f*(h-1);
    for(y=0;y<h;++y)for(x=0;x<w;++x)
    {
//                float   r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
        float   r2 = ((x-x0)*(x-x0)+(y-y0)*(y-y0))/((x0*x0)+(y0*y0));
        CV_MAT_ELEM(pK[0],DefHistType, y, x) = (DefHistType)((r2<1)?(1-r2):0);
    }
}/* allocate kernel for histogramm creation */

class CvBlobTrackerOneMSFGS:public CvBlobTrackerOne
{
private:
    /* parameters */
    float           m_FGWeight;
    float           m_Alpha;
    CvSize          m_ObjSize;
    CvMat*          m_KernelHistModel;
    CvMat*          m_KernelHistCandidate;
    CvSize          m_KernelMeanShiftSize;
    CvMat*          m_KernelMeanShiftK[SCALE_NUM];
    CvMat*          m_KernelMeanShiftG[SCALE_NUM];
    CvMat*          m_Weights;
    int             m_BinBit;
    int             m_ByteShift;
    int             m_BinNum;
    int             m_Dim;
    int             m_BinNumTotal;
    CvMat*          m_HistModel;
    float           m_HistModelVolume;
    CvMat*          m_HistCandidate;
    float           m_HistCandidateVolume;
    CvMat*          m_HistTemp;
    CvBlob          m_Blob;
    void ReAllocHist(int Dim, int BinBit)
    {
        m_BinBit = BinBit;
        m_ByteShift = 8-BinBit;
        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;
    }
    void ReAllocKernel(int  w, int h, float sigma=0.4)
    {
        double  ScaleToObj = sigma*1.39;
        int     kernel_width = cvRound(w/ScaleToObj);
        int     kernel_height = cvRound(h/ScaleToObj);
        int     x,y,s;
        assert(w>0);
        assert(h>0);
        m_ObjSize = cvSize(w,h);
        m_KernelMeanShiftSize = cvSize(kernel_width,kernel_height);

        
        /* create kernels for histogramm calculation */
        if(m_KernelHistModel) cvReleaseMat(&m_KernelHistModel);
        m_KernelHistModel = cvCreateMat(h, w, DefHistTypeMat);
        calcKernelEpanechnikov(m_KernelHistModel);
        if(m_KernelHistCandidate) cvReleaseMat(&m_KernelHistCandidate);
        m_KernelHistCandidate = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
        calcKernelEpanechnikov(m_KernelHistCandidate);

        if(m_Weights) cvReleaseMat(&m_Weights);
        m_Weights = cvCreateMat(kernel_height, kernel_width, CV_32F);
        
        for(s=-SCALE_RANGE;s<=SCALE_RANGE;++s)
        {/* allocate kernwl for meanshifts in space and scale */
            int     si = s+SCALE_RANGE;
            double  cur_sigma = sigma * pow(SCALE_BASE,s);
            double  cur_sigma2 = cur_sigma*cur_sigma;
            double  x0 = 0.5*(kernel_width-1);
            double  y0 = 0.5*(kernel_height-1);
            if(m_KernelMeanShiftK[si]) cvReleaseMat(&m_KernelMeanShiftK[si]);
            if(m_KernelMeanShiftG[si]) cvReleaseMat(&m_KernelMeanShiftG[si]);
            m_KernelMeanShiftK[si] = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
            m_KernelMeanShiftG[si] = cvCreateMat(kernel_height, kernel_width, DefHistTypeMat);
            for(y=0;y<kernel_height;++y)
            {
                DefHistType* pK = (DefHistType*)CV_MAT_ELEM_PTR_FAST( m_KernelMeanShiftK[si][0], y, 0, sizeof(DefHistType) ); 
                DefHistType* pG = (DefHistType*)CV_MAT_ELEM_PTR_FAST( m_KernelMeanShiftG[si][0], y, 0, sizeof(DefHistType) ); 
                for(x=0;x<kernel_width;++x)
                {
                    double r2 = ((x-x0)*(x-x0)/(x0*x0)+(y-y0)*(y-y0)/(y0*y0));
                    double sigma12 = cur_sigma2 / 2.56;
                    double sigma22 = cur_sigma2 * 2.56;
                    pK[x] = (DefHistType)(Gaussian2D(r2, sigma12)/sigma12 - Gaussian2D(r2, sigma22)/sigma22);
                    pG[x] = (DefHistType)(Gaussian2D(r2, cur_sigma2/1.6) - Gaussian2D(r2, cur_sigma2*1.6));
                }
            }/* next line */
        }
    }/* ReallocKernel*/
    inline double Gaussian2D(double x, double sigma2) 
    {
        return (exp(-x/(2*sigma2)) / (2*3.1415926535897932384626433832795*sigma2) );
    }
    void calcHist(IplImage* pImg, IplImage* pMask, CvPoint Center, CvMat* pKernel, CvMat* pHist, DefHistType* pHistVolume)
    {
        int         w = pKernel->width;
        int         h = pKernel->height;
        DefHistType Volume = 0;
        int         x0 = Center.x - w/2;
        int         y0 = Center.y - h/2;
        int         x,y;

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

        if(m_Dim == 3)
        {
            for(y=0;y<h;++y)
            {
                unsigned char* pImgData = NULL;
                unsigned char* pMaskData = NULL;
                DefHistType* pKernelData = NULL;
                if((y0+y)>=pImg->height) continue;
                if((y0+y)<0)continue;
                pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
                pMaskData = pMask?(&CV_IMAGE_ELEM(pMask,unsigned char,y+y0,x0)):NULL;
                pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(pKernel[0],y,0,sizeof(DefHistType));
                for(x=0;x<w;++x,pImgData+=3)
                {
                    if((x0+x)>=pImg->width) continue;
                    if((x0+x)<0)continue;
                    if(pMaskData==NULL || pMaskData[x]>128)
                    {
                        DefHistType K = pKernelData[x];
                        int index = HIST_INDEX(pImgData);
                        assert(index >= 0 && index < pHist->cols);
                        Volume += K;
                        ((DefHistType*)(pHist->data.ptr))[index] += K;
                    }/* only masked pixels */
                }/* next column */
            }/* next row */
        }/* if m_Dim == 3 */
        if(pHistVolume)pHistVolume[0] = Volume;
    };/*calcHist*/
    double calcBhattacharyya()
    {
        cvMul(m_HistCandidate,m_HistModel,m_HistTemp);
        cvPow(m_HistTemp,m_HistTemp,0.5);
        return cvSum(m_HistTemp).val[0] / sqrt(m_HistCandidateVolume*m_HistModelVolume);
    } /* calcBhattacharyyaCoefficient */
    void calcWeights(IplImage* pImg, IplImage* pImgFG, CvPoint Center)
    {
        cvZero(m_Weights);
        /* calc new pos */