diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index adbf6cc..4584c57 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 196c72e..1e8c9a5 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -17,8 +17,7 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> - - + @@ -35,7 +34,7 @@ - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index bfbe609..437a98c 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; @@ -427,10 +427,8 @@ void MsckfVio::imageCallback( // the origin. if (is_first_img) { is_first_img = false; - //cam0.intrinsics *=SCALE; - //cam1.intrinsics *=SCALE; - //cam0.resolution *=SCALE; - //cam1.resolution *=SCALE; + + time_offset = feature_msg->header.stamp.toSec(); state_server.imu_state.time = feature_msg->header.stamp.toSec(); } static double max_processing_time = 0.0; @@ -552,27 +550,13 @@ void MsckfVio::manageMovingWindow( cv::Mat new_cam0; cv::Mat new_cam1; - //cam0.intrinsics *=float(1./SCALE); - //cam1.intrinsics *=float(1./SCALE); - //cam0.resolution *=float(1./SCALE); - //cam1.resolution *=float(1./SCALE); + image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); - //cam0.intrinsics *=SCALE; - //cam1.intrinsics *=SCALE; - //cam0.resolution *=SCALE; - //cam1.resolution *=SCALE; - //resize - - int SCALE = 1;yy - cv::Mat new_cam02; - cv::Mat new_cam12; - cv::resize(new_cam0, new_cam02, cv::Size(), SCALE, SCALE); - cv::resize(new_cam1, new_cam12, cv::Size(), SCALE, SCALE); // save image information into moving window - cam0.moving_window[state_server.imu_state.id].image = new_cam02.clone(); - cam1.moving_window[state_server.imu_state.id].image = new_cam12.clone(); + cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone(); + cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone(); cv::Mat xder; cv::Mat yder; @@ -809,7 +793,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; @@ -855,9 +839,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; @@ -877,6 +861,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; @@ -887,6 +872,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; @@ -1482,12 +1492,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]; @@ -1584,7 +1596,7 @@ void MsckfVio::twodotFeatureJacobian( std::cout << "resume playback" << std::endl; nh.setParam("/play_bag", true); } - return; + return true; } @@ -1912,6 +1924,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]; @@ -2124,12 +2138,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]; @@ -2262,7 +2279,7 @@ void MsckfVio::featureJacobian( myfile.close(); } - return; + return true; } @@ -2648,37 +2665,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; @@ -2792,20 +2782,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. @@ -2818,16 +2813,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); @@ -2974,20 +2975,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) @@ -3001,16 +3005,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)); @@ -3151,21 +3159,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); @@ -3179,16 +3188,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(),