blobtrackingmsfg.cpp

Using open CV draw color histogram, convert RGB To HSI

        for(iter=0;iter<m_IterNum;++iter)
        {
            float   newx=0,newy=0,sum=0;
            //int     x,y;
            double  B0;

            //CvPoint Center = cvPoint(cvRound(m_Blob.x),cvRound(m_Blob.y));
            CollectHist(pImg, NULL, &m_Blob, &m_HistCandidate);
            B0 = calcBhattacharyya();

            if(m_Wnd)if(CV_BLOB_ID(pBlobPrev)==0 && iter == 0)
            {/* debug */
                IplImage*   pW = cvCloneImage(pImgFG);
                IplImage*   pWFG = cvCloneImage(pImgFG);
                int         x,y;

                cvZero(pW);
                cvZero(pWFG);

                assert(m_ObjSize.width < pImg->width);
                assert(m_ObjSize.height < pImg->height);
                /*calc shift vector */
                for(y=0;y<pImg->height;++y)
                {
                    unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y,0);
                    unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y,0)):NULL;
                    for(x=0;x<pImg->width;++x,pImgData+=3)
                    {
                        int         xk = cvRound(x-(m_Blob.x-m_Blob.w*0.5));
                        int         yk = cvRound(y-(m_Blob.y-m_Blob.h*0.5));
                        double      HM = 0;
                        double      HC = 0;
                        double      K;
                        int index = HIST_INDEX(pImgData);
                        assert(index >= 0 && index < m_BinNumTotal);

                        if(fabs(x-m_Blob.x)>m_Blob.w*0.6) continue;
                        if(fabs(y-m_Blob.y)>m_Blob.h*0.6) continue;

                        if(xk < 0 || xk >= m_KernelMeanShift->cols) continue;
                        if(yk < 0 || yk >= m_KernelMeanShift->rows) continue;

                        if(m_HistModel.m_HistVolume>0)
                            HM = ((DefHistType*)m_HistModel.m_pHist->data.ptr)[index]/m_HistModel.m_HistVolume;
                        if(m_HistCandidate.m_HistVolume>0)
                            HC = ((DefHistType*)m_HistCandidate.m_pHist->data.ptr)[index]/m_HistCandidate.m_HistVolume;
                        K = *(DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],yk,xk,sizeof(DefHistType));

                        if(HC>0)
                        {
                            double  V = sqrt(HM / HC);
                            int     Vi = cvRound(V * 64);
                            if(Vi < 0) Vi = 0;
                            if(Vi > 255) Vi = 255;
                            CV_IMAGE_ELEM(pW,uchar,y,x) = (uchar)Vi;

                            V += m_FGWeight*(pMaskData?(pMaskData[x]/255.0f):0);
                            V*=K;
                            Vi = cvRound(V * 64);
                            if(Vi < 0) Vi = 0;
                            if(Vi > 255) Vi = 255;
                            CV_IMAGE_ELEM(pWFG,uchar,y,x) = (uchar)Vi;
                        }

                    }/* next column */
                }/* next row */
                //cvNamedWindow("MSFG_W",0);
                //cvShowImage("MSFG_W",pW);
                //cvNamedWindow("MSFG_WFG",0);
                //cvShowImage("MSFG_WFG",pWFG);
                //cvNamedWindow("MSFG_FG",0);
                //cvShowImage("MSFG_FG",pImgFG);

                //cvSaveImage("MSFG_W.bmp",pW);
                //cvSaveImage("MSFG_WFG.bmp",pWFG);
                //cvSaveImage("MSFG_FG.bmp",pImgFG);
            }/* debug */


            /* calc new pos by meanshift */
            /* calc new pos */
            if(m_Dim == 3)
            {
                int         x0 = cvRound(m_Blob.x - m_ObjSize.width*0.5);
                int         y0 = cvRound(m_Blob.y - m_ObjSize.height*0.5);
                int         x,y;

                assert(m_ObjSize.width < pImg->width);
                assert(m_ObjSize.height < pImg->height);

                /* crop blob bounds */
                if((x0+m_ObjSize.width)>=pImg->width) x0=pImg->width-m_ObjSize.width-1;
                if((y0+m_ObjSize.height)>=pImg->height) y0=pImg->height-m_ObjSize.height-1;
                if(x0<0){ x0=0;}
                if(y0<0){ y0=0;}
                /*calc shift vector */
                for(y=0;y<m_ObjSize.height;++y)
                {
                    unsigned char* pImgData = &CV_IMAGE_ELEM(pImg,unsigned char,y+y0,x0*3);
                    unsigned char* pMaskData = pImgFG?(&CV_IMAGE_ELEM(pImgFG,unsigned char,y+y0,x0)):NULL;
                    DefHistType* pKernelData = (DefHistType*)CV_MAT_ELEM_PTR_FAST(m_KernelMeanShift[0],y,0,sizeof(DefHistType));
                    for(x=0;x<m_ObjSize.width;++x,pImgData+=3)
                    {
                        DefHistType K = pKernelData[x];
                        double      HM = 0;
                        double      HC = 0;
                        int index = HIST_INDEX(pImgData);
                        assert(index >= 0 && index < m_BinNumTotal);

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

                        if(HC>0)
                        {
                            double V = sqrt(HM / HC);
                            if(!m_Collision && m_FGWeight>0 && pMaskData)
                            {
                                V += m_FGWeight*(pMaskData[x]/255.0f);
                            }
                            K *= (float)MIN(V,100000.);
                        }

                        sum += K;
                        newx += K*x;
                        newy += K*y;
                    }/* next column */
                }/* next row */

                if(sum > 0)
                {
                    newx /= sum;
                    newy /= sum;
                }
                newx += x0;
                newy += y0;

            }/* if m_Dim == 3 */
            /* calc new pos be meanshift */

            for(;;)
            {/* iterate using bahattcharrya coefficient */
                double  B1;
                CvBlob  B = m_Blob;
//                CvPoint NewCenter = cvPoint(cvRound(newx),cvRound(newy));
                B.x = newx;
                B.y = newy;
                CollectHist(pImg, NULL, &B, &m_HistCandidate);
                B1 = calcBhattacharyya();
                if(B1 > B0) break;
                newx = 0.5f*(newx+m_Blob.x);
                newy = 0.5f*(newy+m_Blob.y);
                if(fabs(newx-m_Blob.x)<0.1 && fabs(newy-m_Blob.y)<0.1) break;
            }/* iterate using bahattcharrya coefficient */

            if(fabs(newx-m_Blob.x)<0.5 && fabs(newy-m_Blob.y)<0.5) break;
            m_Blob.x = newx;
            m_Blob.y = newy;
        }/* next iteration */

        while(!m_Collision && m_FGWeight>0)
        { /* update size if no collision */
            float       Alpha = 0.04f;
            CvBlob      NewBlob;
            double      M00,X,Y,XX,YY;
            CvMoments   m;
            CvRect      r;
            CvMat       mat;

            r.width = cvRound(m_Blob.w*1.5+0.5);
            r.height = cvRound(m_Blob.h*1.5+0.5);
            r.x = cvRound(m_Blob.x - 0.5*r.width);
            r.y = cvRound(m_Blob.y - 0.5*r.height);
            if(r.x < 0) break;
            if(r.y < 0) break;
            if(r.x+r.width >= pImgFG->width) break;
            if(r.y+r.height >= pImgFG->height) break;
            if(r.height < 5 || r.width < 5) break;

            cvMoments( cvGetSubRect(pImgFG,&mat,r), &m, 0 );
            M00 = cvGetSpatialMoment( &m, 0, 0 );
            if(M00 <= 0 ) break;
            X = cvGetSpatialMoment( &m, 1, 0 )/M00;
            Y = cvGetSpatialMoment( &m, 0, 1 )/M00;
            XX = (cvGetSpatialMoment( &m, 2, 0 )/M00) - X*X;
            YY = (cvGetSpatialMoment( &m, 0, 2 )/M00) - Y*Y;
            NewBlob = cvBlob(r.x+(float)X,r.y+(float)Y,(float)(4*sqrt(XX)),(float)(4*sqrt(YY)));

            NewBlob.w = Alpha*NewBlob.w+m_Blob.w*(1-Alpha);
            NewBlob.h = Alpha*NewBlob.h+m_Blob.h*(1-Alpha);

            m_Blob.w = MAX(NewBlob.w,5);
            m_Blob.h = MAX(NewBlob.h,5);
            break;
        }/* update size if no collision */

        return &m_Blob;
    }; /* CvBlobTrackerOneMSFG::Process */
    virtual double GetConfidence(CvBlob* pBlob, IplImage* pImg, IplImage* /*pImgFG*/ = NULL, IplImage* pImgUnusedReg = NULL)
    {
        double  S = 0.2;
        double  B = GetBhattacharyya(pImg, pImgUnusedReg, pBlob, &m_HistTemp);
        return exp((B-1)/(2*S));
    };/*CvBlobTrackerOneMSFG::*/

    virtual void Update(CvBlob* pBlob, IplImage* pImg, IplImage* pImgFG = NULL)
    {   /* update hist */
        UpdateModelHist(pImg, pImgFG, pBlob?pBlob:&m_Blob);
    }/*CvBlobTrackerOneMSFG::*/

    virtual void Release(){delete this;};
    virtual void SetCollision(int CollisionFlag)
    {
        m_Collision = CollisionFlag;
    }
    virtual void SaveState(CvFileStorage* fs)
    {
        cvWriteStruct(fs, "Blob", &m_Blob, "ffffi");
        cvWriteInt(fs,"Collision", m_Collision);
        cvWriteInt(fs,"HistVolume", cvRound(m_HistModel.m_HistVolume));
        cvWrite(fs,"Hist", m_HistModel.m_pHist);
    };
    virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
    {
        CvMat* pM;
        cvReadStructByName(fs, node, "Blob",&m_Blob, "ffffi");
        m_Collision = cvReadIntByName(fs,node,"Collision",m_Collision);
        pM = (CvMat*)cvRead(fs,cvGetFileNodeByName(fs,node,"Hist"));
        if(pM)
        {
            m_HistModel.m_pHist = pM;
            m_HistModel.m_HistVolume = (float)cvSum(pM).val[0];
        }
    };

}; /*CvBlobTrackerOneMSFG*/

#if 0
void CvBlobTrackerOneMSFG::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.5);
                    float dy = (y+y0-pBlob->y)/(pBlob->h*0.5);
                    double r2 = dx*dx+dy*dy;
                    K = GetKernelHist(r2);
                }

                if(pMaskData)
                {
                    K *= pMaskData[x]*0.003921568627450980392156862745098;
                }
                Volume += K;
                ((DefHistType*)(pHist->m_pHist->data.ptr))[index] += K;
            }/* next column */
        }/* next row */
    }/* if m_Dim == 3 */
    pHist->m_HistVolume = Volume;
};/*CollectHist*/
#endif

CvBlobTrackerOne* cvCreateBlobTrackerOneMSFG()
{
    return (CvBlobTrackerOne*) new CvBlobTrackerOneMSFG;
}
CvBlobTracker* cvCreateBlobTrackerMSFG()
{
    return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSFG);
}

/* Create specific tracker without FG usin - just simple mean-shift tracker*/
class CvBlobTrackerOneMS:public CvBlobTrackerOneMSFG
{
public:
    CvBlobTrackerOneMS()
    {
        SetParam("FGWeight",0);
        DelParam("FGWeight");
    };
};

CvBlobTrackerOne* cvCreateBlobTrackerOneMS()
{
    return (CvBlobTrackerOne*) new CvBlobTrackerOneMS;
}
CvBlobTracker* cvCreateBlobTrackerMS()
{
    return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMS);
}

typedef struct DefParticle
{
    CvBlob  blob;
    float   Vx,Vy;
    double  W;
}DefParticle;

class CvBlobTrackerOneMSPF:public CvBlobTrackerOneMS
{
private:
    /* parameters */
    int             m_ParticleNum;
    float           m_UseVel;
    float           m_SizeVar;
    float           m_PosVar;

    CvSize          m_ImgSize;
    CvBlob          m_Blob;
    DefParticle*    m_pParticlesPredicted;
    DefParticle*    m_pParticlesResampled;
    CvRNG           m_RNG;
#ifdef _OPENMP
    int             m_ThreadNum;
    DefHist*        m_HistForParalel;
#endif

public:
    virtual void SaveState(CvFileStorage* fs)
    {
        CvBlobTrackerOneMS::SaveState(fs);
        cvWriteInt(fs,"ParticleNum",m_ParticleNum);
        cvWriteStruct(fs,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd",m_ParticleNum);