raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

removed incorrect prints

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-11 09:53:43 +02:00
parent 5f6bcd1784
commit 9b4bf12846
3 changed files with 146 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
src/msckf_vio.cpp
View File

@@ -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<StateIDType>& 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<StateIDType> 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;