ObjectTracker::ObjectTracker(const ObjectMatch& match)
: m_nframes_without_detection(0),
m_nframes_with_detection(0),
m_has_updated_pose(false)
{
m_last_pose = match.pose()->pose3d();
m_raw_pose = match.pose()->pose3d();
m_estimated_pose = m_last_pose;
m_model = &(match.model());
m_last_projected_bounding_rect = bounding_box(match.pose()->projectedBoundingRect());
/*
* 12 variables
* tx, Dtx, ty, Dy, tz, Dtz, rx, Drx, ry, Dry, rz, Drz
* with D the derivative.
* This tracking assumes a constant speed model.
* Rotation are given as axis / angle as magnitude
* representation.
*/
m_kalman = KalmanFilter(12, 12, 0);
Mat1f state = Mat(12, 1, CV_32FC1);
Mat1f process_noise (12, 1);
Mat1f measurement (12, 1);
Mat1f measurement_noise (12, 1);
measurement = 0.f;
/*
* If there were only two variables, this generates a matrix
* with the following form:
* 1 1 0 0
* 0 1 0 0
* 0 0 1 1
* 0 0 1 0
*/
m_kalman.transitionMatrix = Mat1f(12, 12);
setIdentity(m_kalman.transitionMatrix);
for (int i = 0; i < 12; i += 2)
{
m_kalman.transitionMatrix.at<float>(i,i+1) = 1;
}
setIdentity(m_kalman.measurementMatrix, 1);
setIdentity(m_kalman.processNoiseCov, 1e-5);
setIdentity(m_kalman.measurementNoiseCov, 1e-1);
setIdentity(m_kalman.errorCovPost, 1);
cv::Vec3f r = m_last_pose.cvRodriguesRotation();
cv::Vec3f t = m_last_pose.cvTranslation();
// derivatives are null initially.
((Mat1f)m_kalman.statePost) << t[0], 0, t[1], 0, t[2], 0, r[0], 0, r[1], 0, r[2], 0;
}
void
runTest(void)
{
double t=0;
double dt=0.01;
int N=400;
std::vector<double> positions;
std::vector<double> velocities;
positions.resize(N);
velocities.resize(N);
std::generate_n(positions.begin(),
N,
[&t, &dt](void)
{
double x=sin(t*2*M_PI);
t += dt;
return x;
});
t = 0.0;
std::generate_n(velocities.begin(),
N,
[&t, &dt](void)
{
double x=2*M_PI*cos(t*2*M_PI);
t += dt;
return x;
});
std::vector<double> noisy_positions;
noisy_positions.resize(N);
std::mt19937 generator;
std::normal_distribution<double> normal(0.0, 0.1);
std::transform(positions.begin(),
positions.end(),
noisy_positions.begin(),
[&normal, &generator](double x)
{
return x+normal(generator);
});
TestUKF ukf(1e-3, 2.0, 0.0);
// acceleration noise
double sigma = (2*M_PI)*(2*M_PI)*10;
TestState st;
st.v=0.0;
ukf.reset(st);
Eigen::MatrixXd measurement_noise(1,1);
measurement_noise(0,0) = 0.02;
std::vector<Eigen::MatrixXd> mcovs;
mcovs.push_back(measurement_noise);
t = 0.0;
std::cout << t << ", " <<
positions[0] << ", " <<
velocities[0] << ", " <<
ukf.state().x << ", " <<
sqrt(ukf.state().Covariance(0,0)) << ", " <<
ukf.state().v << ", " <<
sqrt(ukf.state().Covariance(1,1)) << ", " <<
0.0 << std::endl;
int correct_every=1;
for(int i=0;i<N;i++)
{
ukf.predict(dt, process_noise(dt, sigma));
t += dt;
PositionMeasurement m;
m.x = noisy_positions[i];
if(i%correct_every==0)
ukf.correct(m, mcovs);
std::cout << t << ", " <<
positions[i] << ", " <<
velocities[i] << ", " <<
ukf.state().x << ", " <<
sqrt(ukf.state().Covariance(0,0)) << ", " <<
ukf.state().v << ", " <<
sqrt(ukf.state().Covariance(1,1)) << ", " <<
m.x << ", " <<
sqrt(measurement_noise(0,0)) <<
std::endl;
}
}
