From 2d045660c2f97036d6374692808c47581f94c61e Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 19 Jun 2019 09:13:46 +0200 Subject: [PATCH] added some debug output, added qr decomp --- include/msckf_vio/msckf_vio.h | 1 + launch/msckf_vio_tum.launch | 7 +- src/msckf_vio.cpp | 116 ++++++++++++++-------------------- 3 files changed, 55 insertions(+), 69 deletions(-) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 032e885..ad20d49 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -236,6 +236,7 @@ class MsckfVio { bool PHOTOMETRIC; // debug flag + bool STREAMPAUSE; bool PRINTIMAGES; bool GROUNDTRUTH; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index f6602c1..54e6c7e 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,13 +18,14 @@ output="screen"> - + + - + @@ -71,4 +72,6 @@ + + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 977a95b..a6fe1f9 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -69,6 +69,7 @@ bool MsckfVio::loadParameters() { //Photometry Flag nh.param("PHOTOMETRIC", PHOTOMETRIC, false); nh.param("PrintImages", PRINTIMAGES, false); + nh.param("StreamPause", STREAMPAUSE, false); nh.param("GroundTruth", GROUNDTRUTH, false); // Frame id @@ -304,10 +305,10 @@ bool MsckfVio::initialize() { // Initialize the chi squared test table with confidence // level 0.95. - for (int i = 1; i < 100; ++i) { + 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.2); + boost::math::quantile(chi_squared_dist, 0.05); } if (!createRosIO()) return false; @@ -407,6 +408,10 @@ void MsckfVio::imageCallback( // Return if the gravity vector has not been set. if (!is_gravity_set) return; + // stop playing bagfile if printing images + if(STREAMPAUSE) + nh.setParam("/play_bag", false); + // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -443,6 +448,7 @@ void MsckfVio::imageCallback( double imu_processing_time = ( ros::Time::now()-start_time).toSec(); + //cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); if(PHOTOMETRIC) @@ -455,6 +461,8 @@ void MsckfVio::imageCallback( double state_augmentation_time = ( ros::Time::now()-start_time).toSec(); + + //cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -462,23 +470,29 @@ void MsckfVio::imageCallback( double add_observations_time = ( ros::Time::now()-start_time).toSec(); + + //cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); double manage_moving_window_time = ( ros::Time::now()-start_time).toSec(); + + //cout << "4" << endl; // Perform measurement update if necessary. start_time = ros::Time::now(); removeLostFeatures(); double remove_lost_features_time = ( ros::Time::now()-start_time).toSec(); + //cout << "5" << endl; start_time = ros::Time::now(); - pruneLastCamStateBuffer(); + pruneCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); + //cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -509,6 +523,8 @@ void MsckfVio::imageCallback( publish_time, publish_time/processing_time); } + if(STREAMPAUSE) + nh.setParam("/play_bag", true); return; } @@ -1428,15 +1444,7 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd& H_x, VectorXd& r) { - // stop playing bagfile if printing images - if(PRINTIMAGES) - { - std::cout << "stopped playpack" << std::endl; - nh.setParam("/play_bag", false); - } - const auto& feature = map_server[feature_id]; - // Check how many camera states in the provided camera // id camera has actually seen this feature. @@ -1538,13 +1546,8 @@ void MsckfVio::PhotometricFeatureJacobian( myfile.close(); cout << "---------- LOGGED -------- " << endl; cvWaitKey(0); + } - } - if(PRINTIMAGES) - { - std::cout << "resume playback" << std::endl; - nh.setParam("/play_bag", true); - } return; } @@ -1619,12 +1622,6 @@ void MsckfVio::featureJacobian( MatrixXd& H_x, VectorXd& r) { // stop playing bagfile if printing images - if(PRINTIMAGES) - { - std::cout << "stopped playpack" << std::endl; - nh.setParam("/play_bag", false); - } - const auto& feature = map_server[feature_id]; @@ -1686,9 +1683,6 @@ void MsckfVio::featureJacobian( H_x = A.transpose() * H_xj; r = A.transpose() * r_j; - - cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r) << endl; - // stop playing bagfile if printing images if(PRINTIMAGES) { @@ -1698,8 +1692,6 @@ void MsckfVio::featureJacobian( myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; myfile.close(); cout << "---------- LOGGED -------- " << endl; - std::cout << "stopped playpack" << std::endl; - nh.setParam("/play_bag", true); } return; @@ -1708,8 +1700,8 @@ void MsckfVio::featureJacobian( void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { - if (H.rows() == 0 || r.rows() == 0) return; - + if (H.rows() == 0 || r.rows() == 0) + return; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -1718,7 +1710,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { if(PHOTOMETRIC) augmentationSize = 7; - /* + // QR decomposition to make stuff faster if (H.rows() > H.cols()) { // Convert H to a sparse matrix. SparseMatrix H_sparse = H.sparseView(); @@ -1736,29 +1728,24 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { H_thin = H_temp.topRows(21+state_server.cam_states.size()*augmentationSize); r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize); - //HouseholderQR qr_helper(H); - //MatrixXd Q = qr_helper.householderQ(); - //MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6); - - //H_thin = Q1.transpose() * H; - //r_thin = Q1.transpose() * r; } else { H_thin = H; r_thin = r; } - */ + // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; - MatrixXd S = H*P*H.transpose() + + MatrixXd S = H_thin*P*H_thin.transpose() + Feature::observation_noise*MatrixXd::Identity( - H.rows(), H.rows()); + H_thin.rows(), H_thin.rows()); //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); - MatrixXd K_transpose = S.ldlt().solve(H*P); + cout << "inverting: " << S.rows() << ":" << S.cols() << endl; + MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); - // Compute the error of the state. VectorXd delta_x = K * r; + cout << "update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1800,7 +1787,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } // Update state covariance. - MatrixXd I_KH = MatrixXd::Identity(K.rows(), H.cols()) - K*H; + MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin; //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() + // K*K.transpose()*Feature::observation_noise; state_server.state_cov = I_KH*state_server.state_cov; @@ -1814,7 +1801,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { } bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { - return 1; MatrixXd P1 = H * state_server.state_cov * H.transpose(); @@ -1827,7 +1813,7 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) chi_squared_test_table[dof] << endl; if (chi_squared_test_table[dof] == 0) - return true; + return false; if (gamma < chi_squared_test_table[dof]) { //cout << "passed" << endl; return true; @@ -1929,11 +1915,6 @@ void MsckfVio::removeLostFeatures() { 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(); - cout << "passed" << endl; - } - else - { - cout << "rejected" << endl; } // Put an upper bound on the row size of measurement Jacobian, @@ -1941,6 +1922,8 @@ void MsckfVio::removeLostFeatures() { if (stack_cntr > 1500) break; } + cout << "processed features: " << processed_feature_ids.size() << endl; + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2000,6 +1983,7 @@ void MsckfVio::findRedundantCamStates(vector& rm_cam_state_ids) { void MsckfVio::pruneLastCamStateBuffer() { + if (state_server.cam_states.size() < max_cam_state_size) return; @@ -2058,7 +2042,7 @@ void MsckfVio::pruneLastCamStateBuffer() MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; - + int pruned_cntr = 0; for (auto& item : map_server) { auto& feature = item.second; @@ -2077,25 +2061,27 @@ void MsckfVio::pruneLastCamStateBuffer() else 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())) { + if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) { 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(); - cout << "passed" << endl; - } - else - { - cout << "rejected" << endl; + pruned_cntr++; } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); - // Perform measurement update. - measurementUpdate(H_x, r); - + if(pruned_cntr != 0) + { + cout << "pruned features: " << pruned_cntr << endl; + + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + + // Perform measurement update. + measurementUpdate(H_x, r); + } + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -2137,6 +2123,7 @@ void MsckfVio::pruneLastCamStateBuffer() void MsckfVio::pruneCamStateBuffer() { + if (state_server.cam_states.size() < max_cam_state_size) return; @@ -2228,11 +2215,6 @@ void MsckfVio::pruneCamStateBuffer() { 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(); - cout << "passed" << endl; - } - else - { - cout << "rejected" << endl; } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id);