coord updateCondensation ( CvConDensation* ConDens, coord Measurement, float * stdDX_ptr, float * stdDY_ptr){
coord prediction;
updateProcessProbDens(ConDens, Measurement, stdDX_ptr, stdDY_ptr);
cvConDensUpdateByTime(ConDens);
prediction.set(ConDens->State[0], ConDens->State[1]);
return prediction;
}
bool ParticleFilter::track(Rect& bb, Mat& frame)
{
// An dieser Stelle müssen die posteriors mittels Wahrscheinlichkeitsverteilung gesetzt werden (WIRKLICH??).
// Ich nehme die Gaußsche Verteilung aus dem Buch. Vielleicht ists ja ok.
float* messurements = new float[condensation->MP];
updateConfidence(messurements);
//Und hier werden die Partikel "re-sampled"
cvConDensUpdateByTime( condensation );
return true;
}
Joint Filter::Particle_Filter::process(Joint joint) {
CvPoint mousePosition;
mousePosition.x = (joint.Position.X+1)/2*winWidth; //
mousePosition.y = (joint.Position.Y+1)/2*winHeight;
CvPoint predict_pt=cvPoint((int)condens->State[0],(int)condens->State[1]);
float variance[measureNum]={0};
//get variance/standard deviation of each state
for (int i=0;i<measureNum;i++) {
//sum
float sumState=0;
for (int j=0;j<condens->SamplesNum;j++) {
sumState+=condens->flSamples[i][j];
}
//average
sumState/=sampleNum;
//variance
for (int j=0;j<condens->SamplesNum;j++) {
variance[i]+=(condens->flSamples[i][j]-sumState)*
(condens->flSamples[i][j]-sumState);
}
variance[i]/=sampleNum-1;
}
//3.update particals confidence
CvPoint pt;
if (isPredictOnly) {
pt=predict_pt;
}else{
pt=mousePosition;
}
for (int i=0;i<condens->SamplesNum;i++) {
float probX=(float)exp(-1*(pt.x-condens->flSamples[i][0])
*(pt.x-condens->flSamples[i][0])/(2*variance[0]));
float probY=(float)exp(-1*(pt.y-condens->flSamples[i][1])
*(pt.y-condens->flSamples[i][1])/(2*variance[1]));
condens->flConfidence[i]=probX*probY;
}
//4.update condensation
cvConDensUpdateByTime(condens);
Joint ret(joint);
ret.Position.X = 1.0*predict_pt.x/winWidth*2-1; //1.0
ret.Position.Y = 1.0*predict_pt.y/winHeight*2-1;
return ret;
}
void OpticalFlow::UpdateCondensation(IplImage* /*rgbImage*/,
int prev_indx, int curr_indx)
{
//VERBOSE5(3, "m_condens_state x %f, y %f, vx %f, vy %f, a %f",
// m_condens_state.x, m_condens_state.y, m_condens_state.vx, m_condens_state.vy, m_condens_state.angle);
// for each condensation sample, predict the feature locations,
// compare to the observed KLT tracking, and check the probmask
// at each predicted location. The combination of these yields the
// confidence in this sample's estimate.
int num_ft = (int) m_features[prev_indx].size();
CPointVector predicted;
predicted.resize(num_ft);
CDoubleVector probs_locations;
CDoubleVector probs_colors;
probs_locations.reserve(m_pConDens->SamplesNum);
probs_colors.reserve(m_pConDens->SamplesNum);
double sum_probs_locations = 0.0;
double sum_probs_colors = 0.0;
CDoubleVector old_lens;
CDoubleVector old_d_angles;
// prepare data structures so that prediction based on centroid
// is fast
PreparePredictFeatureLocations(m_condens_state, m_features[prev_indx], old_lens, old_d_angles);
CvPoint2D32f avg_obs, avg_prev;
GetAverage(m_features[curr_indx], avg_prev);
// GetAverage(m_features[prev_indx], avg_prev);
GetAverage(m_features[curr_indx]/*_observation*/, avg_obs);
double dvx = avg_obs.x - avg_prev.x;
double dvy = avg_obs.y - avg_prev.y;
// for each sample
for (int scnt=0; scnt<m_pConDens->SamplesNum; scnt++) {
// hack - todo
if (scnt==m_pConDens->SamplesNum-1) {
m_pConDens->flSamples[scnt][0] = avg_obs.x;
m_pConDens->flSamples[scnt][2] = avg_obs.y;
m_pConDens->flSamples[scnt][1] = (float) dvx;
m_pConDens->flSamples[scnt][3] = (float) dvy;
}
// the Condensation sample's guess:
CondensState sample_state;
sample_state.x = m_pConDens->flSamples[scnt][0];
sample_state.y = m_pConDens->flSamples[scnt][2];
sample_state.vx = m_pConDens->flSamples[scnt][1];
sample_state.vy = m_pConDens->flSamples[scnt][3];
sample_state.angle = 0;//m_pConDens->flSamples[scnt][4];
ASSERT(!isnan(sample_state.x) && !isnan(sample_state.y) && !isnan(sample_state.angle));
double fac = (m_condens_init_rect.right-m_condens_init_rect.left)/3.0;
double dx = avg_obs.x - sample_state.x;
double dy = avg_obs.y - sample_state.y;
double probloc = dx*dx+dy*dy;
probloc = fac/(fac+probloc);
probs_locations.push_back(probloc);
sum_probs_locations += probloc;
#if 0
PredictFeatureLocations(old_lens, old_d_angles, sample_state, predicted);
// probability of predicted feature locations given the KLT observation
int discard_num_distances = (int)(0.15*(double)num_ft);
double probloc = EstimateProbability(predicted, m_features[curr_indx]/*_observation*/, discard_num_distances);
probs_locations.push_back(probloc);
sum_probs_locations += probloc;
// probability of predicted feature locations given the outside probability map (color)
double probcol = EstimateProbability(predicted, rgbImage);
probs_colors.push_back(probcol);
sum_probs_colors += probcol;
#endif
} // end for each sample
// ASSERT(!isnan(sum_probs_locations) && sum_probs_locations>0);
//
// normalize the individual probabilities and set sample confidence
//
int best_sample_indx = -1;
double best_confidence = 0;
for (int scnt=0; scnt<m_pConDens->SamplesNum; scnt++) {
double norm_prob_locations = probs_locations[scnt]/sum_probs_locations;
// double norm_prob_colors = probs_colors[scnt]/sum_probs_colors;
double confidence;
if (sum_probs_colors>0) {
// confidence = norm_prob_locations*norm_prob_colors;
confidence = norm_prob_locations;
} else {
confidence = norm_prob_locations;
}
m_pConDens->flConfidence[scnt] = (float) confidence;
m_sample_confidences[scnt] = confidence;
if (confidence>best_confidence) {
best_confidence = confidence;
best_sample_indx = scnt;
}
}
// for (int scnt=0; scnt<m_pConDens->SamplesNum; scnt++) {
// VERBOSE2(3, "%d: %f ", scnt, m_sample_confidences[scnt]);
// }
ASSERT(best_sample_indx!=-1);
ASSERT(best_sample_indx==m_pConDens->SamplesNum-1);
CondensState best_sample_state;
best_sample_state.x = m_pConDens->flSamples[best_sample_indx][0];
best_sample_state.y = m_pConDens->flSamples[best_sample_indx][2];
best_sample_state.vx = m_pConDens->flSamples[best_sample_indx][1];
best_sample_state.vy = m_pConDens->flSamples[best_sample_indx][3];
best_sample_state.angle = m_pConDens->flSamples[best_sample_indx][4];
//VERBOSE3(3, "sample_state %f, %f, %f",
// sample_state.x, sample_state.y, sample_state.angle);
// VERBOSE4(3, "sample_state %f, %f, %f, %f"),
// sample_state.x, sample_state.y, sample_state.vx, sample_state.vy);
ASSERT(!isnan(best_sample_state.x) && !isnan(best_sample_state.y) && !isnan(best_sample_state.angle));
// probability of predicted feature locations given the KLT observation
m_tmp_predicted.resize(m_features[0].size());
PredictFeatureLocations(old_lens, old_d_angles, best_sample_state, m_tmp_predicted);
//
// do one condensation step, then get the state prediction from Condensation;
//
cvConDensUpdateByTime(m_pConDens);
#if 0
if (false) { // todo
m_condens_state.x = max(0, min(rgbImage->width-1, m_pConDens->State[0]));
m_condens_state.y = max(0, min(rgbImage->height-1, m_pConDens->State[2]));
m_condens_state.vx = m_pConDens->State[1];
m_condens_state.vy = m_pConDens->State[3];
m_condens_state.angle = m_pConDens->State[4];
} else
#endif
{
m_condens_state.x = best_sample_state.x;
m_condens_state.y = best_sample_state.y;
m_condens_state.vx = best_sample_state.vx;
m_condens_state.vy = best_sample_state.vy ;
m_condens_state.angle = best_sample_state.angle;
}
ASSERT(!isnan(m_condens_state.x) && !isnan(m_condens_state.y) && !isnan(m_condens_state.angle));
ASSERT(!isnan(m_condens_state.vx) && !isnan(m_condens_state.vy));
// now move the current features to where Condensation thinks they should be;
// the observation is no longer needed
#if 0
if (false) { // todo
PredictFeatureLocations(old_lens, old_d_angles, m_condens_state, m_tmp_predicted);
FollowObservationForSmallDiffs(m_tmp_predicted, m_features[curr_indx]/*observation*/,
m_features[curr_indx]/*output*/, 2.0);
} else
#endif
{
PredictFeatureLocations(old_lens, old_d_angles, m_condens_state, m_features[curr_indx]);
}
{
// initialize bounds for state
float lower_bound[OF_CONDENS_DIMS];
float upper_bound[OF_CONDENS_DIMS];
// velocity bounds highly depend on the frame rate that we will achieve,
// increase the factor for lower frame rates;
// it states how much the center can move in either direction in a single
// frame, measured in terms of the width or height of the initial match size
double velocity_factor = .25;
CvPoint2D32f avg;
GetAverage(m_features[curr_indx]/*_observation*/, avg);
double cx = avg.x;
double cy = avg.y;
double width = (m_condens_init_rect.right-m_condens_init_rect.left)*velocity_factor;
double height = (m_condens_init_rect.bottom-m_condens_init_rect.top)*velocity_factor;
lower_bound[0] = (float) (cx-width);
upper_bound[0] = (float) (cx+width);
lower_bound[1] = (float) (-width);
upper_bound[1] = (float) (+width);
lower_bound[2] = (float) (cy-height);
upper_bound[2] = (float) (cy+height);
lower_bound[3] = (float) (-height);
upper_bound[3] = (float) (+height);
lower_bound[4] = (float) (-10.0*velocity_factor*M_PI/180.0);
upper_bound[4] = (float) (+10.0*velocity_factor*M_PI/180.0);
CvMat lb = cvMat(OF_CONDENS_DIMS, 1, CV_MAT3x1_32F, lower_bound);
CvMat ub = cvMat(OF_CONDENS_DIMS, 1, CV_MAT3x1_32F, upper_bound);
cvConDensInitSampleSet(m_pConDens, &lb, &ub);
}
}
//
// Transform
// Transform the sample 'in place'
//
HRESULT CCondens::Transform(IMediaSample *pSample)
{
BYTE* pData;
CvImage image;
IplImage* image2; //ianni <======
pSample->GetPointer(&pData);
AM_MEDIA_TYPE* pType = &m_pInput->CurrentMediaType();
VIDEOINFOHEADER *pvi = (VIDEOINFOHEADER *) pType->pbFormat;
// Get the image properties from the BITMAPINFOHEADER
CvSize size = cvSize( pvi->bmiHeader.biWidth, pvi->bmiHeader.biHeight );
int stride = (size.width * 3 + 3) & -4;
image2=cvCreateImage(cvSize(100,100),IPL_DEPTH_8U, 3); //ianni <========
cvReleaseImage(&image2); //ianni <===========
cvInitImageHeader( &image, size, IPL_DEPTH_8U, 3, IPL_ORIGIN_TL, 4, 1 );
cvSetImageData( &image, pData,stride );
if(IsTracking == false)
{
if(IsInit == false)
{
CvPoint p1, p2;
// Draw box
p1.x = cvRound( size.width * m_params.x );
p1.y = cvRound( size.height * m_params.y );
p2.x = cvRound( size.width * (m_params.x + m_params.width));
p2.y = cvRound( size.height * (m_params.y + m_params.height));
CheckBackProject( &image );
cvRectangle( &image, p1, p2, -1, 1 );
}
else
{
m_object.x = cvRound( size.width * m_params.x );
m_object.y = cvRound( size.height * m_params.y );
m_object.width = cvRound( size.width * m_params.width );
m_object.height = cvRound( size.height * m_params.height );
ApplyCamShift( &image, true );
CheckBackProject( &image );
IsTracking = true;
}
}
else
{
cvConDensUpdateByTime(ConDens);
m_object.x = cvRound( ConDens->State[0]-m_object.width*0.5);
m_object.y = cvRound( ConDens->State[2]-m_object.height*0.5 );
ApplyCamShift( &image, false );
CheckBackProject( &image );
cvRectangle( &image,
cvPoint( m_object.x, m_object.y ),
cvPoint( m_object.x + m_object.width, m_object.y + m_object.height ),
-1, 1 );
Rectang(&image,m_Indicat1,-1);
m_X.x = 10;
m_X.y = 10;
m_X.width=50*m_Old.x/size.width;
m_X.height =10;
Rectang(&image,m_X,CV_RGB(0,0,255));
m_Y.x = 10;
m_Y.y = 10;
m_Y.width=10;
m_Y.height = 50*m_Old.y/size.height;
Rectang(&image,m_Y,CV_RGB(255,0,0));
m_Indicat2.x = 0;
m_Indicat2.y = size.height-50;
m_Indicat2.width = 50;
m_Indicat2.height = 50;
Rectang(&image,m_Indicat2,-1);
float Norm = cvSqrt(Measurement[1]*Measurement[1]+Measurement[3]*Measurement[3]);
int VXNorm = (fabs(Measurement[1])>5)?(int)(12*Measurement[1]/Norm):0;
int VYNorm = (fabs(Measurement[3])>5)?(int)(12*Measurement[3]/Norm):0;
CvPoint pp1 = {25,size.height-25};
CvPoint pp2 = {25+VXNorm,size.height-25+VYNorm};
cvLine(&image,pp1,pp2,CV_RGB(0,0,0),3);
}
cvSetImageData( &image, 0, 0 );
return NOERROR;
}
//パーティクルフィルタ
void particleFilter()
{
int i, c;
double w = 0.0, h = 0.0;
cv::VideoCapture capture(0);
//CvCapture *capture = 0;
//capture = cvCreateCameraCapture (0);
int n_stat = 4;
int n_particle = 4000;
CvConDensation *cond = 0;
CvMat *lowerBound = 0;
CvMat *upperBound = 0;
int xx, yy;
capture >> capframe;
//１フレームキャプチャし，キャプチャサイズを取得する．
//frame = cvQueryFrame (capture);
//redimage=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
//greenimage=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
//blueimage=cvCreateImage(cvGetSize(frame),IPL_DEPTH_8U,1);
w = capframe.cols;
h = capframe.rows;
//w = frame->width;
//h = frame->height;
cv::namedWindow("Condensation", CV_WINDOW_AUTOSIZE);
cv::setMouseCallback("Condensation", on_mouse, 0);
//cvNamedWindow ("Condensation", CV_WINDOW_AUTOSIZE);
//cvSetMouseCallback("Condensation",on_mouse,0);
//フォントの設定
//CvFont dfont;
//float hscale = 0.7f;
//float vscale = 0.7f;
//float italicscale = 0.0f;
//int thickness = 1;
//char text[255] = "";
//cvInitFont (&dfont, CV_FONT_HERSHEY_SIMPLEX , hscale, vscale, italicscale, thickness, CV_AA);
//Condensation構造体を作成する．
cond = cvCreateConDensation (n_stat, 0, n_particle);
//状態ベクトル各次元の取りうる最小値・最大値を指定する
//今回は位置(x,y)と速度(xpixcel/frame,ypixcel/frame)の4次元
lowerBound = cvCreateMat (4, 1, CV_32FC1);
upperBound = cvCreateMat (4, 1, CV_32FC1);
cvmSet (lowerBound, 0, 0, 0.0);
cvmSet (lowerBound, 1, 0, 0.0);
cvmSet (lowerBound, 2, 0, -20.0);
cvmSet (lowerBound, 3, 0, -20.0);
cvmSet (upperBound, 0, 0, w);
cvmSet (upperBound, 1, 0, h);
cvmSet (upperBound, 2, 0, 20.0);
cvmSet (upperBound, 3, 0, 20.0);
//Condensation構造体を初期化する
cvConDensInitSampleSet (cond, lowerBound, upperBound);
//ConDensationアルゴリズムにおける状態ベクトルのダイナミクスを指定する
cond->DynamMatr[0] = 1.0;
cond->DynamMatr[1] = 0.0;
cond->DynamMatr[2] = 1.0;
cond->DynamMatr[3] = 0.0;
cond->DynamMatr[4] = 0.0;
cond->DynamMatr[5] = 1.0;
cond->DynamMatr[6] = 0.0;
cond->DynamMatr[7] = 1.0;
cond->DynamMatr[8] = 0.0;
cond->DynamMatr[9] = 0.0;
cond->DynamMatr[10] = 1.0;
cond->DynamMatr[11] = 0.0;
cond->DynamMatr[12] = 0.0;
cond->DynamMatr[13] = 0.0;
cond->DynamMatr[14] = 0.0;
cond->DynamMatr[15] = 1.0;
//ノイズパラメータを再設定する．
cvRandInit (&(cond->RandS[0]), -25, 25, (int) cvGetTickCount ());
cvRandInit (&(cond->RandS[1]), -25, 25, (int) cvGetTickCount ());
cvRandInit (&(cond->RandS[2]), -5, 5, (int) cvGetTickCount ());
cvRandInit (&(cond->RandS[3]), -5, 5, (int) cvGetTickCount ());
while (1)
{
capture >> capframe;
//frame = cvQueryFrame (capture);
//各パーティクルについて尤度を計算する．
for (i = 0; i < n_particle; i++)
{
xx = (int) (cond->flSamples[i][0]);
yy = (int) (cond->flSamples[i][1]);
if (xx < 0 || xx >= w || yy < 0 || yy >= h)
{
cond->flConfidence[i] = 0.0;
}
else
{
cond->flConfidence[i] = calc_likelihood (capframe, xx, yy);
//cond->flConfidence[i] = calc_likelihood (frame, xx, yy);
cv::circle(capframe, cv::Point(xx, yy), 1, CV_RGB(0, 255, 200));
//cvCircle (frame, cvPoint (xx, yy), 1, CV_RGB (0, 255, 200), -1);
}
}
//重みの総和&重心を求める
double wx = 0, wy = 0;
double sumWeight = 0;
for (i = 0; i < n_particle; i++)
{
sumWeight += cond->flConfidence[i];
}
for (i = 0; i < n_particle; i++)
{
wx += (int) (cond->flSamples[i][0]) * (cond->flConfidence[i] / sumWeight);
wy += (int) (cond->flSamples[i][1]) * (cond->flConfidence[i] / sumWeight);
}
//重心表示
cv::circle(capframe, cv::Point((int)wx, (int)wy), 10, cv::Scalar(0, 0, 255));
cv::circle(capframe, cv::Point(20, 20), 10, CV_RGB(red, green, blue), 6);
cv::putText(capframe, "target", cv::Point(0, 50), cv::FONT_HERSHEY_SIMPLEX, 0.7, CV_RGB(red, green, blue));
cv::imshow("Condensation", capframe);
//cvCircle(frame,cvPoint(20,20),10,CV_RGB(red,green,blue),-1);
//cvPutText(frame,"target",cvPoint(0,50),&dfont,CV_RGB(red,green,blue));
//cvShowImage ("Condensation", frame);
c = cv::waitKey(30);
//c = cvWaitKey (30);
if (c == 27) break;
//次のモデルの状態を推定する
cvConDensUpdateByTime (cond);
}
cv::destroyWindow("Condensation");
//cvDestroyWindow ("Condensation");
//cvReleaseCapture (&capture);
//cvReleaseImage(&redimage);
//cvReleaseImage(&greenimage);
//cvReleaseImage(&blueimage);
cvReleaseConDensation (&cond);
cvReleaseMat (&lowerBound);
cvReleaseMat (&upperBound);
}
