blobtrackingmsfgs.cpp

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

        if(m_Dim == 3)
        {
            int         x0 = Center.x - m_KernelMeanShiftSize.width/2;
            int         y0 = Center.y - m_KernelMeanShiftSize.height/2;
            int         x,y;

            assert(m_Weights->width == m_KernelMeanShiftSize.width);
            assert(m_Weights->height == m_KernelMeanShiftSize.height);

            /*calc shift vector */
            for(y=0;y<m_KernelMeanShiftSize.height;++y)
            {
                unsigned char* pImgData = NULL;
                unsigned char* pMaskData = NULL;
                float* pWData = NULL;
                
                if(y+y0 < 0 || y+y0 >= pImg->height) continue;
                
                pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
                pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
                pWData = (float*)CV_MAT_ELEM_PTR_FAST(m_Weights[0],y,0,sizeof(float));
                
                for(x=0;x<m_KernelMeanShiftSize.width;++x,pImgData+=3)
                {
                    double      V  = 0;
                    double      HM = 0;
                    double      HC = 0;
                    int         index;
                    if(x+x0 < 0 || x+x0 >= pImg->width) continue;
                    
                    index = HIST_INDEX(pImgData);
                    assert(index >= 0 && index < m_BinNumTotal);

                    if(m_HistModelVolume>0)
                        HM = ((DefHistType*)m_HistModel->data.ptr)[index]/m_HistModelVolume;
                    if(m_HistCandidateVolume>0)
                        HC = ((DefHistType*)m_HistCandidate->data.ptr)[index]/m_HistCandidateVolume;

                    V = (HC>0)?sqrt(HM / HC):0;
                    V += m_FGWeight*(pMaskData?((pMaskData[x]/255.0f)):0);
                    pWData[x] = (float)MIN(V,100000);
                }/* next column */
            }/* next row */
        }/* if m_Dim == 3 */
    }/* calcWeights */

public:
    CvBlobTrackerOneMSFGS()
    {
        int i;
        m_FGWeight = 0;
        m_Alpha = 0.0;
        
        /* add several parameters for external use */
        AddParam("FGWeight", &m_FGWeight);
        CommentParam("FGWeight","Weight of FG mask using (0 - mask will not be used for tracking)");
        AddParam("Alpha", &m_Alpha);
        CommentParam("Alpha","Coefficient for model histogramm updating (0 - hist is not upated)");

        m_BinBit=0;
        m_Dim = 0;
        m_HistModel = NULL;
        m_HistCandidate = NULL;
        m_HistTemp = NULL;
        m_KernelHistModel = NULL;
        m_KernelHistCandidate = NULL;
        m_Weights = NULL;
        for(i=0;i<SCALE_NUM;++i)
        {
            m_KernelMeanShiftK[i] = NULL;
            m_KernelMeanShiftG[i] = NULL;
        }
        ReAllocHist(3,5);   /* 3D hist, each dim has 2^5 bins*/
    }
    ~CvBlobTrackerOneMSFGS()
    {
        int i;
        if(m_HistModel) cvReleaseMat(&m_HistModel);
        if(m_HistCandidate) cvReleaseMat(&m_HistCandidate);
        if(m_HistTemp) cvReleaseMat(&m_HistTemp);
        if(m_KernelHistModel) cvReleaseMat(&m_KernelHistModel);
        for(i=0;i<SCALE_NUM;++i)
        {
            if(m_KernelMeanShiftK[i]) cvReleaseMat(&m_KernelMeanShiftK[i]);
            if(m_KernelMeanShiftG[i]) cvReleaseMat(&m_KernelMeanShiftG[i]);
        }
    }
    /* 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<3)w=3;
        if(h<3)h=3;
        if(w>pImg->width)w=pImg->width;
        if(h>pImg->height)h=pImg->height;
        ReAllocKernel(w,h);
        calcHist(pImg, pImgFG, cvPointFrom32f(CV_BLOB_CENTER(pBlobInit)), m_KernelHistModel, m_HistModel, &m_HistModelVolume);
        m_Blob = pBlobInit[0];
    };
    virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int     iter;

        if(pBlobPrev)
        {
            m_Blob = pBlobPrev[0];
        }
        for(iter=0;iter<10;++iter)
        {
//            float   newx=0,newy=0,sum=0;
            float   dx=0,dy=0,sum=0;
            int     x,y,si;
            CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
            CvSize  Size = cvSize(cvRound(m_Blob.w),cvRound(m_Blob.h));

            if(m_ObjSize.width != Size.width || m_ObjSize.height != Size.height)
            {   /* realloc kernels */
                ReAllocKernel(Size.width,Size.height);
            }   /* realloc kernels */

            /* mean shift in coordinate space */
            calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
            calcWeights(pImg, pImgFG, Center);
            for(si=1;si<(SCALE_NUM-1);++si)
            {
                CvMat*  pKernel = m_KernelMeanShiftK[si];
                float   sdx = 0, sdy=0, ssum=0;
                int     s = si-SCALE_RANGE;
                float   factor = (1.0f-( float(s)/float(SCALE_RANGE) )*( float(s)/float(SCALE_RANGE) ));

                for(y=0;y<m_KernelMeanShiftSize.height;++y)
                for(x=0;x<m_KernelMeanShiftSize.width;++x)
                {
                    float W = *(float*)CV_MAT_ELEM_PTR_FAST(m_Weights[0],y,x,sizeof(float));
                    float K = *(float*)CV_MAT_ELEM_PTR_FAST(pKernel[0],y,x,sizeof(float));
                    float KW = K*W;
                    ssum += (float)fabs(KW);
                    sdx += KW*(x-m_KernelMeanShiftSize.width*0.5f);
                    sdy += KW*(y-m_KernelMeanShiftSize.height*0.5f);
                }/* next pixel */
                dx += sdx * factor;
                dy += sdy * factor;
                sum  += ssum * factor;
            }/* next scale */
            if(sum > 0)
            {
                dx /= sum;
                dy /= sum;
            }

            m_Blob.x += dx;
            m_Blob.y += dy;

            {   /* mean shift in scale space */
                float   news = 0;
                float   sum = 0;
                float   scale;
                
                Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
                calcHist(pImg, NULL, Center, m_KernelHistCandidate, m_HistCandidate, &m_HistCandidateVolume);
                calcWeights(pImg, pImgFG, Center);
                //cvSet(m_Weights,cvScalar(1));
                for(si=0; si<SCALE_NUM; si++) 
                {
                    double  W = cvDotProduct(m_Weights, m_KernelMeanShiftG[si]);;   
                    int     s = si-SCALE_RANGE;
                    sum += (float)fabs(W);
                    news += (float)(s*W);
                }
                if(sum>0)
                {
                    news /= sum;
                }
                scale = (float)pow((double)SCALE_BASE,(double)news);
                m_Blob.w *= scale;
                m_Blob.h *= scale;
            }   /* mean shift in scale space */
            
            /* check fo finish */
            if(fabs(dx)<0.1 && fabs(dy)<0.1) break;
        }/* next iteration */

        if(m_Alpha>0)
        {/* update hist */
            double  Vol, WM, WC;
            CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
            calcHist(pImg, pImgFG, Center, m_KernelHistModel, m_HistCandidate, &m_HistCandidateVolume);
            Vol = 0.5*(m_HistModelVolume + m_HistCandidateVolume);
            WM = Vol*(1-m_Alpha)/m_HistModelVolume;
            WC = Vol*(m_Alpha)/m_HistCandidateVolume;
            cvAddWeighted(m_HistModel, WM, m_HistCandidate,WC,0,m_HistModel);
            m_HistModelVolume = (float)cvSum(m_HistModel).val[0];
        }/* update hist */

        return &m_Blob;
    }; /* Process */
    virtual void Release(){delete this;};
}; /*CvBlobTrackerOneMSFGS*/

CvBlobTrackerOne* cvCreateBlobTrackerOneMSFGS()
{
    return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFGS;
}
CvBlobTracker* cvCreateBlobTrackerMSFGS()
{
    return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFGS);
}