diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index a47c142..f5a0d1e 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -329,7 +329,6 @@ void Feature::RhoJacobian(const Eigen::Isometry3d& T_c0_ci, double& w) const { - // Compute hi1, hi2, and hi3 as Equation (37). const double& rho = x; Eigen::Vector3d h = T_c0_ci.linear()* @@ -1138,10 +1137,6 @@ bool Feature::initializePosition(const CamStateServer& cam_states) { // Organize camera poses and feature observations properly. - -initializeRho(cam_states); -std::cout << "init rho is: " << anchor_rho << std::endl; - std::vector > cam_poses(0); std::vector& rm_cam_state_ids); + + void pruneLastCamStateBuffer(); void pruneCamStateBuffer(); // Reset the system online if the uncertainty is too large. void onlineReset(); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 4a03598..74b689d 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -475,7 +475,7 @@ void MsckfVio::imageCallback( ros::Time::now()-start_time).toSec(); start_time = ros::Time::now(); - pruneCamStateBuffer(); + pruneLastCamStateBuffer(); double prune_cam_states_time = ( ros::Time::now()-start_time).toSec(); @@ -1360,31 +1360,13 @@ void MsckfVio::PhotometricMeasurementJacobian( H_plj = dI_dhj * dh_dXplj; // 1 x 6 H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6 - /* - cout << endl; - std::cout << H_plj << endl; - cout << r_photo.segment(count, 1) << endl; - std::cout << H_plj.colPivHouseholderQr().solve(r_photo.segment(count, 1)) << std::endl; - */ H_rho.block<1, 1>(count, 0) = H_rhoj; H_pl.block<1, 6>(count, 0) = H_plj; H_pA.block<1, 6>(count, 0) = H_pAj; - //cout << "H_pl\n" << H_plj << endl; - - //cout << "H_pA\n" << H_pAj << endl; - - //cout << "H_rho\n" << H_rhoj << endl; count++; } - /* - std::cout << "\n\n frame change through patch" << std::endl; - std::cout << H_pl << std::endl; - std::cout << r_photo << std::endl; - std::cout << endl; - std::cout << H_pl.colPivHouseholderQr().solve(r_photo) << std::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); @@ -1426,7 +1408,7 @@ void MsckfVio::PhotometricMeasurementJacobian( if(PRINTIMAGES) { feature.MarkerGeneration(marker_pub, state_server.cam_states); - //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); + feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector); } return; @@ -1530,9 +1512,10 @@ void MsckfVio::PhotometricFeatureJacobian( { ofstream myfile; myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); - myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl; - myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; + myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl; + myfile << "kernel\n" << A_null_space << endl; myfile.close(); + myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; cout << "---------- LOGGED -------- " << endl; } if(PRINTIMAGES) @@ -1860,7 +1843,7 @@ void MsckfVio::removeLostFeatures() { invalid_feature_ids.push_back(feature.id); continue; } else { - if(!feature.initializePosition(state_server.cam_states)) { + if(!feature.initializeRho(state_server.cam_states)) { invalid_feature_ids.push_back(feature.id); continue; } @@ -1989,6 +1972,143 @@ void MsckfVio::findRedundantCamStates(vector& rm_cam_state_ids) { return; } +void MsckfVio::pruneLastCamStateBuffer() +{ + if (state_server.cam_states.size() < max_cam_state_size) + return; + + auto rm_cam_state_id = state_server.cam_states.begin()->first; + + // Set the size of the Jacobian matrix. + int jacobian_row_size = 0; + int augmentationSize = 6; + if(PHOTOMETRIC) + augmentationSize = 7; + + //initialize all the features which are going to be removed + for (auto& item : map_server) { + auto& feature = item.second; + + // check if feature is involved, if not continue + if (feature.observations.find(rm_cam_state_id) == + feature.observations.end()) + continue; + + + if (!feature.is_initialized) { + + // Check if the feature can be initialized + if (!feature.checkMotion(state_server.cam_states)) { + + // remove any features that can't be initialized + feature.observations.erase(rm_cam_state_id); + continue; + } else { + if(!feature.initializeRho(state_server.cam_states)) { + feature.observations.erase(rm_cam_state_id); + continue; + } + } + } + if(!feature.is_anchored) + { + if(!feature.initializeAnchor(cam0, N)) + { + feature.observations.erase(rm_cam_state_id); + continue; + } + } + + 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; + + } + + 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) { + auto& feature = item.second; + + // check if feature is involved, if not continue + if (feature.observations.find(rm_cam_state_id) == + feature.observations.end()) + continue; + + + vector involved_cam_state_ids(0); + for (const auto& cam_state : state_server.cam_states) + involved_cam_state_ids.push_back(cam_state.first); + + if(PHOTOMETRIC) + PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); + else + 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(); + cout << "passed" << endl; + } + else + { + cout << "rejected" << endl; + } + for (const auto& cam_id : involved_cam_state_ids) + feature.observations.erase(cam_id); + } + H_x.conservativeResize(stack_cntr, H_x.cols()); + r.conservativeResize(stack_cntr); + + // Perform measurement update. + measurementUpdate(H_x, r); + + //reduction + int cam_sequence = std::distance(state_server.cam_states.begin(), + state_server.cam_states.find(rm_cam_state_id)); + int cam_state_start = 21 + augmentationSize*cam_sequence; + int cam_state_end = cam_state_start + augmentationSize; + + + // Remove the corresponding rows and columns in the state + // covariance matrix. + if (cam_state_end < state_server.state_cov.rows()) { + state_server.state_cov.block(cam_state_start, 0, + state_server.state_cov.rows()-cam_state_end, + state_server.state_cov.cols()) = + state_server.state_cov.block(cam_state_end, 0, + state_server.state_cov.rows()-cam_state_end, + state_server.state_cov.cols()); + + state_server.state_cov.block(0, cam_state_start, + state_server.state_cov.rows(), + state_server.state_cov.cols()-cam_state_end) = + state_server.state_cov.block(0, cam_state_end, + state_server.state_cov.rows(), + state_server.state_cov.cols()-cam_state_end); + + state_server.state_cov.conservativeResize( + state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + } else { + state_server.state_cov.conservativeResize( + state_server.state_cov.rows()-augmentationSize, state_server.state_cov.cols()-augmentationSize); + } + + // Remove this camera state in the state vector. + state_server.cam_states.erase(rm_cam_state_id); + cam0.moving_window.erase(rm_cam_state_id); + cam1.moving_window.erase(rm_cam_state_id); + + return; +} + void MsckfVio::pruneCamStateBuffer() { if (state_server.cam_states.size() < max_cam_state_size) @@ -2030,7 +2150,7 @@ void MsckfVio::pruneCamStateBuffer() { feature.observations.erase(cam_id); continue; } else { - if(!feature.initializePosition(state_server.cam_states)) { + if(!feature.initializeRho(state_server.cam_states)) { for (const auto& cam_id : involved_cam_state_ids) feature.observations.erase(cam_id); continue;