www.pudn.com > Particlefilter_code.zip > JetStream.cpp
///////////////////////////////////////////////////////////////////// // // Implementation file of JetStream, // which extracts contour based on particle filter // // XinFan 2003.5.26 //Reference: // P. P¨Śrez, A. Blake, and M. Gangnet. // JetStream: Probabilistic contour extraction with particles. // Proc. Int. Conf. on Computer Vision (ICCV), II:524-531, 2001. ////////////////////////////////////////////////////////////////////// #include#include #include #include "JetStream.h" #include "MemAlloc.h" #include "Utility.h" #include "FileIO.h" //#include "ipl.h" #include "cv.h" #include "macro.h" //#define _WRITE_SAMPLE //#define _WRITE_X_SAMPLE /////////////////////////////////////////////////////////////////////////////////////// // // Constructors and Deconstructors // /////////////////////////////////////////////////////////////////////////////////////// CJetStream::CJetStream() { m_nStepNum = 0; m_nCurStep = 0; m_nDataWidth = 0; m_nDataHeight = 0; m_flStateSeq = NULL; m_flStream = NULL; m_tmpStream = NULL; } CJetStream::CJetStream(int nStateDim, int nSamplesNum, int nStepNum) : CParticle(nStateDim, nSamplesNum) { m_nStepNum = nStepNum; if (nStepNum == 0) return; m_nCurStep = 0; m_nDataWidth = 0; m_nDataHeight = 0; //Allocate buffer for particle stream m_flStream = NULL; m_tmpStream = NULL; AllocMem(m_flStream, nStepNum, nStateDim, nSamplesNum); AllocMem(m_tmpStream, nStepNum, nStateDim, nSamplesNum); m_flStateSeq = NULL; AllocMem(m_flStateSeq, nStateDim, nStepNum); /*m_flStream = (float ***)malloc(nStepNum * sizeof(float**)); m_tmpStream = (float ***)malloc(nStepNum * sizeof(float**)); float **sample_i; for (int i = 0; i < nStepNum; i++) { sample_i = NULL; AllocMem(sample_i, nStateDim, nSamplesNum); memset(*sample_i, 0, nStateDim * nSamplesNum *sizeof(float)); *(m_flStream + i) = sample_i; sample_i = NULL; }*/ } CJetStream::~CJetStream() { if (m_nStepNum != 0) { /* for (int i = 0; i < m_nStepNum; i++) *(m_flStream + i) = FreeMem(*(m_flStream + i)); free(m_flStream); */ m_flStream = FreeMem(m_flStream, m_nStepNum); m_tmpStream = FreeMem(m_tmpStream, m_nStepNum); m_flStateSeq = FreeMem(m_flStateSeq); } } ///////////////////////////////////////////////////////////////////// // Initialization // ///////////////////////////////////////////////////////////////////// void CJetStream::Initialize(const JETPARA* jetInitPara, const CJetImgData* jetImgData, const CJetInit* jetInit) { //Initialize JetStream Parameters //All member variables are valuse //bit-wise copy is OK m_jetPara = *jetInitPara; // //Get the norm of the gradient //if neccessary, operator "=" can be overloaded // m_jetData.CopyOf(jetImgData); CvMat *img = (CvMat *) m_jetData.gradient_norm; m_nDataWidth = img->cols; m_nDataHeight = img->rows; // for (int i = 0; i < jetInit->ptNum; i++) { //Set initial samples InitSample(*(m_flStream + i), jetInit->pt[i], m_jetPara.step_length); //Set initial estimated states as the specified ones //memcpy(*(m_flStateSeq + i), jetInit->pt[i], m_nStDim * sizeof(float)); //SetVec(*(m_flStateSeq + i), jetInit->pt[i], m_nStDim); #ifdef _WRITE_SAMPLE CvMat *sampleMat = cvCreateMatHeader(m_nSamplesNum, m_nStDim, CV_32FC1); cvSetData(sampleMat, *(*(m_flStream + i)), m_nStDim * sizeof(float)); char name[50]; sprintf(name, "inisample-%d.dat", i); WriteMat32F(name, sampleMat); cvReleaseMatHeader(&sampleMat); #endif } //set current samples, weiths, and cumulative prob. SetVec(*m_flSamples, *(m_flStream[i - 1]), m_nStDim * m_nSamplesNum); SetVec(m_flConfidence, (float)1.0 / m_nSamplesNum, m_nSamplesNum); CalCumulative(); //Set prior and proposal probabilites memset(m_flPriorProb, 0, m_nSamplesNum * sizeof(float)); memset(m_flProposalProb, 0, m_nSamplesNum * sizeof(float)); m_nCurStep = jetInit->ptNum - 1; EstState(1); //Set estimated states as initial points } ////////////////////////////////////////////////////////////////////// // // InitSample(float **sampleArr, float *startPt, float d) //Purpose: // Initializing samples with a starting point specified by jetInit // The initial samples are generated as Xi = Xs + U(d/2) // Xi--------Initial samples, sampleArry // Xs--------Starting point, startPt // U(d/2)----Random numbers uniformly distributed between (-d/2, d/2] ////////////////////////////////////////////////////////////////////// void CJetStream::InitSample(float **sampleArr, float *startPt, float d) { //Initializing x(0:1) for (int i = 0; i < m_nSamplesNum; i++) { for (int j = 0; j < m_nStDim; j++) { *(*(sampleArr + i) + j) = *(startPt + j) + (float)(uniform_random() - 0.5) * d; } } } ///////////////////////////////////////////////////////////////////////////// // EvalWeight() // //Purpose: // Evaluate the weights of predicted samples // Currently, the weights is the liklihood ratio //TODO: // Implement weights update with propsal density // differing from prior probability // XinFan 2003.5.28 // ///////////////////////////////////////////////////////////////////////////// void CJetStream::EvalWeight(int nStep) { //Evalating predicted states //if ((nStep m_nCurStep) != 1) if (nStep != m_nCurStep) return; //Current point CvPoint2D32f *x_i_k, *x_im1_k; float **x_im1 = *(m_flStream + nStep - 1); int x, y; //pointer to the gradient norm and orientation at current location float *norm; float *or; //normal of the greadient orientation at current location float or_norm; //angle_xk, current curve orientation //pasia, the angle between angle_xk and the gradient normal float angle_xk, pasai; float poff, pon; for (int k = 0; k < m_nSamplesNum; k++) { //x(i) - x(i-1) x_i_k = (CvPoint2D32f *) (*(m_flSamples + k)); x_im1_k = (CvPoint2D32f *) (*(x_im1 + k)); angle_xk = atan_u(x_i_k->x - x_im1_k->x, x_i_k->y - x_im1_k->y); //the nearest point x = (int)(x_i_k -> x); y = (int)(x_i_k -> y); assert(x < m_nDataWidth && x >= 0); assert(y < m_nDataHeight && y >= 0); norm = (float *)(cvGetPtrAt(m_jetData.gradient_norm, y, x)); //gradient normal or = (float *)(cvGetPtrAt(m_jetData.gradient_orient, y, x)); // *or = (*or >= 0) ? (*or - PI/2) : (PI/2 + *or); or_norm = (*or >= 0) ? (*or - PI/2) : (PI/2 + *or); //Evaluate poff poff = (float)exp( - *norm / m_jetPara.mea_lamda); //Evaluate pon if (*(cvGetPtrAt(m_jetData.corner_mask, y,x))) { pon = (float)(1.0 / PI); } else { // pasai = *or - angle_xk; pasai = or_norm - angle_xk; pasai = fabs(pasai) > PI / 2 ? (PI - fabs(pasai)) : pasai; pon = evaluate_gaussian(sqrt(*norm) * pasai, m_jetPara.mea_sigma); } //likelihood ratio if (poff < 1.0e-2) { m_flConfidence[k] = pon; // m_flConfidence[k] = (float)(1.0 / m_nSamplesNum); } else { m_flConfidence[k] = pon / poff; } m_flConfidence[k] *= m_flPriorProb[k] / m_flProposalProb[k]; } //normalizing weights NormWeights(); CalCumulative(); #ifdef _WRITE_SAMPLE CvMat *sampleMat = cvCreateMatHeader(m_nSamplesNum, 1, CV_32FC1); cvSetData(sampleMat, m_flConfidence, sizeof(float)); char name[50]; sprintf(name, "weights-%d.dat", nStep); WriteMat32F(name, sampleMat); cvReleaseMatHeader(&sampleMat); #endif } ///////////////////////////////////////////////////////////////////////////// // // UpdateByTime() // //Purpose: // Update JetStream Particles as (Ref.[1]): // x_i+1 = x_i + R(sita_i)(x_i - x_i-1) // with q(sita_i) = mu / PI + (1 - v)N(sita_i; 0, d* sigma_sita ^2) // ///////////////////////////////////////////////////////////////////////////// void CJetStream::UpdateByTime(int nStep) { //Predict next step if ((nStep - m_nCurStep) != 1) return; //Samples x_i, x_i-1 float **x_i = *(m_flStream + nStep - 1); float **x_im1 = *(m_flStream + nStep - 2); //predicted sample float **x_ip1 = NULL; //AllocMem(x_ip1, m_nStDim, m_nSamplesNum); //Contour points int x, y; CvPoint2D32f *x_i_k, *x_im1_k, *x_ip1_k; //CvMat *corner; double sita; float d = m_jetPara.step_length; for (int k = 0; k < m_nSamplesNum; k++) { //the kth sample x_i_k = (CvPoint2D32f *) (*(x_i + k)); x_im1_k = (CvPoint2D32f *) (*(x_im1 + k)); x_ip1_k = (CvPoint2D32f *) (*(m_flSamples + k)); x = (int)(x_i_k->x ); y = (int)(x_i_k->y ); assert(x < m_nDataWidth && x > 0); assert(y < m_nDataHeight && y > 0); //whether a corner //corner = (CvMat*) m_jetData.corner_mask; //SAMPLE: if (*(cvGetPtrAt(m_jetData.corner_mask, y,x))) { sita = (uniform_random() - 0.5) * 2 * PI; //Evaluate Proposal Probability m_flProposalProb[k] = (float)(1.0 / PI / 2); } else { sita = gaussian_random() * m_jetPara.dyn_sigma * sqrt(m_jetPara.step_length); //Evaluate Proposal Probability m_flProposalProb[k] = evaluate_gaussian(sita, m_jetPara.dyn_sigma * sqrt(m_jetPara.step_length)); } //predict x_ip1_k->x = x_i_k->x + cos(sita) * (x_i_k->x - x_im1_k->x) - sin(sita) * (x_i_k->y - x_im1_k->y); x_ip1_k->y = x_i_k->y + sin(sita) * (x_i_k->x - x_im1_k->x) + cos(sita) * (x_i_k->y - x_im1_k->y); // if (x_ip1_k->x > m_nDataWidth - 1 || x_ip1_k->y > m_nDataHeight - 1) // goto SAMPLE; x_ip1_k->x = BOUND(x_ip1_k->x, 1, m_nDataWidth - 1); x_ip1_k->y = BOUND(x_ip1_k->y, 1, m_nDataHeight - 1 ); //Evaluate Prior Probability m_flPriorProb[k] = (float)(m_jetPara.dyn_mu / PI / 2) + (1.0 - m_jetPara.dyn_mu) * evaluate_gaussian(sita, m_jetPara.dyn_sigma * sqrt(m_jetPara.step_length)); } #ifdef _WRITE_SAMPLE CvMat *sampleMat = cvCreateMatHeader(m_nSamplesNum, m_nStDim, CV_32FC1); cvSetData(sampleMat, *m_flSamples , m_nStDim * sizeof(float)); char name[50]; sprintf(name, "samples-%d.dat", nStep); WriteMat32F(name, sampleMat); cvReleaseMatHeader(&sampleMat); #endif m_nCurStep++; //Set current sample //SetVec(*m_flSamples, *x_ip1, m_nStDim * m_nSamplesNum); } /////////////////////////////////////////////////////////////////// // // Selection() // //Purpose: // Sample index a(m)(m = 1, ..., M) from m_flConfidence[k] over // {1,...,M}, and Set x_0:i+1(m) = (x_0:i_(a(m)), x_i+1(a(m)) // /////////////////////////////////////////////////////////////////// void CJetStream::Selection(int nStep) { //Selection performed at each step // if ((nStep - m_nCurStep) != 1) if (nStep != m_nCurStep) return; int *index = new int[m_nSamplesNum]; //backup for(int i = 0; i <= nStep - 1; i++) { memcpy(*(*(m_tmpStream + i)), *(*(m_flStream + i)), m_nStDim * m_nSamplesNum * sizeof(float)); } float **tmp_i, **stream_i; //Sample a(m) Resample(index); //a(m) int index_a; //re-set for (int m = 0; m < m_nSamplesNum; m ++) { //a(m) index_a = index[m]; //Update x_0:i for (i = 0; i <= nStep - 1; i++) { tmp_i = *(m_tmpStream + i); stream_i = *(m_flStream + i); memcpy(*(stream_i + m), *(tmp_i + index_a), m_nStDim * sizeof(float)); //SetVec(*(stream_i + m), *(tmp_i + index_a), m_nStDim); } //Update x_i+1 stream_i = *(m_flStream + nStep); memcpy(*(stream_i + m), *(m_flSamples + index_a), m_nStDim * sizeof(float)); } //Update current samples and confidences #ifdef _WRITE_SAMPLE CvMat *sampleMat = cvCreateMatHeader(m_nSamplesNum, m_nStDim, CV_32FC1); cvSetData(sampleMat, *(*(m_flStream + nStep )) , m_nStDim * sizeof(float)); char name[50]; sprintf(name, "Updated-%d.dat", nStep); WriteMat32F(name, sampleMat); cvReleaseMatHeader(&sampleMat); #endif memcpy(*(m_flSamples), *(*(m_flStream + nStep )), m_nStDim * m_nSamplesNum * sizeof(float)); SetVec(m_flConfidence, (float)(1.0 / m_nSamplesNum), m_nSamplesNum); CalCumulative(); delete [] index; //Write the samples projected to X-axis until now //The ith row of smapleXMat is the X-Sample of the ith Step #ifdef _WRITE_X_SAMPLE CvMat *sampleXMat = cvCreateMat(nStep + 1, m_nSamplesNum, CV_32FC1); float *data = NULL; float *srcSample = NULL; for (i = 0; i <= nStep; i++) { data = (float *)cvGetPtrAt(sampleXMat, i); srcSample = *(*(m_flStream + i)); for (m = 0; m < m_nSamplesNum; m++, srcSample += m_nStDim) { *data++ = *srcSample; } } char Xname[50]; //sprintf(Xname, "Samples-X-%d-%S.dat", nStep, GetTime()); sprintf(Xname, "Samples-X-%d.dat", nStep, GetTime()); WriteMat32F(Xname, sampleXMat); cvReleaseMat(&sampleXMat); #endif //An iteration complete //m_nCurStep++; } ///////////////////////////////////////////////////////////////////////////////////// // // EstState() // //Purpose: // Estimate the states as the mean of the particles // ///////////////////////////////////////////////////////////////////////////////////// void CJetStream::EstState(int nStep) { //Here Estimation performed after the selection/resampling if (nStep > m_nCurStep) return; for (int i = 0; i <= nStep; i++) { MeanSample(*(m_flStateSeq + i), *(m_flStream + i)); } memcpy(m_flState, *(m_flStateSeq + nStep), m_nStDim * sizeof(float)); } void * CJetStream::GetState(int nStep) const { //Return current estmated states if (nStep > m_nCurStep) return 0; CvMat *state = cvCreateMat(nStep + 1, m_nStDim, CV_32FC1); float *data = (float *)cvGetPtrAt(state, 0, 0); memcpy(data, *m_flStateSeq, (nStep + 1) * m_nStDim * sizeof(float)); return (void*)state; } ////////////////////////////////////////////////////////////////////////////////////\ // | // | // IMPLEMENTATION for CJetImgData | // | // | ///////////////////////////////////////////////////////////////////////////////////// CJetImgData::CJetImgData() { corner_mask = NULL; gradient_norm = NULL; gradient_orient = NULL; } CJetImgData::~CJetImgData() { if (corner_mask != NULL) cvReleaseMat((CvMat**)&corner_mask); if (gradient_norm != NULL) cvReleaseMat((CvMat**)&gradient_norm); if (gradient_orient != NULL) cvReleaseMat((CvMat**)&gradient_orient); } CJetImgData::CopyOf(const CJetImgData *jetImgData) { if (jetImgData->gradient_norm != NULL) gradient_norm = (void *)cvCloneMat((CvMat*)jetImgData->gradient_norm); if (jetImgData->gradient_orient != NULL) gradient_orient = (void *)cvCloneMat((CvMat*)jetImgData->gradient_orient); if (jetImgData->corner_mask != NULL) corner_mask = (void *)cvCloneMat((CvMat*)jetImgData->corner_mask); } ////////////////////////////////////////////////////////////////////////////////////// // // SetMagOr() //Purpose: // Compute the magnitude and orientation of the gradients //Paras: // dX ---------------- derivative image along x // dY ---------------- derivative image along y ///////////////////////////////////////////////////////////////////////////////////// void CJetImgData::SetMagOr(void * dX, void * dY) { //Access to Original Image if (((IplImage*)dX)->depth != IPL_DEPTH_16S || ((IplImage*)dY)->depth != IPL_DEPTH_16S) return ; short * _dX = (short *)(((IplImage *)dX)->imageData); short * _dY = (short *)(((IplImage *)dY)->imageData); int width = ((IplImage *)dX)->width; int height = ((IplImage *)dX)->height; //Access to allocated norm and or if (gradient_norm != NULL) cvReleaseMat( (CvMat**)&gradient_norm ); gradient_norm = cvCreateMat(height, width, CV_32FC1); if (gradient_orient != NULL) cvReleaseMat( (CvMat**)&gradient_orient ); gradient_orient = cvCreateMat(height, width, CV_32FC1); int step; CvSize size; float *_norm, *_or; cvGetRawData(gradient_norm, (unsigned char**)&_norm, &step, &size); cvGetRawData(gradient_orient , (unsigned char**)&_or, &step, &size); short dx, dy; for (int i = 0; i < width * height; i++) { dx = *(_dX + i); dy = *(_dY + i); //Cal magnitude //can be replaced by abs(dx) + abs(dy) *(_norm+i) = (float)sqrt(dx * dx + dy * dy); //with hadeling dx is close to zero *(_or+i) = (float) atan_u( (double)dx, (double)dy ); } //gradient_norm = (void *) norm; //gradient_orient = (void *)or; #ifdef _DEBUG WriteMat32F("GradNorm.dat", gradient_norm); WriteMat32F("GradOr.dat", gradient_orient); #endif } /////////////////////////////////////////////////////////////////////////////////////// // SetCornerMask() //Purpose: // Set corner pixels as '255' //Paras: // corners ---------------- Detected corners stored in CvPoint2D32F structure // corner_count------------ count of the detected corners // /////////////////////////////////////////////////////////////////////////////////////// void CJetImgData::SetCornerMask(void *corners, int corner_count, void * szImg) { CvPoint2D32f *_corners = (CvPoint2D32f *)corners; CvSize *_szImg = (CvSize *)szImg; CvMat *mask = cvCreateMat(_szImg->height, _szImg->width, CV_8UC1); cvSetZero(mask); // int step; // CvSize size; // unsigned char * pMask; // cvGetRawData(or , (unsigned char**)&_or, &step, &size); int x, y; int xl, yl; int width = _szImg->width; int height = _szImg->height; for (int i = 0; i < corner_count; i++) { x = (int)_corners[i].x; y = (int)_corners[i].y; for (xl = MAX(0,x - mask_bound); xl < MIN(width, x + mask_bound); xl++) for (yl = MAX(0,y - mask_bound); yl < MIN(height, y + mask_bound); yl++) *(cvGetPtrAt(mask, yl, xl)) = (unsigned char)mask_value; // *(cvGetPtrAt(mask, y, x)) = (unsigned char)mask_value; } if (corner_mask != NULL) cvReleaseMat( (CvMat**)corner_mask ); corner_mask = (void *)mask; #ifdef _DEBUG WriteMat("corners.dat", corner_mask); #endif } ////////////////////////////////////////////////////////////////////////////// // NormalizeNorm() // //Purpose: // Normalize norm of the gradient such that the norms fall between 0 and 1 ///////////////////////////////////////////////////////////////////////////// float CJetImgData::NormalizeNorm() { assert(gradient_norm != NULL); CvSize size; float *_norm; int step; cvGetRawData(gradient_norm, (unsigned char**)&_norm, &step, &size); //Find the maximum float max = 0.0; for (int i = 0; i < size.width * size.height; i++) { if (*(_norm + i) > max) max = *(_norm + i); } //the maximum is not zero if (max > 1.0e-3) { for(i = 0; i < size.width * size.height; i++) *(_norm + i) /= max; } return max; } ////////////////////////////////////////////////////////////////////////////////////\ // | // | // IMPLEMENTATION for CJetInit | // | // | ///////////////////////////////////////////////////////////////////////////////////// CJetInit::CJetInit(int nPtNum) { assert(nPtNum <= max_order); //if (nPtNum <= max_order) // return; ptNum = nPtNum; for (int i = 0; i < nPtNum; i++) { pt[i] = (float*) new CvPoint2D32f; } } CJetInit::~CJetInit() { for (int i = 0; i < ptNum; i++) delete pt[i]; } void CJetInit::SetPoints(long *point, int i) { assert(i <= ptNum); CvPoint2D32f * curPt = (CvPoint2D32f *)pt[i]; curPt->x = (float)*point; curPt->y = (float)*(point+1); }