diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 4d1a9e0..1d9d92e 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -441,7 +441,7 @@ bool Feature::VisualizePatch( float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const { - return (float)(image.at(pose.x, pose.y)); + return ((float)image.at(pose.x, pose.y))/256; } cv::Point2f Feature::projectPositionToCamera( @@ -496,7 +496,7 @@ bool Feature::initializeAnchor( //initialize patch Size //TODO make N size a ros parameter - int N = 9; + int N = 3; int n = (int)(N-1)/2; auto anchor = observations.begin(); @@ -551,7 +551,7 @@ bool Feature::initializeAnchor( return false; } for(auto point : vec) - anchorPatch.push_back((double)anchorImage.at((int)point.x,(int)point.y)); + anchorPatch.push_back(PixelIrradiance(point, anchorImage)); // project patch pixel to 3D space for(auto point : und_v) diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index 50e78d1..04efc65 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -204,6 +204,8 @@ class MsckfVio { // Photometry flag bool PHOTOMETRIC; + bool nan_flag; + // Chi squared test table. static std::map chi_squared_test_table; diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index a4a6874..c58dc27 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -55,6 +55,7 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh): is_gravity_set(false), is_first_img(true), image_sub(10), + nan_flag(false), nh(pnh) { return; } @@ -488,6 +489,7 @@ bool MsckfVio::resetCallback( std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) { + cout << "reset" << endl; ROS_WARN("Start resetting msckf vio..."); // Temporarily shutdown the subscribers to prevent the // state from updating. @@ -901,8 +903,10 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) { size_t old_rows = state_server.state_cov.rows(); size_t old_cols = state_server.state_cov.cols(); + MatrixXd temp_cov = state_server.state_cov; + // add 7 for camera state + irradiance bias eta = b_l - state_server.state_cov.conservativeResize(old_rows+7, old_cols+7); + state_server.state_cov.conservativeResizeLike(Eigen::MatrixXd::Zero(old_rows+7, old_cols+7)); // Rename some matrix blocks for convenience. const Matrix& P11 = @@ -918,13 +922,13 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) { J * P11 * J.transpose(); // Add photometry P_eta and surrounding Zeros - state_server.state_cov.block<1, 12>(old_rows+6, 0) = Matrix::Zero(); - state_server.state_cov.block<12, 1>(0, old_cols+6) = Matrix::Zero(); state_server.state_cov(old_rows+6, old_cols+6) = irradiance_frame_bias; + // Fix the covariance to be symmetric MatrixXd state_cov_fixed = (state_server.state_cov + state_server.state_cov.transpose()) / 2.0; state_server.state_cov = state_cov_fixed; + return; } @@ -976,7 +980,7 @@ void MsckfVio::PhotometricMeasurementJacobian( const Vector3d& t_c0_w = cam_state.position; //temp N - const int N = 9; + const int N = 3; // Cam1 pose. Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear(); @@ -1112,7 +1116,7 @@ void MsckfVio::PhotometricMeasurementJacobian( //TODO make this more fluent as well count = 0; for(auto data : photo_r) - r[count++] = data; + r[count++] = data; return; } @@ -1124,7 +1128,7 @@ void MsckfVio::PhotometricFeatureJacobian( const auto& feature = map_server[feature_id]; - int N = 9; + 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); @@ -1194,26 +1198,24 @@ void MsckfVio::PhotometricFeatureJacobian( int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); for(int i = 0; i < singularValues.size(); i++) - if(singularValues[i] < 1e-3) + if(singularValues[i] > 1e-5) sv_size++; int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); MatrixXd A = svd_helper.matrixU().rightCols( - jacobian_row_size-null_space_size); + jacobian_row_size-sv_size); H_x = A.transpose() * H_xi; r = A.transpose() * r_i; - cout << "r\n" << r << endl; - cout << "Hx\n" << H_x << endl; - return; } void MsckfVio::measurementJacobian( const StateIDType& cam_state_id, const FeatureIDType& feature_id, - Matrix& H_x, Matrix& H_f, Vector4d& r) { + Matrix& H_x, Matrix& H_f, Vector4d& r) +{ // Prepare all the required data. const CAMState& cam_state = state_server.cam_states[cam_state_id]; @@ -1275,7 +1277,8 @@ void MsckfVio::measurementJacobian( void MsckfVio::featureJacobian( const FeatureIDType& feature_id, const std::vector& cam_state_ids, - MatrixXd& H_x, VectorXd& r) { + MatrixXd& H_x, VectorXd& r) +{ const auto& feature = map_server[feature_id]; @@ -1322,29 +1325,28 @@ void MsckfVio::featureJacobian( int sv_size = 0; Eigen::VectorXd singularValues = svd_helper.singularValues(); for(int i = 0; i < singularValues.size(); i++) - if(singularValues[i] < 1e-3) + if(singularValues[i] > 1e-5) sv_size++; - int null_space_size = svd_helper.matrixU().cols() - svd_helper.singularValues().size(); - - //cout << "singular values: \n" << svd_helper.singularValues(); - cout << "null_space: " << null_space_size << endl; + int null_space_size = svd_helper.matrixU().cols() - sv_size; MatrixXd A = svd_helper.matrixU().rightCols( - jacobian_row_size - 3); + jacobian_row_size - sv_size); H_x = A.transpose() * H_xj; r = A.transpose() * r_j; + cout << "A: \n" << A.transpose() << endl; return; } -void MsckfVio::measurementUpdate( - const MatrixXd& H, const VectorXd& r) { +void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) { if (H.rows() == 0 || r.rows() == 0) return; + + cout << "decomposition..."; // Decompose the final Jacobian matrix to reduce computational // complexity as in Equation (28), (29). MatrixXd H_thin; @@ -1377,7 +1379,8 @@ void MsckfVio::measurementUpdate( 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() + @@ -1387,6 +1390,8 @@ void MsckfVio::measurementUpdate( 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; @@ -1444,12 +1449,19 @@ void MsckfVio::measurementUpdate( return; } -bool MsckfVio::gatingTest( - const MatrixXd& H, const VectorXd& r, const int& dof) { +bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) { MatrixXd P1 = H * state_server.state_cov * H.transpose(); + + 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 << " " << @@ -1465,13 +1477,14 @@ bool MsckfVio::gatingTest( } 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; 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. @@ -1507,7 +1520,11 @@ void MsckfVio::removeLostFeatures() { } } - jacobian_row_size += 4*feature.observations.size() - 3; + if(PHOTOMETRIC) + //just use max. size, as gets shrunken down after anyway + jacobian_row_size += N*N*feature.observations.size(); + else + jacobian_row_size += 4*feature.observations.size() - 3; processed_feature_ids.push_back(feature.id); } @@ -1523,8 +1540,12 @@ 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+6*state_server.cam_states.size()); + 21+augmentationSize*state_server.cam_states.size()); VectorXd r = VectorXd::Zero(jacobian_row_size); int stack_cntr = 0; @@ -1548,11 +1569,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 << "made gating test" << endl; + cout << "approved chi" << endl; } else { - cout << "failed gating test" << endl; + cout << "rejected chi" << endl; } // Put an upper bound on the row size of measurement Jacobian, @@ -1573,8 +1594,7 @@ void MsckfVio::removeLostFeatures() { return; } -void MsckfVio::findRedundantCamStates( - vector& rm_cam_state_ids) { +void MsckfVio::findRedundantCamStates(vector& rm_cam_state_ids) { // Move the iterator to the key position. auto key_cam_state_iter = state_server.cam_states.end(); @@ -1627,6 +1647,8 @@ 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; for (auto& item : map_server) { @@ -1671,7 +1693,10 @@ void MsckfVio::pruneCamStateBuffer() { continue; } } - jacobian_row_size += 4*involved_cam_state_ids.size() - 3; + if(PHOTOMETRIC) + jacobian_row_size += N*N*involved_cam_state_ids.size(); + else + jacobian_row_size += 4*involved_cam_state_ids.size() - 3; } //cout << "jacobian row #: " << jacobian_row_size << endl; @@ -1716,9 +1741,12 @@ void MsckfVio::pruneCamStateBuffer() { 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);