www.pudn.com > Particlefilter_code.zip > ProbContour.cpp
//////////////////////////////////////////////////////////////////////////////////////
//
// Implementation file of Probabilistic Contour Extraction Approach
//
// XinFan 2003.5.26
//
//Reference:
// [1] 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 "ProbContour.h"
#include "cv.h"
#include "FileIO.h"
#include "utility.h"
#define _WRITE_RESULT
/////////////////////////////////////////////////////////////////////////////////////
//
// Implementation Code for JetStream
//
//////////////////////////////////////////////////////////////////////////////////////
//
//An Convenient Routine for Set Jetstream Parameters
// Currently, the parameters are set as Ref.[1]
//
void cvrJetInitPara(JETPARA *jetPara, const CJetImgData *jetData)
{
jetPara->dyn_mu = (float)0.05;
jetPara->dyn_sigma = (float)0.1;
//TODO:
// Replace as the Mean of Gradient Norm
// XinFan 2003.5.26
//COMPLETED: XinFan 2003.5.27
if (jetData == 0) //Ad hoc specify
{
jetPara->mea_lamda = 1.0;
}
else//Mean of image gradients
{
//(CvMat *)imgNorm = (CvMat *)jetData->GetNorm();
CvScalar mean;
mean = cvAvg(jetData->GetNorm());
jetPara->mea_lamda = (float)(mean.val[0]);
}
jetPara->mea_sigma = 1.0;
jetPara->step_length = 1.0;
}
//
//Compute gradient norm and angle
//
void cvrJetInitData(const void * src, CJetImgData *jetData)
{
const int max_cornercount = 30;
const double quanlity_level = 0.01;
const double min_distance = 10;
IplImage *pSrc = (IplImage *)src;
//Construct derivative
CvSize szImg;
szImg.width = pSrc->width;
szImg.height = pSrc->height;
////////////////////////////////////////////////////////////////////////\
// |
// Image Gradients |
// |
/////////////////////////////////////////////////////////////////////////
IplImage* dX = cvCreateImage(szImg, IPL_DEPTH_16S, pSrc->nChannels);
IplImage* dY = cvCreateImage(szImg, IPL_DEPTH_16S, pSrc->nChannels);
memset( dX->imageData, 0, dX->imageSize );
memset( dY->imageData, 0, dY->imageSize );
//smooth and difference
cvSobel(pSrc, dX, 1, 0, 3);
cvSobel(pSrc, dY, 0, 1, 3);
//Calculate the norm & or
jetData->SetMagOr(dX, dY);
cvReleaseImage(&dX);
cvReleaseImage(&dY);
////////////////////////////////////////////////////////////////////////\
// |
// Corner Detection |
// |
/////////////////////////////////////////////////////////////////////////
int cornercount = max_cornercount;
CvPoint2D32f *corners = new CvPoint2D32f[cornercount];
//CvPoint *corners = new CvPoint[cornercount];
IplImage *eigImage = cvCreateImage(szImg, IPL_DEPTH_32F, 1);
IplImage *tmpImage = cvCreateImage(szImg, IPL_DEPTH_32F, 1);
// CvMat *cornerMask = cvCreateMat(szImg.height, szImg.width, CV_8U);
//detecting strong corners
cvGoodFeaturesToTrack(pSrc, eigImage, tmpImage, corners, &cornercount,
quanlity_level, min_distance);
jetData->SetCornerMask(corners, cornercount, &szImg);
cvReleaseImage(&eigImage);
cvReleaseImage(&tmpImage);
delete [] corners;
}
//
//A Generic Particle Predict-Update iteration
//
void cvrParticleIteration(CParticle *cvrParticle, int nStep)
{
//Predict...
cvrParticle->UpdateByTime(nStep);
//Update weights
cvrParticle->EvalWeight(nStep);
//if resampling is performed at each step
//cvrParticle->Resample(nStep);
//if Estimation performed
//cvrParticle->EstState();
}
//
//Stream Iteration process
//
void cvrStreamIter(CJetStream *cvrStream, int nStartStep, int nEndStep)
{
for (int i = nStartStep; i < nEndStep; i ++)
{
cvrParticleIteration(cvrStream, i);
//Act as resampling, but all the previous states will be updated
//Need to discuss more...
cvrStream->Selection(i);
cvrStream->EstState(i);
}
}
///////////////////////////////////////////////////////////////////////////////////////
//
//
// Drawing routines
//
//
///////////////////////////////////////////////////////////////////////////////////////
//
//Draw estmitated contour to image
//
void cvrProbContour(void *srcImg, const CParticle *cvParticle, int nStep)
{
const contour_color = 255;
//source img
//IplImage *img = (IplImage *)srcImg;
//contour with
// control_point_count rows
// StateDim cols(here 2)
CvMat *contour = 0;
contour = (CvMat*) cvParticle->GetState(nStep);
if (contour->cols != 2)
return;
//
//Draw contour
//
CvPoint2D32f * ctrlPt;
CvPoint ptStart, ptEnd;
/***************************************************\
//For testing line_pt
int *line_x = NULL;
int *line_y = NULL;
int line_num = 0;
unsigned char line_color = 255;
\****************************************************/
for (int i = 0; i < contour->rows - 1; i ++)
{
// assert(x < m_nDataWidth);
// assert(y < m_nDataHeight);
ctrlPt = (CvPoint2D32f *)(cvGetPtrAt(contour, i));
//I think there must be some simple way to convert
//CvPoint2D32f to CvPoint
//XinFan 2003.5.28
ptStart.x = (int)ctrlPt->x; ptStart.y = (int)ctrlPt->y;
DrawCross(srcImg, (int *)&ptStart, 3);
ctrlPt = (CvPoint2D32f *)(cvGetPtrAt(contour, i + 1));
ptEnd.x = (int)ctrlPt->x; ptEnd.y = (int)ctrlPt->y;
DrawCross(srcImg, (int *)&ptEnd, 3);
/**********************************************************************************\
//For testing line_pt
line_pt(ptStart.x, ptStart.y, ptEnd.x, ptEnd.y, line_x, line_y, line_num);
for (int j = 0; j < line_num; j++)
iplPutPixel((IplImage *)srcImg, line_x[j], line_y[j], &line_color);
delete line_x; line_x = NULL;
delete line_y; line_y = NULL;
line_num = 0;
\*********************************************************************************/
cvLine(srcImg, ptStart, ptEnd, contour_color);
}
cvReleaseMat(&contour);
}
//
//Draw Face Contour
//
void cvrDrawFaceContour(void *srcImg,
const CParticle *cvParticle,
int nStep,
const std::vector &ptInfo,
const CString &szPathName)
{
const contour_color = 255;
CvMat *contour = 0;
contour = (CvMat*) cvParticle->GetState(nStep);
//Set Filename of the Result data, which can be read by MATLAB function
// -- ReadMat
if (!szPathName.IsEmpty())
{
WriteMat32F(RemoveExt(szPathName) + GetTime() + ".dat", contour);
}
if (contour->cols != 2)
return;
//
//Draw contour
//
CvPoint2D32f * ctrlPt;
CvPoint ptStart, ptEnd;
CAAMShape::CAAMPointInfo curInfo;
int nextInd;
for (int i = 0; i < contour->rows - 1; i ++)
{
//Current point info
curInfo = ptInfo[i];
ctrlPt = (CvPoint2D32f *)(cvGetPtrAt(contour, i));
//I think there must be some simple way to convert
//CvPoint2D32f to CvPoint
//XinFan 2003.5.28
ptStart.x = (int)ctrlPt->x; ptStart.y = (int)ctrlPt->y;
// DrawCross(srcImg, (int *)&ptStart, 3);
//next point
nextInd = curInfo.m_iConnectTo;
ctrlPt = (CvPoint2D32f *)(cvGetPtrAt(contour, nextInd));
ptEnd.x = (int)ctrlPt->x; ptEnd.y = (int)ctrlPt->y;
// DrawCross(srcImg, (int *)&ptEnd, 3);
cvLine(srcImg, ptStart, ptEnd, contour_color);
}
cvReleaseMat(&contour);
}
void DrawAAMShape(void *srcImg, const CAAMShape &shape)
{
ASSERT(shape.NPoints() > 47);
std::vector ptInfo =
shape.PointAux();
int nextInd;
double x, y;
CvPoint ptStart, ptEnd;
CAAMShape::CAAMPointInfo curInfo;
for (int i = 0; i < 46; i++)
{
shape.GetPoint(i, x, y);
ptStart.x = (int) x; ptStart.y = (int)y;
nextInd = ptInfo[i].m_iConnectTo;
shape.GetPoint(nextInd, x, y);
ptEnd.x = (int)x; ptEnd.y = (int) y;
cvLine(srcImg, ptStart, ptEnd, 255);
}
}
//
//Draw crossing point on an image at a specific location
//
void DrawCross(void *srcImg, int *point, int cross_width)
{
CvPoint *pt = (CvPoint *)point;
CvPoint ptStart, ptEnd;
ptStart = *pt; ptEnd = *pt;
ptStart.x = pt->x - cross_width; ptEnd.x = pt->x + cross_width;
cvLine(srcImg, ptStart, ptEnd, 0);
ptStart = *pt; ptEnd = *pt;
ptStart.y = pt->y - cross_width; ptEnd.y = pt->y + cross_width;
cvLine(srcImg, ptStart, ptEnd,0);
}
void DrawCross(void *srcImg, long *point, int cross_width )
{
CvPoint pt;
pt.x = (int)*(point); pt.y = (int)*(point + 1);
CvPoint ptStart, ptEnd;
ptStart = pt; ptEnd = pt;
ptStart.x = pt.x - cross_width; ptEnd.x = pt.x + cross_width;
cvLine(srcImg, ptStart, ptEnd, 0);
ptStart = pt; ptEnd = pt;
ptStart.y = pt.y - cross_width; ptEnd.y = pt.y + cross_width;
cvLine(srcImg, ptStart, ptEnd, 0);
}
void DrawCross(void *srcImg, float *point, int cross_width)
{
CvPoint pt;
pt.x = (int)*(point); pt.y = (int)*(point + 1);
CvPoint ptStart, ptEnd;
ptStart = pt; ptEnd = pt;
ptStart.x = pt.x - cross_width; ptEnd.x = pt.x + cross_width;
cvLine(srcImg, ptStart, ptEnd, 0);
ptStart = pt; ptEnd = pt;
ptStart.y = pt.y - cross_width; ptEnd.y = pt.y + cross_width;
cvLine(srcImg, ptStart, ptEnd, 0);
}
//
//Draw a dark point on an image at a specific location to highlight a specific point,
// default radius = 3
//
void DrawCircle(void *srcImg, int *point, int radius)
{
CvPoint *pCenter = (CvPoint *)point;
cvCircle(srcImg, *pCenter, radius,0, -1);
}