diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 1d9d92e..75bd36b 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -126,7 +126,7 @@ struct Feature { */ bool initializeAnchor( - const CameraCalibration& cam); + const CameraCalibration& cam, int N); /* @@ -491,12 +491,10 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, //@test center projection must always be initial feature projection bool Feature::initializeAnchor( - const CameraCalibration& cam) + const CameraCalibration& cam, int N) { //initialize patch Size - //TODO make N size a ros parameter - int N = 3; int n = (int)(N-1)/2; auto anchor = observations.begin(); diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 04efc65..ba88474 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -206,6 +206,9 @@ class MsckfVio { bool nan_flag; + // Patch size for Photometry + int N; + // Chi squared test table. static std::map chi_squared_test_table; diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch index edd0657..9c95b1c 100644 --- a/launch/msckf_vio_tum.launch +++ b/launch/msckf_vio_tum.launch @@ -20,6 +20,7 @@ + diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index c58dc27..c87bfd4 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -127,6 +127,10 @@ bool MsckfVio::loadParameters() { nh.param("initial_covariance/irradiance_frame_bias", irradiance_frame_bias, 0.1); + // Get the photometric patch size + nh.param("patch_size_n", + N, 3); + // get camera information (used for back projection) nh.param("cam0/distortion_model", cam0.distortion_model, string("radtan")); @@ -288,7 +292,7 @@ bool MsckfVio::initialize() { for (int i = 1; i < 100; ++i) { boost::math::chi_squared chi_squared_dist(i); chi_squared_test_table[i] = - boost::math::quantile(chi_squared_dist, 0.05); + boost::math::quantile(chi_squared_dist, 0.2); } if (!createRosIO()) return false; @@ -979,9 +983,6 @@ void MsckfVio::PhotometricMeasurementJacobian( Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation); const Vector3d& t_c0_w = cam_state.position; - //temp N - const int N = 3; - // Cam1 pose. Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0; @@ -1128,7 +1129,6 @@ void MsckfVio::PhotometricFeatureJacobian( const auto& feature = map_server[feature_id]; - int N = 3; // Check how many camera states in the provided camera // id camera has actually seen this feature. vector valid_cam_state_ids(0); @@ -1346,7 +1346,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) return; - cout << "decomposition..."; + cout << "Updating..."; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -1379,8 +1379,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { H_thin = H; r_thin = r; } - cout << "done" << endl; - cout << "computing K..."; + // Compute the Kalman gain. const MatrixXd& P = state_server.state_cov; MatrixXd S = H_thin*P*H_thin.transpose() + @@ -1390,8 +1389,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { MatrixXd K_transpose = S.ldlt().solve(H_thin*P); MatrixXd K = K_transpose.transpose(); - cout << "done" << endl; - // Compute the error of the state. VectorXd delta_x = K * r_thin; @@ -1457,15 +1454,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) MatrixXd P2 = Feature::observation_noise * MatrixXd::Identity(H.rows(), H.rows()); - //cout << "H: \n" << H << endl; - //cout << "cov: \n" << state_server.state_cov << endl; - //cout << "P1: \n" << P1 << endl; - //cout << "solv: \n" << (P1+P2).ldlt().solve(r) << endl; - double gamma = r.transpose() * (P1+P2).ldlt().solve(r); cout << dof << " " << gamma << " " << - chi_squared_test_table[dof] << " "; + chi_squared_test_table[dof] << endl; if (gamma < chi_squared_test_table[dof]) { //cout << "passed" << endl; @@ -1480,11 +1472,14 @@ void MsckfVio::removeLostFeatures() { // Remove the features that lost track. // BTW, find the size the final Jacobian matrix and residual vector. int jacobian_row_size = 0; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; + + vector invalid_feature_ids(0); vector processed_feature_ids(0); - int N = 3; - for (auto iter = map_server.begin(); iter != map_server.end(); ++iter) { // Rename the feature to be checked. @@ -1513,7 +1508,7 @@ void MsckfVio::removeLostFeatures() { } if(!feature.is_anchored) { - if(!feature.initializeAnchor(cam0)) + if(!feature.initializeAnchor(cam0, N)) { invalid_feature_ids.push_back(feature.id); continue; @@ -1540,10 +1535,6 @@ void MsckfVio::removeLostFeatures() { // Return if there is no lost feature to be processed. if (processed_feature_ids.size() == 0) return; - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; - MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+augmentationSize*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); @@ -1569,11 +1560,11 @@ 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 << "approved chi" << endl; + cout << "passed" << endl; } else { - cout << "rejected chi" << endl; + cout << "rejected" << endl; } // Put an upper bound on the row size of measurement Jacobian, @@ -1647,10 +1638,12 @@ void MsckfVio::pruneCamStateBuffer() { vector rm_cam_state_ids(0); findRedundantCamStates(rm_cam_state_ids); - int N = 3; - // Find the size of the Jacobian matrix. int jacobian_row_size = 0; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; + for (auto& item : map_server) { auto& feature = item.second; // Check how many camera states to be removed are associated @@ -1686,7 +1679,7 @@ void MsckfVio::pruneCamStateBuffer() { } if(!feature.is_anchored) { - if(!feature.initializeAnchor(cam0)) + if(!feature.initializeAnchor(cam0, N)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); @@ -1702,8 +1695,9 @@ void MsckfVio::pruneCamStateBuffer() { //cout << "jacobian row #: " << jacobian_row_size << endl; // Compute the Jacobian and residual. - MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, - 21+6*state_server.cam_states.size()); + MatrixXd H_xj; + VectorXd r_j; + 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; for (auto& item : map_server) { @@ -1719,9 +1713,6 @@ void MsckfVio::pruneCamStateBuffer() { if (involved_cam_state_ids.size() == 0) continue; - MatrixXd H_xj; - VectorXd r_j; - if(PHOTOMETRIC) PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); else @@ -1731,29 +1722,23 @@ 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 << "made gating test" << endl; + cout << "passed" << endl; } else { - cout << "failed gating test" << endl; + cout << "rejected" << endl; } for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); } - cout << "resize" << endl; - H_x.conservativeResize(stack_cntr, H_x.cols()); r.conservativeResize(stack_cntr); - cout << "done" << endl; // Perform measurement update. measurementUpdate(H_x, r); - - int augmentationSize = 6; - if(PHOTOMETRIC) - augmentationSize = 7; + //reduction for (const auto& cam_id : rm_cam_state_ids) { int cam_sequence = std::distance(state_server.cam_states.begin(), state_server.cam_states.find(cam_id));