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

moved camera calibration information into a struct to make data handling smoother

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-16 19:05:11 +02:00
parent 010d36d216
commit 7f2140ae88
5 changed files with 107 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
src/msckf_vio.cpp
View File

@ -119,49 +119,49 @@ bool MsckfVio::loadParameters() {
// get camera information (used for back projection)
nh.param<string>("cam0/distortion_model",
cam0_distortion_model, string("radtan"));
cam0.distortion_model, string("radtan"));
nh.param<string>("cam1/distortion_model",
cam1_distortion_model, string("radtan"));
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];
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];
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];
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];
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];
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];
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];
@ -904,7 +904,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//photometric observation
std::vector<uint8_t> photo_z;
feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs, cam0_moving_window, photo_z);
feature.IrradianceOfAnchorPatch(cam_state, cam_state_id, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs, cam0_moving_window, photo_z);
// Convert the feature position from the world frame to
@ -921,9 +921,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
/*
//prints the feature projection in pixel space
std::vector<cv::Point2f> my_p = image_handler::distortPoints( out_v,
cam0_intrinsics,
cam0_distortion_model,
cam0_distortion_coeffs);
cam0.intrinsics,
cam0.distortion_model,
cam0.distortion_coeffs);
printf("projection: %f, %f\n", my_p[0].x, my_p[0].y);
*/
@ -1316,7 +1316,7 @@ void MsckfVio::removeLostFeatures() {
}
}
if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs))
if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs))
{
invalid_feature_ids.push_back(feature.id);
continue;
@ -1470,7 +1470,7 @@ void MsckfVio::pruneCamStateBuffer() {
}
}
if(!feature.initializeAnchor(cam0_moving_window, cam0_intrinsics, cam0_distortion_model, cam0_distortion_coeffs))
if(!feature.initializeAnchor(cam0_moving_window, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs))
{
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);