From 5451c2d097696fb0502ab3a76fc6c4cde2a0fadb Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Thu, 27 Jun 2019 16:04:29 +0200 Subject: [PATCH] uncommented stuff --- launch/msckf_vio_tum.launch | 4 ++-- src/msckf_vio.cpp | 38 +++++++++++++++++++------------------ 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 1c2c689..1096e37 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -22,10 +22,10 @@ - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 9a31f42..d6f8326 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -554,15 +554,19 @@ void MsckfVio::manageMovingWindow( // undistort Images + + const cv::Matx33d K(cam0.intrinsics[0], 0.0, cam0.intrinsics[2], + 0.0, cam0.intrinsics[1], cam0.intrinsics[3], + 0.0, 0.0, 1.0); - //cv::Mat undistortedCam0; - //cv::Mat undistortedCam1; + cv::Mat undistortedCam0; + cv::Mat undistortedCam1; - //image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); - //image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - //cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); - //cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone(); + cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); + cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone(); //TODO handle any massive overflow correctly (should be pruned, before this ever triggers) while(cam0.moving_window.size() > 100) @@ -2061,20 +2065,20 @@ void MsckfVio::removeLostFeatures() { MatrixXd pH_xj; VectorXd pr_j; - //PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); + PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } - /* + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - */ + // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. //if (stack_cntr > 1500) break; @@ -2087,7 +2091,7 @@ void MsckfVio::removeLostFeatures() { // Perform the measurement update step. measurementUpdate(H_x, r); - //photometricMeasurementUpdate(pH_x, pr); + photometricMeasurementUpdate(pH_x, pr); // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) @@ -2217,7 +2221,7 @@ void MsckfVio::pruneLastCamStateBuffer() for (const auto& cam_state : state_server.cam_states) involved_cam_state_ids.push_back(cam_state.first); - //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { @@ -2226,13 +2230,11 @@ void MsckfVio::pruneLastCamStateBuffer() stack_cntr += H_xj.rows(); pruned_cntr++; } - /* if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - */ for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2245,7 +2247,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Perform measurement update. measurementUpdate(H_x, r); - //photometricMeasurementUpdate(pH_x, pr); + photometricMeasurementUpdate(pH_x, pr); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), @@ -2372,7 +2374,7 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - //PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { @@ -2380,13 +2382,13 @@ void MsckfVio::pruneCamStateBuffer() { r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } - /* + if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pstack_cntr += pH_xj.rows(); } - */ + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2399,7 +2401,7 @@ void MsckfVio::pruneCamStateBuffer() { // Perform measurement update. measurementUpdate(H_x, r); - //photometricMeasurementUpdate(pH_x, pr); + photometricMeasurementUpdate(pH_x, pr); //reduction for (const auto& cam_id : rm_cam_state_ids) {