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

reinstantiated photometry removed slow-down problem

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-17 17:06:44 +02:00
parent f4a17f8512
commit cfecefe29f
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/msckf_vio.cpp
View File

@ -917,7 +917,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
//photometric observation //photometric observation
std::vector<float> photo_z; std::vector<float> photo_z;
//feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z); feature.FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, photo_z);
// Convert the feature position from the world frame to // Convert the feature position from the world frame to
// the cam0 and cam1 frame. // the cam0 and cam1 frame.
@ -983,12 +983,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
//estimate photometric measurement //estimate photometric measurement
std::vector<float> estimate_photo_z; std::vector<float> estimate_photo_z;
//feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z); feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, cam0_moving_window, estimate_photo_z);
std::vector<float> photo_r; std::vector<float> photo_r;
//calculate photom. residual //calculate photom. residual
//for(int i = 0; i < photo_z.size(); i++) for(int i = 0; i < photo_z.size(); i++)
// photo_r.push_back(photo_z[i] - estimate_photo_z[i]); photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
// printf("-----\n"); // printf("-----\n");
// for(int i = 0; i < photo_z.size(); i++) // for(int i = 0; i < photo_z.size(); i++)
@ -1341,11 +1341,11 @@ void MsckfVio::removeLostFeatures() {
continue; continue;
} }
} }
/*if(!feature.initializeAnchor(cam0_moving_window, cam0)) if(!feature.initializeAnchor(cam0_moving_window, cam0))
{ {
invalid_feature_ids.push_back(feature.id); invalid_feature_ids.push_back(feature.id);
continue; continue;
}*/ }
} }
jacobian_row_size += 4*feature.observations.size() - 3; jacobian_row_size += 4*feature.observations.size() - 3;
@ -1494,12 +1494,12 @@ void MsckfVio::pruneCamStateBuffer() {
continue; continue;
} }
} }
/*if(!feature.initializeAnchor(cam0_moving_window, cam0)) if(!feature.initializeAnchor(cam0_moving_window, cam0))
{ {
for (const auto& cam_id : involved_cam_state_ids) for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id); feature.observations.erase(cam_id);
continue; continue;
}*/ }
} }
jacobian_row_size += 4*involved_cam_state_ids.size() - 3; jacobian_row_size += 4*involved_cam_state_ids.size() - 3;