diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 435f22f..b818dc6 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -1024,8 +1024,8 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N) cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3); cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3); - xderImage/=8; - yderImage/=8; + xderImage/=8.; + yderImage/=8.; cv::convertScaleAbs(xderImage, abs_xderImage); cv::convertScaleAbs(yderImage, abs_yderImage); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index f8e7d62..995c5a3 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -212,7 +212,7 @@ class MsckfVio { const FeatureIDType& feature_id, Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r); - void PhotometricMeasurementJacobian( + bool PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, Eigen::MatrixXd& H_x, @@ -224,7 +224,7 @@ class MsckfVio { const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); - void PhotometricFeatureJacobian( + bool PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, Eigen::MatrixXd& H_x, Eigen::VectorXd& r); @@ -263,6 +263,11 @@ class MsckfVio { // Chi squared test table. static std::map chi_squared_test_table; + + // change in position + Eigen::Vector3d delta_position; + Eigen::Vector3d delta_orientation; + // State vector StateServer state_server; StateServer photometric_state_server; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index b7e9365..b3a66e5 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,7 +18,7 @@ output="screen"> - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 4dacfce..f9178c2 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -412,8 +412,6 @@ void MsckfVio::imageCallback( return; } - - // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -433,10 +431,6 @@ void MsckfVio::imageCallback( batchImuProcessing(feature_msg->header.stamp.toSec()); - // save true state in seperate state vector - - batchTruthProcessing(feature_msg->header.stamp.toSec()); - if(GROUNDTRUTH) { state_server.imu_state.position = true_state_server.imu_state.position; @@ -489,6 +483,10 @@ void MsckfVio::imageCallback( double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); + // save true state in seperate state vector and calculate relative error in change + batchTruthProcessing(feature_msg->header.stamp.toSec()); + + cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); @@ -808,6 +806,8 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { // Counter how many IMU msgs in the buffer are used. int used_truth_msg_cntr = 0; + const IMUState old_true_state = true_state_server.imu_state; + for (const auto& truth_msg : truth_msg_buffer) { double truth_time = truth_msg.header.stamp.toSec(); if (truth_time < true_state_server.imu_state.time) { @@ -839,6 +839,22 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) { truth_msg_buffer.erase(truth_msg_buffer.begin(), truth_msg_buffer.begin()+used_truth_msg_cntr); + // calculate error in position + + // calculate error in change + + // calculate change + Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position; + Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation); + Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2); + Eigen::Vector3d error_delta_position = delta_true_position - delta_position; + Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation; + + double dx = (error_delta_position[0]/delta_true_position[0]); + double dy = (error_delta_position[1]/delta_true_position[1]); + double dz = (error_delta_position[2]/delta_true_position[2]); + cout << "relative pos error: " << sqrt(dx*dx + dy*dy + dz*dz) * 100 << "%" << endl; + } void MsckfVio::processTruthtoIMU(const double& time, @@ -1463,7 +1479,7 @@ void MsckfVio::twodotFeatureJacobian( -void MsckfVio::PhotometricMeasurementJacobian( +bool MsckfVio::PhotometricMeasurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) @@ -1531,6 +1547,7 @@ void MsckfVio::PhotometricMeasurementJacobian( int count = 0; double dx, dy; + cout << "patching" << endl; for (auto point : feature.anchorPatch_3d) { //cout << "____feature-measurement_____\n" << endl; @@ -1538,6 +1555,11 @@ void MsckfVio::PhotometricMeasurementJacobian( cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point); cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point); + if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1) + { + cout << "skipped" << endl; + return false; + } //add observation photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame)); @@ -1597,8 +1619,9 @@ void MsckfVio::PhotometricMeasurementJacobian( count++; } + cout << "done" << endl; MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7); - MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1); + MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1); // set anchor Jakobi // get position of anchor in cam states @@ -1616,15 +1639,18 @@ void MsckfVio::PhotometricMeasurementJacobian( H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl; // set ones for irradiance bias - H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); + // H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N); // set irradiance error Block - H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N); + H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N); // TODO make this calculation more fluent - for(int i = 0; i< N*N; i++) + + /*for(int i = 0; i< N*N; i++) H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i]; - H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho; + */ + + H_yl.block(0, N*N, N*N, 1) = -H_rho; H_x = H_xl; H_y = H_yl; @@ -1636,15 +1662,16 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); + feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss); //feature.VisualizeKernel(cam_state, cam_state_id, cam0); } - return; + cout << "returning" << endl; + return true; } -void MsckfVio::PhotometricFeatureJacobian( +bool MsckfVio::PhotometricFeatureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, MatrixXd& H_x, VectorXd& r) @@ -1669,7 +1696,7 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size, 21+state_server.cam_states.size()*7); - MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1); + MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1); VectorXd r_i = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1679,7 +1706,8 @@ void MsckfVio::PhotometricFeatureJacobian( MatrixXd H_yl; Eigen::VectorXd r_l = VectorXd::Zero(N*N); - PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l); + if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l)) + continue; auto cam_state_iter = state_server.cam_states.find(cam_id); int cam_state_cntr = std::distance( @@ -1691,7 +1719,11 @@ void MsckfVio::PhotometricFeatureJacobian( r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; } - + if(stack_cntr == 0) + { + cout << "skip feature" << endl; + return false; + } // Project the residual and Jacobians onto the nullspace // of H_yj. @@ -1754,12 +1786,20 @@ void MsckfVio::PhotometricFeatureJacobian( << "# columns: " << A_null_space.cols() << "\n" << A_null_space << endl; + myfile << "# name: P\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.rows() << "\n" + << state_server.state_cov << endl; + + myfile.close(); cout << "---------- LOGGED -------- " << endl; - cvWaitKey(0); } - return; + cout << "donefeature" << endl; + + return true; } void MsckfVio::measurementJacobian( @@ -1929,6 +1969,13 @@ void MsckfVio::featureJacobian( << "# columns: " << A.cols() << "\n" << A << endl; + myfile << "# name: P\n" + << "# type: matrix\n" + << "# rows: " << state_server.state_cov.rows() << "\n" + << "# columns: " << state_server.state_cov.rows() << "\n" + << state_server.state_cov << endl; + + myfile.close(); } @@ -1970,19 +2017,50 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; + MatrixXd S = H_thin*P*H_thin.transpose() + Feature::observation_noise*MatrixXd::Identity( H_thin.rows(), H_thin.rows()); + + + //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P); 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 << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; - // Update the IMU state. if(FILTER != 0) return; + + if(PRINTIMAGES) + { + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; + + myfile.close(); + } + delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); + delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); + + // Update the IMU state. const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -2046,7 +2124,7 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // complexity as in Equation (28), (29). MatrixXd H_thin; VectorXd r_thin; -/* + if (H.rows() > H.cols()) { // Convert H to a sparse matrix. SparseMatrix H_sparse = H.sparseView(); @@ -2070,10 +2148,10 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { //H_thin = Q1.transpose() * H; //r_thin = Q1.transpose() * r; - } else {*/ + } else { H_thin = H; r_thin = r; - //} + } @@ -2093,6 +2171,9 @@ void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { // Update the IMU state. if (FILTER != 2) return; + delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); + delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); + const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -2196,6 +2277,33 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Update the IMU state. if (FILTER != 1) return; + cout << "here" << endl; + + if(PRINTIMAGES) + { + //octave + ofstream myfile; + + myfile.open("/home/raphael/dev/octave/measurement2octave"); + myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST \n" + << "# name: K\n" + << "# type: matrix\n" + << "# rows: " << K.rows() << "\n" + << "# columns: " << K.cols() << "\n" + << K << endl; + + myfile << "# name: r\n" + << "# type: matrix\n" + << "# rows: " << r.rows() << "\n" + << "# columns: " << r.cols() << "\n" + << r << endl; + + myfile.close(); + } + + delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]); + delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]); + const VectorXd& delta_x_imu = delta_x.head<21>(); if (//delta_x_imu.segment<3>(0).norm() > 0.15 || @@ -2374,8 +2482,18 @@ void MsckfVio::removeLostFeatures() { cout << "measuring" << endl; - PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); - cout << "norm" << endl; + if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true); + { + cout << "p gating" << endl; + + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + 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(); + } + } + + cout << "donephoto" << endl; featureJacobian(feature.id, cam_state_ids, H_xj, r_j); cout << "two" << endl; twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); @@ -2387,11 +2505,6 @@ void MsckfVio::removeLostFeatures() { r.segment(stack_cntr, r_j.rows()) = r_j; stack_cntr += H_xj.rows(); } - if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { - 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(); - } 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; @@ -2403,17 +2516,23 @@ void MsckfVio::removeLostFeatures() { //if (stack_cntr > 1500) break; } + if(pstack_cntr) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + + photometricMeasurementUpdate(pH_x, pr); + } + cout << "resizing" << endl; H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); // Perform the measurement update step. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); twoMeasurementUpdate(twoH_x, twor); // Remove all processed features from the map. @@ -2553,7 +2672,18 @@ void MsckfVio::pruneLastCamStateBuffer() cout << "measuring" << endl; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true); + { + cout << "p gating" << endl; + + if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { + 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(); + } + } + + cout << "norm" << endl; featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); cout << "two" << endl; @@ -2566,11 +2696,7 @@ void MsckfVio::pruneLastCamStateBuffer() stack_cntr += H_xj.rows(); pruned_cntr++; } - if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { - 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(); - } + 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; @@ -2580,12 +2706,17 @@ void MsckfVio::pruneLastCamStateBuffer() feature.observations.erase(cam_id); } + if (pstack_cntr) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + + photometricMeasurementUpdate(pH_x, pr); + } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); // Perform measurement update. twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); @@ -2594,7 +2725,6 @@ void MsckfVio::pruneLastCamStateBuffer() measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); twoMeasurementUpdate(twoH_x, twor); //reduction int cam_sequence = std::distance(state_server.cam_states.begin(), @@ -2729,28 +2859,28 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; cout << "measuring" << endl; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); + if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true) + { + cout << "p gating" << endl; + if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { + 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(); + } + } cout << "norm" << endl; featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); cout << "two" << endl; twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j); cout << "gating" << endl; - PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); - 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 (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { - 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(); - } + 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; @@ -2762,16 +2892,22 @@ void MsckfVio::pruneCamStateBuffer() { } + if(pstack_cntr > 0) + { + pH_x.conservativeResize(pstack_cntr, pH_x.cols()); + pr.conservativeResize(pstack_cntr); + + photometricMeasurementUpdate(pH_x, pr); + } + H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - pH_x.conservativeResize(pstack_cntr, pH_x.cols()); - pr.conservativeResize(pstack_cntr); + twoH_x.conservativeResize(twostack_cntr, twoH_x.cols()); twor.conservativeResize(twostack_cntr); // Perform measurement update. measurementUpdate(H_x, r); - photometricMeasurementUpdate(pH_x, pr); twoMeasurementUpdate(twoH_x, twor); //reduction for (const auto& cam_id : rm_cam_state_ids) {