From a7c296ca3d14b7b639032421fcb6b9fc18244cfe Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 5 Jul 2019 13:51:58 +0200 Subject: [PATCH] removed dx filter, corrected jacobi calculation with bigger sobel (and correct division), removed scale for mahalanobis --- include/msckf_vio/feature.hpp | 3 +- include/msckf_vio/msckf_vio.h | 2 +- launch/msckf_vio_euroc.launch | 4 +- src/msckf_vio.cpp | 70 +++++++++++++++++++++-------------- 4 files changed, 48 insertions(+), 31 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index a2a3d63..17e0ce1 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -736,6 +736,7 @@ bool Feature::VisualizeKernel( cvWaitKey(0); } + bool Feature::VisualizePatch( const CAMState& cam_state, const StateIDType& cam_state_id, @@ -934,7 +935,7 @@ bool Feature::VisualizePatch( //cv::imwrite(loc.str(), cam0.featureVisu); cv::imshow("patch", cam0.featureVisu); - cvWaitKey(0); + cvWaitKey(1); } float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 06bad0f..3ffc586 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -261,7 +261,7 @@ class MsckfVio { void twoMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r); bool gatingTest(const Eigen::MatrixXd& H, - const Eigen::VectorXd&r, const int& dof); + const Eigen::VectorXd&r, const int& dof, int filter=0); void removeLostFeatures(); void findRedundantCamStates( std::vector& rm_cam_state_ids); diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index b818735..b57704e 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 016ef8b..ef8f76e 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -312,7 +312,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 1000; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.1); + boost::math::quantile(chi_squared_dist, 0.4); } if (!createRosIO()) return false; @@ -554,23 +554,34 @@ void MsckfVio::manageMovingWindow( cv::Mat yder; cv::Mat deeper_frame; + // generate derivative matrix for cam0 cam0_img_ptr->image.convertTo(deeper_frame,CV_16S); cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); - xder/=72.; - yder/=72.; + xder/=96.; + yder/=96.; + // save into moving window cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); + // generate derivative matrix for cam 1 cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); - xder/=72.; - yder/=72.; + xder/=96.; + yder/=96.; + + /* + cv::Mat norm_abs_xderImage; + cv::convertScaleAbs(xder, norm_abs_xderImage); + cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1); + + cv::imshow("xder", norm_abs_xderImage); cvWaitKey(0); - + */ + // save into moving window cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); @@ -1721,9 +1732,9 @@ bool MsckfVio::PhotometricPatchPointJacobian( H_pAj = dI_dhj * dh_dC0pij * dC0pij_dGpij * dGpj_XpAj + dI_dhj * dh_dC1pij * dC1pij_dGpij * dGpj_XpAj; // 4 x 6 // check if any direction not large enough for eval - if(dI_dhj(0, 0) < 0.01 or dI_dhj(0, 1) < 0.01 or dI_dhj(1, 2) < 0.01 or dI_dhj(1, 3) < 0.01) + /*if((dI_dhj(0, 0) < 0.1 or dI_dhj(0, 1) < 0.1) and (dI_dhj(1, 2) < 0.1 or dI_dhj(1, 3) < 0.1)) return false; - + */ // check if point nullspaceable if (H_rhoj(0, 0) != 0) return true; @@ -1766,11 +1777,15 @@ bool MsckfVio::PhotometricMeasurementJacobian( int count = 0; bool valid = false; Matrix dI_dhj;// = Matrix::Zero(); + int valid_count = 0; for (auto point : feature.anchorPatch_3d) { // get jacobi of single point in patch if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj)) + { + valid_count++; valid = true; + } // stack point into entire jacobi H_rho.block<2, 1>(count*2, 0) = H_rhoj; @@ -1781,7 +1796,7 @@ bool MsckfVio::PhotometricMeasurementJacobian( count++; } - + cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl; //Eigen::Matrix h_photo = (dI_dh.transpose() * dI_dh).inverse() * dI_dh.transpose() * r_photo; //cout << "h photo: \n" << h_photo << endl; @@ -2548,7 +2563,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r } -bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { +bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, int filter) { MatrixXd P1 = H * state_server.state_cov * H.transpose(); @@ -2559,6 +2574,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) // cout << "r" << r << endl; // cout << "procov" << P1+P2 << endl; + if(filter == 1) cout << "gate: " << dof << " " << gamma << " " << chi_squared_test_table[dof] << endl; @@ -2588,9 +2604,9 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) myfile.close(); } - if (chi_squared_test_table[dof] == 0) + if (chi_squared_test_table[dof] == 0) return false; - if (gamma < chi_squared_test_table[dof]*10) { + if (gamma < chi_squared_test_table[dof]) { // cout << "passed" << endl; return true; } else { @@ -2696,7 +2712,7 @@ void MsckfVio::removeLostFeatures() { if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j)) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; @@ -2704,7 +2720,7 @@ void MsckfVio::removeLostFeatures() { } } - /* + featureJacobian(feature.id, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2718,7 +2734,7 @@ void MsckfVio::removeLostFeatures() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - */ + // Put an upper bound on the row size of measurement Jacobian, // which helps guarantee the executation time. @@ -2733,7 +2749,7 @@ void MsckfVio::removeLostFeatures() { photometricMeasurementUpdate(pH_x, pr); } - /* + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2743,7 +2759,7 @@ void MsckfVio::removeLostFeatures() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - */ + // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) map_server.erase(feature_id); @@ -2882,14 +2898,14 @@ void MsckfVio::pruneLastCamStateBuffer() if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) { cout << "passed" << endl; 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(); } } - /* + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -2905,7 +2921,7 @@ void MsckfVio::pruneLastCamStateBuffer() twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - */ + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -2919,7 +2935,7 @@ void MsckfVio::pruneLastCamStateBuffer() photometricMeasurementUpdate(pH_x, pr); } - /* + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2929,7 +2945,7 @@ void MsckfVio::pruneLastCamStateBuffer() // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - */ + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -3064,7 +3080,7 @@ void MsckfVio::pruneCamStateBuffer() { if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) { - if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) { cout << "passed" << endl; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pr.segment(pstack_cntr, pr_j.rows()) = pr_j; @@ -3072,7 +3088,7 @@ void MsckfVio::pruneCamStateBuffer() { } } - /* + featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); @@ -3087,7 +3103,7 @@ void MsckfVio::pruneCamStateBuffer() { twor.segment(twostack_cntr, twor_j.rows()) = twor_j; twostack_cntr += twoH_xj.rows(); } - */ + for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } @@ -3101,7 +3117,7 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } - /* + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -3111,7 +3127,7 @@ void MsckfVio::pruneCamStateBuffer() { // Perform the measurement update step. measurementUpdate(H_x, r); twoMeasurementUpdate(twoH_x, twor); - */ + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(),