void test_EKF_Panonly_noise_ptz_data()
{
vcl_string ptz_file("/Users/jimmy/Desktop/images/33_slam_data/ptz_145420_145719.txt");
vcl_vector<PTZData> ptzs;
bool isRead = readPTZCameraFile(ptz_file.c_str(), ptzs, 1);
assert(isRead);
vcl_vector<double> gdPans;
vcl_vector<double> gdTilts;
vcl_vector<double> gdZooms;
vcl_vector<double> observedPans;
vcl_vector<double> observedTilts;
vcl_vector<double> observedZooms;
vcl_vector<vcl_vector<vgl_point_2d<double> > > observedImagePts;
const int width = 1280;
const int height = 720;
vnl_random rnd;
double delta = 2.0;
vgl_point_2d<double> pp(width/2, height/2);
vcl_vector<vgl_point_2d<double> > firstWorldPts;
for (int i = 0; i<ptzs.size(); i++) {
gdPans.push_back(ptzs[i].pan);
gdTilts.push_back(ptzs[i].tilt);
gdZooms.push_back(ptzs[i].fl);
vpgl_perspective_camera<double> gdCamera;
bool isCamera = VxlPTZCamera::PTZToCamera(ptzs[i].fl, ptzs[i].pan, ptzs[i].tilt, gdCamera);
assert(isCamera);
// project image position
vcl_vector<vgl_point_2d<double> > worldPts;
vcl_vector<vgl_point_2d<double> > dump;
vcl_vector<vgl_point_2d<double> > imagePts;
DisneyWorldBasketballCourt::projectCalibPoints(gdCamera, width, height, worldPts, dump, imagePts, 10000);
// assert(worldPts.size() == 35);
if (firstWorldPts.size() == 0) {
firstWorldPts = worldPts;
}
assert(firstWorldPts.size() == worldPts.size());
// add noise to image points
for (int j = 0; j<imagePts.size(); j++) {
double x = imagePts[j].x();
double y = imagePts[j].y();
x += delta * rnd.normal();
y += delta * rnd.normal();
imagePts[j].set(x, y);
}
observedImagePts.push_back(imagePts);
// recalibrate camera
vpgl_perspective_camera<double> initCamera;
vpgl_perspective_camera<double> finalCamera;
bool isInit = VpglPlus::init_calib(worldPts, imagePts, pp, initCamera);
if (!isInit) {
printf("initiate camera error\n");
continue;
}
bool isFinal = VpglPlus::optimize_perspective_camera(worldPts, imagePts, initCamera, finalCamera);
if (!isFinal) {
printf("final camera error\n");
continue;
}
//
double pan = 0;
double tilt = 0;
double zoom = 0;
bool isPTZ = VxlPTZCamera::CameraToPTZ(finalCamera, pan, tilt, zoom);
assert(isPTZ);
observedPans.push_back(pan);
observedTilts.push_back(tilt);
observedZooms.push_back(zoom);
}
assert(gdPans.size() == observedPans.size());
assert(gdTilts.size() == observedTilts.size());
assert(gdZooms.size() == observedZooms.size());
double pan0 = gdPans[0];
const int C_M = 2; //camera state length
PanCalibrationEKF panEKF;
vnl_vector<double> x0(C_M + (int)firstWorldPts.size() * 3, 0);
x0[0] = pan0; x0[1] = 1e-10;
for (int i = 0; i<firstWorldPts.size(); i++) {
x0[C_M + 3 * i + 0] = firstWorldPts[i].x();
x0[C_M + 3 * i + 1] = firstWorldPts[i].y();
x0[C_M + 3 * i + 2] = 0.0;
}
//todo, needs to improve
vnl_matrix<double> P0(x0.size(), x0.size(), 0);
// P0(0, 0) = 0.1; P0(1, 1) = INT_MAX;
panEKF.init(x0, P0);
const int M = (int)firstWorldPts.size();
vnl_matrix<double> Q(C_M + M * 3, C_M + M * 3, 0);
Q(0, 0) = 0.0004; Q(1, 1) = 0.0001;
for (int i = 0; i<M; i++) {
Q(C_M + i, C_M + i) = 0.0;
}
vnl_matrix<double> Rk(M*2, M*2, 0);
for (int i = 0; i<M*2; i++) {
Rk(i, i) = 0.05;
}
panEKF.initNoiseCovariance(Q, Rk);
vcl_vector<vnl_vector<double> > states;
vcl_vector<double> smoothedPan;
for (int i = 0; i<observedPans.size(); i++) {
vnl_vector<double> zk(2 * M, 0); // initialize landmarks
printf("observed pan is %f\n", observedPans[i]);
// add observed point in the image space
for (int j = 0; j<M; j++) {
zk[j * 2] = observedImagePts[i][j].x();
zk[j * 2 + 1] = observedImagePts[i][j].y();
}
vnl_vector<double> xk;
vnl_matrix<double> pk;
panEKF.set_tilt_focal_length(gdTilts[i], gdZooms[i]);
panEKF.update(zk, xk, pk);
states.push_back(xk);
printf("final pan, pan_velocity is %f %f\n", xk[0], xk[1]);
printf("gd pan, is %f \n\n", gdPans[i]);
// break;
smoothedPan.push_back(xk[0]);
}
vcl_string save_file("EKF_panOnly_simulated_noise.mat");
vnl_matlab_filewrite awriter(save_file.c_str());
awriter.write(vnl_vector<double>(&observedPans[0], (int)observedPans.size()), "observed_pan");
awriter.write(vnl_vector<double>(&smoothedPan[0], (int)smoothedPan.size()), "smoothed_pan");
awriter.write(vnl_vector<double>(&gdPans[0], (int)gdPans.size()), "gdPans");
printf("save to %s\n", save_file.c_str());
}
void System::get_kalman_path( vector<State>& obs, vector<double>& obs_times, list<State>& kalman_path)
{
#if 0
kalman_path.clear();
kalman_path.push_back(init_state);
Matrix3d Q;
Q << init_var[0], 0, 0, 0, init_var[1], 0, 0, 0, init_var[2];
Vector3d curr_state;
curr_state(0) = init_state.x[0];
curr_state(1) = init_state.x[1];
curr_state(2) = init_state.x[2];
MatrixXd Rk(3,3);
Rk << obs_noise[0], 0, 0, 0, obs_noise[1], 0, 0, 0, obs_noise[2];
Matrix3d Cd = Matrix3d::Identity();
double prev_time = 0;
for(unsigned int i=0; i< obs.size(); i++)
{
State& next_obs = obs[i];
Vector3d noisy_obs;
noisy_obs(0) = next_obs.x[0];
noisy_obs(1) = next_obs.x[1];
noisy_obs(2) = next_obs.x[2];
double delta_t = obs_times[i] - prev_time;
//cout<<"delta_t: "<< delta_t << endl;
State stmp1;
stmp1.x[0] = curr_state(0);
stmp1.x[1] = curr_state(1);
stmp1.x[2] = curr_state(2);
State next_state = integrate(stmp1, delta_t, true);
State clean_obs = observation(next_state, true);
Matrix3d Ad;
Ad << 0, 0, -sin(stmp1.x[2]), 0, 0, cos(stmp1.x[2]), 0, 0, 0;
Matrix3d Wk;
Wk << process_noise[0]*delta_t, 0, 0, 0, process_noise[1]*delta_t, 0, 0, 0, process_noise[2]*delta_t;
Matrix3d Q_new = Ad * Q * Ad.transpose() + Wk;
Matrix3d Lk = Q_new*Cd.transpose()*(Cd*Q_new*Cd.transpose() + Rk).inverse();
Vector3d Sk;
curr_state(0) = next_state.x[0];
curr_state(1) = next_state.x[1];
curr_state(2) = next_state.x[2];
Sk = noisy_obs - Cd*curr_state;
Vector3d estimate = curr_state + Lk*Sk;
Matrix3d covar = (Matrix3d::Identity() - Lk*Cd)*Q_new;
Q = covar;
curr_state = estimate;
State stmp2;
stmp2.x[0] = curr_state(0);
stmp2.x[1] = curr_state(1);
stmp2.x[2] = curr_state(2);
kalman_path.push_back(stmp2);
prev_time = obs_times[i];
}
#endif
}
