blobtrackingmsfg.cpp

Using open CV draw color histogram, convert RGB To HSI

        cvWriteStruct(fs,"ParticlesResampled",m_pParticlesResampled,"ffffiffd",m_ParticleNum);
    };
    virtual void LoadState(CvFileStorage* fs, CvFileNode* node)
    {
        //CvMat* pM;
        CvBlobTrackerOneMS::LoadState(fs,node);
        m_ParticleNum = cvReadIntByName(fs,node,"ParticleNum",m_ParticleNum);
        if(m_ParticleNum>0)
        {
            Realloc();
            printf("sizeof(DefParticle) is %d\n", sizeof(DefParticle));
            cvReadStructByName(fs,node,"ParticlesPredicted",m_pParticlesPredicted,"ffffiffd");
            cvReadStructByName(fs,node,"ParticlesResampled",m_pParticlesResampled,"ffffiffd");
        }
    };
    CvBlobTrackerOneMSPF()
    {
        m_pParticlesPredicted = NULL;
        m_pParticlesResampled = NULL;
        m_ParticleNum = 200;

        AddParam("ParticleNum",&m_ParticleNum);
        CommentParam("ParticleNum","Number of particles");
        Realloc();

        m_UseVel = 0;
        AddParam("UseVel",&m_UseVel);
        CommentParam("UseVel","Percent of particles which use velocity feature");

        m_SizeVar = 0.05f;
        AddParam("SizeVar",&m_SizeVar);
        CommentParam("SizeVar","Size variation (in object size)");

        m_PosVar = 0.2f;
        AddParam("PosVar",&m_PosVar);
        CommentParam("PosVar","Position variation (in object size)");

        m_RNG = cvRNG(0);
#ifdef _OPENMP
        {
            m_ThreadNum = omp_get_num_procs();
            m_HistForParalel = new DefHist[m_ThreadNum];
        }
#endif
    }
    ~CvBlobTrackerOneMSPF()
    {
        if(m_pParticlesResampled)cvFree(&m_pParticlesResampled);
        if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
#ifdef _OPENMP
        if(m_HistForParalel) delete[] m_HistForParalel;
#endif
    }
private:
    void Realloc()
    {
        if(m_pParticlesResampled)cvFree(&m_pParticlesResampled);
        if(m_pParticlesPredicted)cvFree(&m_pParticlesPredicted);
        m_pParticlesPredicted = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
        m_pParticlesResampled = (DefParticle*)cvAlloc(sizeof(DefParticle)*m_ParticleNum);
    };/* Realloc*/
    void DrawDebug(IplImage* pImg, IplImage* /*pImgFG*/)
    {
        int k;
        for(k=0;k<2;++k)
        {
            DefParticle*    pBP = k?m_pParticlesResampled:m_pParticlesPredicted;
            //const char*           name = k?"MSPF resampled particle":"MSPF Predicted particle";
            IplImage*       pI = cvCloneImage(pImg);
            int             h,hN = m_ParticleNum;
            CvBlob          C = cvBlob(0,0,0,0);
            double          WS = 0;
            for(h=0;h<hN;++h)
            {
                CvBlob  B = pBP[h].blob;
                int     CW = cvRound(255*pBP[h].W);
                CvBlob* pB = &B;
                int x = cvRound(CV_BLOB_RX(pB)), y = cvRound(CV_BLOB_RY(pB));
                CvSize  s = cvSize(MAX(1,x), MAX(1,y));
                double  W = pBP[h].W;
                C.x += pB->x;
                C.y += pB->y;
                C.w += pB->w;
                C.h += pB->h;
                WS+=W;

                s = cvSize(1,1);
                cvEllipse( pI,
                    cvPointFrom32f(CV_BLOB_CENTER(pB)),
                    s,
                    0, 0, 360,
                    CV_RGB(CW,0,0), 1 );
            }/* next hypothetis */
            C.x /= hN;
            C.y /= hN;
            C.w /= hN;
            C.h /= hN;
            cvEllipse( pI,
                cvPointFrom32f(CV_BLOB_CENTER(&C)),
                cvSize(cvRound(C.w*0.5),cvRound(C.h*0.5)),
                0, 0, 360,
                CV_RGB(0,0,255), 1 );
            cvEllipse( pI,
                cvPointFrom32f(CV_BLOB_CENTER(&m_Blob)),
                cvSize(cvRound(m_Blob.w*0.5),cvRound(m_Blob.h*0.5)),
                0, 0, 360,
                CV_RGB(0,255,0), 1 );
            //cvNamedWindow(name,0);
            //cvShowImage(name,pI);
            cvReleaseImage(&pI);
        }/*  */

        //printf("Blob %d, point (%.1f,%.1f) size (%.1f,%.1f)\n",m_Blob.ID,m_Blob.x,m_Blob.y,m_Blob.w,m_Blob.h);
    }/* ::DrawDebug */
private:
    void Prediction()
    {
        int p;
        for(p=0;p<m_ParticleNum;++p)
        {/* "prediction" of particle */
            //double  t;
            float   r[5];
            CvMat   rm = cvMat(1,5,CV_32F,r);
            cvRandArr(&m_RNG,&rm,CV_RAND_NORMAL,cvScalar(0),cvScalar(1));

            m_pParticlesPredicted[p] = m_pParticlesResampled[p];

            if(cvRandReal(&m_RNG)<0.5)
            {/* half of particles will predict based on external blob */
                m_pParticlesPredicted[p].blob = m_Blob;
            }

            if(cvRandReal(&m_RNG)<m_UseVel)
            {/* predict moving particle by usual way by using speed */
                m_pParticlesPredicted[p].blob.x += m_pParticlesPredicted[p].Vx;
                m_pParticlesPredicted[p].blob.y += m_pParticlesPredicted[p].Vy;
            }
            else
            {/* stop several particles */
                m_pParticlesPredicted[p].Vx = 0;
                m_pParticlesPredicted[p].Vy = 0;
            }

            {/* update position */
                float S = (m_Blob.w + m_Blob.h)*0.5f;
                m_pParticlesPredicted[p].blob.x += m_PosVar*S*r[0];
                m_pParticlesPredicted[p].blob.y += m_PosVar*S*r[1];
                /* update velocity */
                m_pParticlesPredicted[p].Vx += (float)(m_PosVar*S*0.1*r[3]);
                m_pParticlesPredicted[p].Vy += (float)(m_PosVar*S*0.1*r[4]);
            }

            /* update size */
            m_pParticlesPredicted[p].blob.w *= (1+m_SizeVar*r[2]);
            m_pParticlesPredicted[p].blob.h *= (1+m_SizeVar*r[2]);

            /* truncate size of particle */
            if(m_pParticlesPredicted[p].blob.w > m_ImgSize.width*0.5f)
            {
                m_pParticlesPredicted[p].blob.w = m_ImgSize.width*0.5f;
            }
            if(m_pParticlesPredicted[p].blob.h > m_ImgSize.height*0.5f)
            {
                m_pParticlesPredicted[p].blob.h = m_ImgSize.height*0.5f;
            }
            if(m_pParticlesPredicted[p].blob.w < 1 )
            {
                m_pParticlesPredicted[p].blob.w = 1;
            }
            if(m_pParticlesPredicted[p].blob.h < 1)
            {
                m_pParticlesPredicted[p].blob.h = 1;
            }
        }/* "prediction" of particle */
    }/* Prediction */

    void UpdateWeightsMS(IplImage* pImg, IplImage* /*pImgFG*/)
    {
        int p;
#ifdef _OPENMP
        if( m_HistForParalel[0].m_pHist==NULL || m_HistForParalel[0].m_pHist->cols != m_BinNumTotal)
        {
            int t;
            for(t=0;t<m_ThreadNum;++t)
                m_HistForParalel[t].Resize(m_BinNumTotal);
        }
#endif

#ifdef _OPENMP
#pragma omp parallel for num_threads(m_ThreadNum),schedule(runtime)
#endif
        for(p=0;p<m_ParticleNum;++p)
        {/* calc weights for particles */
            double  S = 0.2;
            double  B = 0;
#ifdef _OPENMP
            assert(omp_get_thread_num()<m_ThreadNum);
#endif

            B = GetBhattacharyya(
                pImg, NULL,
                &(m_pParticlesPredicted[p].blob)
#ifdef _OPENMP
                ,&(m_HistForParalel[omp_get_thread_num()])
#endif
                );
            m_pParticlesPredicted[p].W *= exp((B-1)/(2*S));
        }/* calc weights for particles */
    }
    void UpdateWeightsCC(IplImage* /*pImg*/, IplImage* /*pImgFG*/)
    {
        int p;
#ifdef _OPENMP
#pragma omp parallel for
#endif
        for(p=0;p<m_ParticleNum;++p)
        {/* calc weights for particles */
            double W = 1;
            m_pParticlesPredicted[p].W *= W;
        }/* calc weights for particles */
    }
    void Resample()
    {/* resample particle */
        int         p;
        double      Sum = 0;
        for(p=0;p<m_ParticleNum;++p)
        {
            Sum += m_pParticlesPredicted[p].W;
        }
        for(p=0;p<m_ParticleNum;++p)
        {
            double  T = Sum * cvRandReal(&m_RNG); /* set current random threshold for cululative weight */
            int     p2;
            double  Sum2 = 0;
            for(p2=0;p2<m_ParticleNum;++p2)
            {
                Sum2 += m_pParticlesPredicted[p2].W;
                if(Sum2 >= T)break;
            }
            if(p2>=m_ParticleNum)p2=m_ParticleNum-1;
            m_pParticlesResampled[p] = m_pParticlesPredicted[p2];
            m_pParticlesResampled[p].W = 1;
        }/* find next particle */
    } /* resample particle */


public:
    virtual void Init(CvBlob* pBlobInit, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int i;
        CvBlobTrackerOneMSFG::Init(pBlobInit, pImg, pImgFG);
        DefParticle PP;
        PP.W = 1;
        PP.Vx = 0;
        PP.Vy = 0;
        PP.blob = pBlobInit[0];
        for(i=0;i<m_ParticleNum;++i)
        {
            m_pParticlesPredicted[i] = PP;
            m_pParticlesResampled[i] = PP;
        }
        m_Blob = pBlobInit[0];
    } /* CvBlobTrackerOneMSPF::Init*/
    virtual CvBlob* Process(CvBlob* pBlobPrev, IplImage* pImg, IplImage* pImgFG = NULL)
    {
        int p;

        m_ImgSize.width = pImg->width;
        m_ImgSize.height = pImg->height;


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

        Prediction();

#ifdef REPORT_TICKS
        int64 ticks = cvGetTickCount();
#endif

        UpdateWeightsMS(pImg, pImgFG);

#ifdef REPORT_TICKS
        ticks = cvGetTickCount() - ticks;
        fprintf(stderr, "PF UpdateWeights, %d ticks\n",  (int)ticks);
        ticks = cvGetTickCount();
#endif

        Resample();

#ifdef REPORT_TICKS
        ticks = cvGetTickCount() - ticks;
        fprintf(stderr, "PF Resampling, %d ticks\n",  (int)ticks);
#endif

        {/* find average result */
            float   x = 0;
            float   y = 0;
            float   w = 0;
            float   h = 0;
            float   Sum = 0;
            DefParticle* pP = m_pParticlesResampled;
            for(p=0;p<m_ParticleNum;++p)
            {
                float W = (float)pP[p].W;
                x += W*pP[p].blob.x;
                y += W*pP[p].blob.y;
                w += W*pP[p].blob.w;
                h += W*pP[p].blob.h;
                Sum += W;
            }
            if(Sum>0)
            {
                m_Blob.x = x / Sum;
                m_Blob.y = y / Sum;
                m_Blob.w = w / Sum;
                m_Blob.h = h / Sum;
            }
        }/* find average result */

        if(m_Wnd)
        {
            DrawDebug(pImg, pImgFG);
        }

        return &m_Blob;
    }/* CvBlobTrackerOneMSPF::Process */
    virtual void SkipProcess(CvBlob* pBlob, IplImage* /*pImg*/, IplImage* /*pImgFG*/ = NULL)
    {
        int p;
        for(p=0;p<m_ParticleNum;++p)
        {
            m_pParticlesResampled[p].blob = pBlob[0];
            m_pParticlesResampled[p].Vx = 0;
            m_pParticlesResampled[p].Vy = 0;
            m_pParticlesResampled[p].W = 1;
        }
    }

    virtual void Release(){delete this;};
    virtual void ParamUpdate()
    {
        Realloc();
    }
}; /* CvBlobTrackerOneMSPF */

CvBlobTrackerOne* cvCreateBlobTrackerOneMSPF()
{
    return (CvBlobTrackerOne*) new CvBlobTrackerOneMSPF;
}
CvBlobTracker* cvCreateBlobTrackerMSPF()
{
    return cvCreateBlobTrackerList(cvCreateBlobTrackerOneMSPF);
}