raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added position calculation

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-12 17:37:01 +02:00
parent a85a4745f2
commit 8227a8e48d
8 changed files with 241 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
src/msckf_vio.cpp
View File

@ -117,6 +117,54 @@ bool MsckfVio::loadParameters() {
nh.param<double>("initial_covariance/extrinsic_translation_cov",
extrinsic_translation_cov, 1e-4);
// get camera information (used for back projection)
nh.param<string>("cam0/distortion_model",
cam0_distortion_model, string("radtan"));
nh.param<string>("cam1/distortion_model",
cam1_distortion_model, string("radtan"));
vector<int> cam0_resolution_temp(2);
nh.getParam("cam0/resolution", cam0_resolution_temp);
cam0_resolution[0] = cam0_resolution_temp[0];
cam0_resolution[1] = cam0_resolution_temp[1];
vector<int> cam1_resolution_temp(2);
nh.getParam("cam1/resolution", cam1_resolution_temp);
cam1_resolution[0] = cam1_resolution_temp[0];
cam1_resolution[1] = cam1_resolution_temp[1];
vector<double> cam0_intrinsics_temp(4);
nh.getParam("cam0/intrinsics", cam0_intrinsics_temp);
cam0_intrinsics[0] = cam0_intrinsics_temp[0];
cam0_intrinsics[1] = cam0_intrinsics_temp[1];
cam0_intrinsics[2] = cam0_intrinsics_temp[2];
cam0_intrinsics[3] = cam0_intrinsics_temp[3];
vector<double> cam1_intrinsics_temp(4);
nh.getParam("cam1/intrinsics", cam1_intrinsics_temp);
cam1_intrinsics[0] = cam1_intrinsics_temp[0];
cam1_intrinsics[1] = cam1_intrinsics_temp[1];
cam1_intrinsics[2] = cam1_intrinsics_temp[2];
cam1_intrinsics[3] = cam1_intrinsics_temp[3];
vector<double> cam0_distortion_coeffs_temp(4);
nh.getParam("cam0/distortion_coeffs",
cam0_distortion_coeffs_temp);
cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
cam0_distortion_coeffs[2] = cam0_distortion_coeffs_temp[2];
cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];
vector<double> cam1_distortion_coeffs_temp(4);
nh.getParam("cam1/distortion_coeffs",
cam1_distortion_coeffs_temp);
cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
state_server.state_cov = MatrixXd::Zero(21, 21);
for (int i = 3; i < 6; ++i)
state_server.state_cov(i, i) = gyro_bias_cov;
@ -1114,7 +1162,7 @@ void MsckfVio::removeLostFeatures() {
}
}
if(!feature.initializeAnchor(cam0_moving_window))
if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs))
{
invalid_feature_ids.push_back(feature.id);
continue;