From 1a07ba3d3c85b5ce84506fb671119c5614128115 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 5 Jul 2019 09:43:35 +0200 Subject: [PATCH] added larger sobel filter in calculation - converges sometimes for a few seconds --- launch/msckf_vio_tinytum.launch | 4 +- launch/msckf_vio_tum.launch | 4 +- src/msckf_vio.cpp | 99 +++++++++++++++++++++++---------- 3 files changed, 73 insertions(+), 34 deletions(-) diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 28ae944..d5de69a 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -25,7 +25,7 @@ - + @@ -33,7 +33,7 @@ - + diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b7e9365..7b1b86a 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,14 +18,14 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 5e3dd8c..016ef8b 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity(); // Static member variables in Feature class. FeatureIDType Feature::next_id = 0; -double Feature::observation_noise = 0.01; +double Feature::observation_noise = 0.05; Feature::OptimizationConfig Feature::optimization_config; map MsckfVio::chi_squared_test_table; @@ -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.05); + boost::math::quantile(chi_squared_dist, 0.1); } if (!createRosIO()) return false; @@ -555,19 +555,21 @@ void MsckfVio::manageMovingWindow( cv::Mat deeper_frame; cam0_img_ptr->image.convertTo(deeper_frame,CV_16S); - cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); - cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); - xder/=8.; - yder/=8.; + cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); + xder/=72.; + yder/=72.; cam0.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam0.moving_window[state_server.imu_state.id].dyimage = yder.clone(); cam1_img_ptr->image.convertTo(deeper_frame,CV_16S); - cv::Sobel(deeper_frame, xder, -1, 1, 0, 3); - cv::Sobel(deeper_frame, yder, -1, 0, 1, 3); - xder/=8.; - yder/=8.; + cv::Sobel(deeper_frame, xder, -1, 1, 0, 5); + cv::Sobel(deeper_frame, yder, -1, 0, 1, 5); + xder/=72.; + yder/=72.; + + cvWaitKey(0); cam1.moving_window[state_server.imu_state.id].dximage = xder.clone(); cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone(); @@ -1679,7 +1681,7 @@ bool MsckfVio::PhotometricPatchPointJacobian( dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; - cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + //cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; // add jacobian @@ -1718,6 +1720,10 @@ bool MsckfVio::PhotometricPatchPointJacobian( H_plj = dI_dhj * dh_dC0pij * dC0pij_dXplj + dI_dhj * dh_dC1pij * R_c0_c1 * dC0pij_dXplj; // 4 x 6 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) + return false; + // check if point nullspaceable if (H_rhoj(0, 0) != 0) return true; @@ -1751,9 +1757,11 @@ bool MsckfVio::PhotometricMeasurementJacobian( Eigen::MatrixXd H_pA(2 * N*N, 6); // calcualte residual of patch - PhotometricPatchPointResidual(cam_state_id, feature, r_photo); + if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) + return false; - cout << "r\n" << r_photo << endl; + + //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0; bool valid = false; @@ -2446,7 +2454,6 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r r_thin = r; } - // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; MatrixXd S = H_thin*P*H_thin.transpose() + @@ -2536,12 +2543,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r MatrixXd state_cov_fixed = (state_server.state_cov + state_server.state_cov.transpose()) / 2.0; state_server.state_cov = state_cov_fixed; + return; } bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - return true; + MatrixXd P1 = H * state_server.state_cov * H.transpose(); MatrixXd P2 = Feature::observation_noise * @@ -2549,12 +2557,40 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - //cout << "gate: " << dof << " " << gamma << " " << - //chi_squared_test_table[dof] << endl; + // cout << "r" << r << endl; + // cout << "procov" << P1+P2 << endl; + cout << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; + + if(gamma > 1000000) + { + cout << " logging " << endl; + ofstream myfile; + myfile.open("/home/raphael/dev/octave/log2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: H\n" + << "# type: matrix\n" + << "# rows: " << H.rows() << "\n" + << "# columns: " << H.cols() << "\n" + << H << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << 1 << "\n" + << r << endl; + + myfile << "# name: C\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.cols() << "\n" + << state_server.state_cov << endl; + myfile.close(); + } if (chi_squared_test_table[dof] == 0) return false; - if (gamma < chi_squared_test_table[dof]) { + if (gamma < chi_squared_test_table[dof]*10) { // cout << "passed" << endl; return true; } else { @@ -2661,13 +2697,14 @@ 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)) { - //cout << "passed" << endl; + 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, cam_state_ids, H_xj, r_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2681,6 +2718,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. @@ -2695,7 +2733,7 @@ void MsckfVio::removeLostFeatures() { photometricMeasurementUpdate(pH_x, pr); } - + /* H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2705,7 +2743,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); @@ -2845,13 +2883,13 @@ 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)) { - //cout << "passed" << endl; + 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); @@ -2867,7 +2905,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); } @@ -2881,7 +2919,7 @@ void MsckfVio::pruneLastCamStateBuffer() photometricMeasurementUpdate(pH_x, pr); } - + /* H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2891,7 +2929,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)); @@ -3024,17 +3062,17 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - 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())) { - //cout << "passed" << endl; + 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); @@ -3049,7 +3087,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); } @@ -3063,6 +3101,7 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } + /* H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -3072,7 +3111,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(),