diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index c7fdcea..5be54eb 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -202,7 +202,7 @@ class MsckfVio { Eigen::Vector4d& r); // This function computes the Jacobian of all measurements viewed // in the given camera states of this feature. - void featureJacobian( + bool featureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); @@ -246,7 +246,7 @@ class MsckfVio { Eigen::VectorXd& r); - void twodotFeatureJacobian( + bool twodotFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); @@ -283,6 +283,7 @@ class MsckfVio { bool nan_flag; bool play; double last_time_bound; + double time_offset; // Patch size for Photometry int N; diff --git a/launch/image_processor_tinytum.launch b/launch/image_processor_tinytum.launch index ec463d9..efe6e7c 100644 --- a/launch/image_processor_tinytum.launch +++ b/launch/image_processor_tinytum.launch @@ -18,7 +18,7 @@ - + @@ -27,9 +27,9 @@ - - - + + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 5e5e3b3..58e2ca2 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -17,8 +17,7 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + @@ -33,7 +32,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index f6fa93f..d9e66f3 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.05; +double Feature::observation_noise = 0.01; Feature::OptimizationConfig Feature::optimization_config; map MsckfVio::chi_squared_test_table; @@ -425,6 +425,7 @@ void MsckfVio::imageCallback( // the origin. if (is_first_img) { is_first_img = false; + time_offset = feature_msg->header.stamp.toSec(); state_server.imu_state.time = feature_msg->header.stamp.toSec(); } static double max_processing_time = 0.0; @@ -786,7 +787,7 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) { void MsckfVio::calcErrorState() { - // imu error + // imu "error" err_state_server.imu_state.id = state_server.imu_state.id; err_state_server.imu_state.time = state_server.imu_state.time; @@ -832,9 +833,9 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. int used_truth_msg_cntr = 0; - + double truth_time; for (const auto& truth_msg : truth_msg_buffer) { - double truth_time = truth_msg.header.stamp.toSec(); + truth_time = truth_msg.header.stamp.toSec(); if (truth_time < true_state_server.imu_state.time) { ++used_truth_msg_cntr; continue; @@ -854,6 +855,7 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Execute process model. processTruthtoIMU(truth_time, m_rotation, m_translation); ++used_truth_msg_cntr; + } last_time_bound = time_bound; @@ -864,6 +866,31 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); + std::ofstream logfile; + + int FileHandler; + char FileBuffer[1024]; + float load = 0; + + FileHandler = open("/proc/loadavg", O_RDONLY); + if( FileHandler >= 0) { + auto file = read(FileHandler, FileBuffer, sizeof(FileBuffer) - 1); + sscanf(FileBuffer, "%f", &load); + close(FileHandler); + } + + auto gt = true_state_server.imu_state.position; + auto gr = true_state_server.imu_state.orientation; + logfile.open("/home/raphael/dev/MSCKF_ws/bag/log.txt", std::ios_base::app); + //ros time, cpu load , ground truth, estimation, ros time + logfile << true_state_server.imu_state.time - time_offset << "; " << load << "; "; + logfile << gt[0] << "; " << gt[1] << "; " << gt[2] << "; " << gr[0] << "; " << gr[1] << "; " << gr[2] << "; " << gr[3] << ";"; + + // estimation + auto et = state_server.imu_state.position; + auto er = state_server.imu_state.orientation; + logfile << et[0] << "; " << et[1] << "; " << et[2] << "; " << er[0] << "; " << er[1] << "; " << er[2] << "; " << er[3] << "; \n" << endl;; + logfile.close(); /* // calculate change delta_position = state_server.imu_state.position - old_imu_state.position; @@ -1459,12 +1486,14 @@ void MsckfVio::twodotMeasurementJacobian( return; } -void MsckfVio::twodotFeatureJacobian( +bool MsckfVio::twodotFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { - + if(FILTER != 2) + return false; + const auto& feature = map_server[feature_id]; @@ -1561,7 +1590,7 @@ void MsckfVio::twodotFeatureJacobian( std::cout << "resume playback" << std::endl; nh.setParam("/play_bag", true); } - return; + return true; } @@ -1891,6 +1920,8 @@ bool MsckfVio::PhotometricFeatureJacobian( const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { + if(FILTER != 1) + return false; const auto& feature = map_server[feature_id]; @@ -2101,12 +2132,15 @@ void MsckfVio::measurementJacobian( return; } -void MsckfVio::featureJacobian( +bool MsckfVio::featureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) { - // stop playing bagfile if printing images + + + if(FILTER != 0) + return false; const auto& feature = map_server[feature_id]; @@ -2239,7 +2273,7 @@ void MsckfVio::featureJacobian( myfile.close(); } - return; + return true; } @@ -2625,37 +2659,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof, double gamma = r.transpose() * (P1+P2).ldlt().solve(r); - 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]) { // cout << "passed" << endl; - if(filter == 1) cout << "gate: " << dof << " " << gamma << " " << chi_squared_test_table[dof] << endl; return true; @@ -2769,20 +2776,25 @@ void MsckfVio::removeLostFeatures() { } } - featureJacobian(feature.id, cam_state_ids, H_xj, r_j); - twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_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(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { - twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; - twor.segment(twostack_cntr, twor_j.rows()) = twor_j; - twostack_cntr += twoH_xj.rows(); + if(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(twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j)) + { + if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + 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. @@ -2795,16 +2807,22 @@ void MsckfVio::removeLostFeatures() { pr.conservativeResize(pstack_cntr); photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); + + if(stack_cntr) + { + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + measurementUpdate(H_x, r); - twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); - twor.conservativeResize(twostack_cntr); - - // Perform the measurement update step. - measurementUpdate(H_x, r); - twoMeasurementUpdate(twoH_x, twor); - + } + if(twostack_cntr) + { + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); + twoMeasurementUpdate(twoH_x, twor); + } + + // Remove all processed features from the map. for (const auto& feature_id : processed_feature_ids) map_server.erase(feature_id); @@ -2951,20 +2969,23 @@ void MsckfVio::pruneLastCamStateBuffer() } } - featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); - twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); - + if(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())) { - 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(); - pruned_cntr++; + 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(); + pruned_cntr++; } - - if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { - twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; - twor.segment(twostack_cntr, twor_j.rows()) = twor_j; - twostack_cntr += twoH_xj.rows(); + } + + if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j)) + { + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + twor.segment(twostack_cntr, twor_j.rows()) = twor_j; + twostack_cntr += twoH_xj.rows(); + } } for (const auto& cam_id : involved_cam_state_ids) @@ -2978,16 +2999,20 @@ void MsckfVio::pruneLastCamStateBuffer() photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); - - twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); - twor.conservativeResize(twostack_cntr); - - // Perform the measurement update step. - measurementUpdate(H_x, r); - twoMeasurementUpdate(twoH_x, twor); + if(stack_cntr) + { + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + measurementUpdate(H_x, r); + } + if(twostack_cntr) + { + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); + twoMeasurementUpdate(twoH_x, twor); + } + //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(rm_cam_state_id)); @@ -3128,21 +3153,22 @@ void MsckfVio::pruneCamStateBuffer() { 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); - - if (gatingTest(H_xj, r_j, involved_cam_state_ids.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(); + if(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())) { + 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(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { - twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; - twor.segment(twostack_cntr, twor_j.rows()) = twor_j; - twostack_cntr += twoH_xj.rows(); + if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j)) + { + if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) { + twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj; + 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); @@ -3156,16 +3182,20 @@ void MsckfVio::pruneCamStateBuffer() { photometricMeasurementUpdate(pH_x, pr); } - H_x.conservativeResize(stack_cntr, H_x.cols()); - r.conservativeResize(stack_cntr); - - twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); - twor.conservativeResize(twostack_cntr); - - // Perform the measurement update step. - measurementUpdate(H_x, r); - twoMeasurementUpdate(twoH_x, twor); - + if(stack_cntr) + { + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + measurementUpdate(H_x, r); + } + + if(stack_cntr) + { + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); + twor.conservativeResize(twostack_cntr); + twoMeasurementUpdate(twoH_x, twor); + } + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(),