From 1d6100ed13c4e5280bc55bf39f7f4fab6f3b0754 Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Tue, 25 Jun 2019 19:05:53 +0200 Subject: [PATCH] added live toggle for photometric --- include/msckf_vio/feature.hpp | 2 -- launch/msckf_vio_euroc.launch | 10 ++++++ launch/msckf_vio_tinytum.launch | 17 +++++----- launch/msckf_vio_tum.launch | 4 +-- src/msckf_vio.cpp | 55 +++++++++++++++++++++------------ 5 files changed, 55 insertions(+), 33 deletions(-) diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 466d1e9..405f2d4 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -1008,8 +1008,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal bool Feature::initializeAnchor(const CameraCalibration& cam, int N) { - - //initialize patch Size int n = (int)(N-1)/2; diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 896feff..c9e9923 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -17,6 +17,16 @@ args='standalone msckf_vio/MsckfVioNodelet' output="screen"> + + + + + + + + + + diff --git a/launch/msckf_vio_tinytum.launch b/launch/msckf_vio_tinytum.launch index 94deb7c..44b3312 100644 --- a/launch/msckf_vio_tinytum.launch +++ b/launch/msckf_vio_tinytum.launch @@ -6,7 +6,7 @@ default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/> - + @@ -18,14 +18,14 @@ output="screen"> - + - - + + - + @@ -64,16 +64,13 @@ - - + + - - - diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index 0240de3..a5b65c7 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -18,10 +18,10 @@ output="screen"> - + - + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index b6245e2..a3d5dd6 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -412,6 +412,8 @@ void MsckfVio::imageCallback( if(STREAMPAUSE) nh.setParam("/play_bag", false); + + nh.param("PHOTOMETRIC", PHOTOMETRIC, false); // Start the system if the first image is received. // The frame where the first image is received will be // the origin. @@ -448,7 +450,7 @@ void MsckfVio::imageCallback( double imu_processing_time = ( ros::Time::now()-start_time).toSec(); - //cout << "1" << endl; + // cout << "1" << endl; // Augment the state vector. start_time = ros::Time::now(); //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); @@ -457,7 +459,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "2" << endl; + // cout << "2" << endl; // Add new observations for existing features or new // features in the map server. start_time = ros::Time::now(); @@ -466,7 +468,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "3" << endl; + // cout << "3" << endl; // Add new images to moving window start_time = ros::Time::now(); manageMovingWindow(cam0_img, cam1_img, feature_msg); @@ -474,20 +476,20 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); - //cout << "4" << endl; + // 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; + // cout << "5" << endl; start_time = ros::Time::now(); - pruneLastCamStateBuffer(); + pruneCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); - //cout << "6" << endl; + // cout << "6" << endl; // Publish the odometry. start_time = ros::Time::now(); publish(feature_msg->header.stamp); @@ -1368,7 +1370,9 @@ void MsckfVio::PhotometricMeasurementJacobian( } 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+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 @@ -1389,13 +1393,17 @@ void MsckfVio::PhotometricMeasurementJacobian( 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++) 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, 0, N*N, N*N) = Eigen::MatrixXd::Identity(N*N, N*N); + H_yl.block(0, N*N, N*N, 1) = -H_rho; + H_x = H_xl; H_y = H_yl; @@ -1439,7 +1447,10 @@ 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+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; @@ -1460,6 +1471,8 @@ void MsckfVio::PhotometricFeatureJacobian( H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl; r_i.segment(stack_cntr, N*N) = r_l; stack_cntr += N*N; + + } // Project the residual and Jacobians onto the nullspace @@ -1798,8 +1811,10 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) { - if (H.rows() == 0 || r.rows() == 0) + 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; @@ -1840,10 +1855,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r // Compute the error of the state. VectorXd delta_x = K * r; - // cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; - // cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // Update the IMU state. + + if (not PHOTOMETRIC) return; + + cout << "msc veloci: " << delta_x[6] << ", " << delta_x[7] << ", " << delta_x[8] << endl; + cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; const VectorXd& delta_x_imu = delta_x.head<21>(); @@ -1900,6 +1918,7 @@ void MsckfVio::photometricMeasurementUpdate(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(); @@ -1908,8 +1927,8 @@ 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 << "gate: " << dof << " " << gamma << " " << + chi_squared_test_table[dof] << endl; if (chi_squared_test_table[dof] == 0) return false; @@ -2008,12 +2027,12 @@ void MsckfVio::removeLostFeatures() { PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); 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 (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; @@ -2180,7 +2199,6 @@ void MsckfVio::pruneLastCamStateBuffer() feature.observations.erase(cam_id); } - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); @@ -2188,7 +2206,6 @@ void MsckfVio::pruneLastCamStateBuffer() pr.conservativeResize(pstack_cntr); // Perform measurement update. - measurementUpdate(H_x, r); photometricMeasurementUpdate(pH_x, pr);