Xiaochun Cao and Hassan Foroosh
School of Computer Science, University of Central Florida
Classical techniques for camera calibration [1, 2, 3] require a so called calibration rig, with a set of correspondences between known points in the 3D space and their projections in the 2D image plane. Recent techniques propose more flexible plane-based calibration approaches [4, 5].
// count the number of detections in measurement process
int count_detections (void)
{
// set cases of measurement results
double mtype[4];
mtype [0] = 0.0;
mtype [1] = 0.5;
mtype [2] = mtype[1] + 0.3;
mtype [3] = mtype[2] + 0.2;
// cout << "check measurement type3 = " << mtype[3] << endl; // just to check
// select a measurement case
double mrn = uniform_random();
int type = 1;
for ( int k = 0; k < 3; k++ )
{
if ( mrn < mtype[k] )
{
type = k;
break;
}
}
return type;
}
// distance between measurement and prediction
double distance(CvMat* measurement, CvMat* prediction)
{
double distance2 = 0;
double differance = 0;
for (int u = 0; u < 2; u++)
{
differance = cvmGet(measurement,u,0) - cvmGet(prediction,u,0);
distance2 += differance * differance;
}
return sqrt(distance2);
}
// likelihood based on multivariative normal distribution (ref. 15p eqn(2.4))
double likelihood(CvMat *mean, CvMat *covariance, CvMat *sample) {
/*
struct particle
{
double weight; // weight of a particle
CvMat* loc_p = cvCreateMat(2, 1, CV_64FC1); // previously estimated position of a particle
CvMat* loc = cvCreateMat(2, 1, CV_64FC1); // currently estimated position of a particle
CvMat* velocity = cvCreateMat(2, 1, CV_64FC1); // estimated velocity of a particle
cvSub(loc, loc_p, velocity); // loc - loc_p -> velocity
};
*/
int main (int argc, char * const argv[]) {
srand(time(NULL));
int width = 400; // width of image window
int height = 400; // height of image window
CvMat* state = cvCreateMat(2, 1, CV_64FC1);
// declare particles
/* particle pb[N]; // N estimated particles
particle pp[N]; // N predicted particles
particle pu[N]; // temporary variables for updating particles
*/
CvMat* pb [N]; // estimated particles
CvMat* pp [N]; // predicted particles
CvMat* pu [N]; // temporary variables to update a particle
CvMat* v[N]; // estimated velocity of each particle
CvMat* vu[N]; // temporary varialbe to update the velocity
double w[N]; // weight of each particle
for (int n = 0; n < N; n++)
{
pb[n] = cvCreateMat(2, 1, CV_64FC1);
pp[n] = cvCreateMat(2, 1, CV_64FC1);
pu[n] = cvCreateMat(2, 1, CV_64FC1);
v[n] = cvCreateMat(2, 1, CV_64FC1);
vu[n] = cvCreateMat(2, 1, CV_64FC1);
}
// initialize particles and the state
for (int n = 0; n < N; n++)
{
w[n] = (double) 1 / (double) N; // equally weighted
for (int row=0; row < 2; row++)
{
cvmSet(pb[n], row, 0, 0.0);
cvmSet(v[n], row, 0, 15 * uniform_random());
cvmSet(state, row, 0, 0.0);
}
}
// set the system
CvMat* groundtruth = cvCreateMat(2, 1, CV_64FC1); // groundtruth of states
CvMat* measurement [3]; // measurement of states
for (int k = 0; k < 3; k++) // 3 types of measurement
{
measurement[k] = cvCreateMat(2, 1, CV_64FC1);
}
// evaluate measurements
double range = (double) width; // range to search measurements for each particle
// cout << "range of distances = " << range << endl;
int mselected;
for (int index = 0; index < count; index++)
{
double d = distance(measurement[index], pp[n]);
if ( d < range )
{
range = d;
mselected = index; // selected measurement
}
}
/// cout << "selected measurement # = " << mselected << endl;
like[n] = likelihood(measurement[mselected], measurement_noise, pp[n]);
/// cout << "likelihood of #" << n << " = " << like[n] << endl;
Lessons:
1. transition noise와 measurement noise, (그것들의 covariances) 그리고 각 입자의 초기 위치와 상태를 알맞게 설정하는 것이 관건임을 배웠다. 그것을 Tuning이라 부른다는 것도.
1-1. 코드에서 가정한 system에서는 특히 입자들의 초기 속도를 어떻게 주느냐에 따라 tracking의 성공 여부가 좌우된다.
2. 실제 상태는 등가속도 운동을 하는 비선형 시스템이나, 여기에서는 프레임 간의 운동을 등속으로 가정하여 선형 시스템으로 근사한 모델을 적용한 것이다.
2-1. 그러므로 여기에 Kalman filter를 적용하여 결과를 비교해 볼 수 있겠다. 이 경우, 3차원 Kalman gain을 계산해야 한다.
2-2. 분홍색 부분을 고쳐 비선형 모델로 만든 후 Particle filtering을 하면 결과가 더 좋지 않을까? Tuning도 더 쉬어지지 않을까?
3. 코드 좀 정리하자. 너무 지저분하다. ㅡㅡ;
4. 아니, 근데 영 헤매다가 갑자기 따라잡는 건 뭐지??? (아래 결과)
// count the number of detections in measurement process
int count_detections (void)
{
// set cases of measurement results
double mtype[4];
mtype [0] = 0.0;
mtype [1] = 0.5;
mtype [2] = mtype[1] + 0.3;
mtype [3] = mtype[2] + 0.2;
// cout << "check measurement type3 = " << mtype[3] << endl; // just to check
// select a measurement case
double mrn = uniform_random();
int type = 1;
for ( int k = 0; k < 3; k++ )
{
if ( mrn < mtype[k] )
{
type = k;
break;
}
}
return type;
}
// get measurements
int get_measurement (CvMat* measurement[], CvMat* measurement_noise, double x, double y)
{
// set measurement types
double c1 = 1.0, c2 = 4.0;
// measured point 1
cvmSet(measurement[0], 0, 0, x + (c1 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
cvmSet(measurement[0], 1, 0, y + (c1 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
// measured point 2
cvmSet(measurement[1], 0, 0, x + (c2 * cvmGet(measurement_noise,0,0) * gaussian_random())); // x-value
cvmSet(measurement[1], 1, 0, y + (c2 * cvmGet(measurement_noise,1,1) * gaussian_random())); // y-value
// measured point 3 // clutter noise
cvmSet(measurement[2], 0, 0, width*uniform_random()); // x-value
cvmSet(measurement[2], 1, 0, height*uniform_random()); // y-value
// count the number of measured points
int number = count_detections(); // number of detections
cout << "# of measured points = " << number << endl;
// get measurement
for (int index = 0; index < number; index++)
{
cout << "measurement #" << index << " : "
<< cvmGet(measurement[index],0,0) << " " << cvmGet(measurement[index],1,0) << endl;
// set the system
CvMat* state = cvCreateMat(2, 1, CV_64FC1); // state of the system to be estimated
CvMat* groundtruth = cvCreateMat(2, 1, CV_64FC1); // groundtruth of states
CvMat* measurement [3]; // measurement of states
for (int k = 0; k < 3; k++) // 3 types of measurement
{
measurement[k] = cvCreateMat(2, 1, CV_64FC1);
}
// declare particles
CvMat* pb [N]; // estimated particles
CvMat* pp [N]; // predicted particles
CvMat* pu [N]; // temporary variables to update a particle
CvMat* v[N]; // estimated velocity of each particle
CvMat* vu[N]; // temporary varialbe to update the velocity
double w[N]; // weight of each particle
for (int n = 0; n < N; n++)
{
pb[n] = cvCreateMat(2, 1, CV_64FC1);
pp[n] = cvCreateMat(2, 1, CV_64FC1);
pu[n] = cvCreateMat(2, 1, CV_64FC1);
v[n] = cvCreateMat(2, 1, CV_64FC1);
vu[n] = cvCreateMat(2, 1, CV_64FC1);
}
// initialize the state and particles
for (int n = 0; n < N; n++)
{
w[n] = (double) 1 / (double) N; // equally weighted
for (int row=0; row < 2; row++)
{
cvmSet(state, row, 0, 0.0);
cvmSet(pb[n], row, 0, 0.0);
cvmSet(v[n], row, 0, 15 * uniform_random());
}
}
// set the process noise
// covariance of Gaussian noise to control
CvMat* transition_noise = cvCreateMat(2, 2, CV_64FC1);
cvmSet(transition_noise, 0, 0, 3); //set transition_noise(0,0) to 3.0
cvmSet(transition_noise, 0, 1, 0.0);
cvmSet(transition_noise, 1, 0, 0.0);
cvmSet(transition_noise, 1, 1, 6);
// set the measurement noise
// covariance of Gaussian noise to measurement
CvMat* measurement_noise = cvCreateMat(2, 2, CV_64FC1);
cvmSet(measurement_noise, 0, 0, 2); //set measurement_noise(0,0) to 2.0
cvmSet(measurement_noise, 0, 1, 0.0);
cvmSet(measurement_noise, 1, 0, 0.0);
cvmSet(measurement_noise, 1, 1, 2);
// initialize the image window
cvZero(iplImg);
cvNamedWindow("ParticleFilter-3d", 0);
for (int t = 0; t < step; t++) // for "step" steps
{
// cvZero(iplImg);
cout << "step " << t << endl;
// get the groundtruth
double gx = 10 * t;
double gy = (-1.0 / width ) * (gx - width) * (gx - width) + height;
get_groundtruth(groundtruth, gx, gy);
// get measurements
int count = get_measurement(measurement, measurement_noise, gx, gy);
double like[N]; // likelihood between measurement and prediction
double like_sum = 0; // sum of likelihoods
for (int n = 0; n < N; n++) // for "N" particles
{
// predict
double prediction;
for (int row = 0; row < 2; row++)
{
prediction = cvmGet(pb[n],row,0) + cvmGet(v[n],row,0) + cvmGet(transition_noise,row,row) * gaussian_random();
cvmSet(pp[n], row, 0, prediction);
}
// cvLine(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), cvPoint(cvRound(cvmGet(pb[n],0,0)), cvRound(cvmGet(pb[n],1,0))), CV_RGB(100,100,0), 1);
// cvCircle(iplImg, cvPoint(cvRound(cvmGet(pp[n],0,0)), cvRound(cvmGet(pp[n],1,0))), 1, CV_RGB(255, 255, 0));
// evaluate measurements
double range = (double) width; // range to search measurements for each particle
// cout << "range of distances = " << range << endl;
int mselected;
for (int index = 0; index < count; index++)
{
double d = distance(measurement[index], pp[n]);
if ( d < range )
{
range = d;
mselected = index; // selected measurement
}
}
// cout << "selected measurement # = " << mselected << endl;
like[n] = likelihood(measurement[mselected], measurement_noise, pp[n]);
// cout << "likelihood of #" << n << " = " << like[n] << endl;
like_sum += like[n];
}
// cout << "sum of likelihoods = " << like_sum << endl;
// estimate states
double state_x = 0.0;
double state_y = 0.0;
// estimate the state with predicted particles
for (int n = 0; n < N; n++) // for "N" particles
{
w[n] = like[n] / like_sum; // update normalized weights of particles
// cout << "w" << n << "= " << w[n] << " ";
state_x += w[n] * cvmGet(pp[n], 0, 0); // x-value
state_y += w[n] * cvmGet(pp[n], 1, 0); // y-value
}
cvmSet(state, 0, 0, state_x);
cvmSet(state, 1, 0, state_y);
// define integrated portions of each particles; 0 < a[0] < a[1] < a[2] = 1
a[0] = w[0];
for (int n = 1; n < N; n++)
{
a[n] = a[n - 1] + w[n];
// cout << "a" << n << "= " << a[n] << " ";
}
// cout << "a" << N << "= " << a[N] << " " << endl;
for (int n = 0; n < N; n++)
{
// select a particle from the distribution
int pselected;
for (int k = 0; k < N; k++)
{
if ( uniform_random() < a[k] )
{
pselected = k;
break;
}
}
// cout << "p " << n << " => " << pselected << " ";
CvMat* state = cvCreateMat(2, 1, CV_64FC1); //states to be estimated
CvMat* state_p = cvCreateMat(2, 1, CV_64FC1); //states to be predicted
CvMat* velocity = cvCreateMat(2, 1, CV_64FC1); //motion controls to change states
CvMat* measurement = cvCreateMat(2, 1, CV_64FC1); //measurement of states
CvMat* cov = cvCreateMat(2, 2, CV_64FC1); //covariance to be updated
CvMat* cov_p = cvCreateMat(2, 2, CV_64FC1); //covariance to be predicted
CvMat* gain = cvCreateMat(2, 2, CV_64FC1); //Kalman gain to be updated
// temporary matrices to be used for estimation
CvMat* Kalman = cvCreateMat(2, 2, CV_64FC1); //
CvMat* invKalman = cvCreateMat(2, 2, CV_64FC1); //
CvMat* I = cvCreateMat(2,2,CV_64FC1);
cvSetIdentity(I); // does not seem to be working properly
// cvSetIdentity (I, cvRealScalar (1));
// check matrix
for(int i=0; i<2; i++)
{
for(int j=0; j<2; j++)
{
cout << cvmGet(I, i, j) << "\t";
}
cout << endl;
}
// set the initial state
cvmSet(state, 0, 0, 0.0); //x-value //set state(0,0) to 0.0
cvmSet(state, 1, 0, 0.0); //y-value //set state(1,0) to 0.0
// set the initital covariance of state
cvmSet(cov, 0, 0, 0.5); //set cov(0,0) to 0.5
cvmSet(cov, 0, 1, 0.0); //set cov(0,1) to 0.0
cvmSet(cov, 1, 0, 0.0); //set cov(1,0) to 0.0
cvmSet(cov, 1, 0, 0.4); //set cov(1,1) to 0.4
// set the initial control
cvmSet(velocity, 0, 0, 10.0); //x-direction //set velocity(0,0) to 1.0
cvmSet(velocity, 1, 0, 10.0); //y-direction //set velocity(0,0) to 1.0
for (int t=0; t<step; t++)
{
// retain the current state
CvMat* state_out = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvmSet(state_out, 0, 0, cvmGet(state,0,0));
cvmSet(state_out, 1, 0, cvmGet(state,1,0));
// predict
cvAdd(state, velocity, state_p); // state + velocity -> state_p
cvAdd(cov, transition_noise, cov_p); // cov + transition_noise -> cov_p
// measure
cvmSet(measurement, 0, 0, measurement_set[t]); //x-value
cvmSet(measurement, 1, 0, measurement_set[step+t]); //y-value
// estimate Kalman gain
cvAdd(cov_p, measurement_noise, Kalman); // cov_p + measure_noise -> Kalman
cvInvert(Kalman, invKalman); // inv(Kalman) -> invKalman
cvMatMul(cov_p, invKalman, gain); // cov_p * invKalman -> gain
// update the state
CvMat* err = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvSub(measurement, state_p, err); // measurement - state_p -> err
CvMat* adjust = cvCreateMat(2, 1, CV_64FC1); // temporary vector
cvMatMul(gain, err, adjust); // gain*err -> adjust
cvAdd(state_p, adjust, state); // state_p + adjust -> state
// update the covariance of states
CvMat* cov_up = cvCreateMat(2, 2, CV_64FC1); // temporary matrix
cvSub(I, gain, cov_up); // I - gain -> cov_up
cvMatMul(cov_up, cov_p, cov); // cov_up *cov_p -> cov
// update the control
cvSub(state, state_out, velocity); // state - state_p -> velocity