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

uncommented stuff

Browse Source
This commit is contained in:
Raphael Maenle 2019-06-27 16:04:29 +02:00
parent b3e86b3e64
commit 5451c2d097
2 changed files with 22 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
launch/msckf_vio_tum.launch
View File

@ -22,10 +22,10 @@
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/> <param name="StreamPause" value="true"/>
<param name="PrintImages" value="true"/> <param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="5"/> <param name="patch_size_n" value="1"/>
<!-- Calibration parameters --> <!-- Calibration parameters -->
<rosparam command="load" file="$(arg calibration_file)"/> <rosparam command="load" file="$(arg calibration_file)"/>

38
src/msckf_vio.cpp
View File

@ -554,15 +554,19 @@ void MsckfVio::manageMovingWindow(
// undistort Images // undistort Images
const cv::Matx33d K(cam0.intrinsics[0], 0.0, cam0.intrinsics[2],
0.0, cam0.intrinsics[1], cam0.intrinsics[3],
0.0, 0.0, 1.0);
//cv::Mat undistortedCam0; cv::Mat undistortedCam0;
//cv::Mat undistortedCam1; cv::Mat undistortedCam1;
//image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); image_handler::undistortImage(cam0.moving_window[state_server.imu_state.id].image, undistortedCam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
//image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); image_handler::undistortImage(cam1.moving_window[state_server.imu_state.id].image, undistortedCam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
//cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone(); cam0.moving_window[state_server.imu_state.id].image = undistortedCam0.clone();
//cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone(); cam1.moving_window[state_server.imu_state.id].image = undistortedCam1.clone();
//TODO handle any massive overflow correctly (should be pruned, before this ever triggers) //TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
while(cam0.moving_window.size() > 100) while(cam0.moving_window.size() > 100)
@ -2061,20 +2065,20 @@ void MsckfVio::removeLostFeatures() {
MatrixXd pH_xj; MatrixXd pH_xj;
VectorXd pr_j; VectorXd pr_j;
//PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
featureJacobian(feature.id, cam_state_ids, H_xj, r_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) { if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj; H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
r.segment(stack_cntr, r_j.rows()) = r_j; r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows(); stack_cntr += H_xj.rows();
} }
/*
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) { if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows(); pstack_cntr += pH_xj.rows();
} }
*/
// Put an upper bound on the row size of measurement Jacobian, // Put an upper bound on the row size of measurement Jacobian,
// which helps guarantee the executation time. // which helps guarantee the executation time.
//if (stack_cntr > 1500) break; //if (stack_cntr > 1500) break;
@ -2087,7 +2091,7 @@ void MsckfVio::removeLostFeatures() {
// Perform the measurement update step. // Perform the measurement update step.
measurementUpdate(H_x, r); measurementUpdate(H_x, r);
//photometricMeasurementUpdate(pH_x, pr); photometricMeasurementUpdate(pH_x, pr);
// Remove all processed features from the map. // Remove all processed features from the map.
for (const auto& feature_id : processed_feature_ids) for (const auto& feature_id : processed_feature_ids)
@ -2217,7 +2221,7 @@ void MsckfVio::pruneLastCamStateBuffer()
for (const auto& cam_state : state_server.cam_states) for (const auto& cam_state : state_server.cam_states)
involved_cam_state_ids.push_back(cam_state.first); involved_cam_state_ids.push_back(cam_state.first);
//PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); 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())) { if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
@ -2226,13 +2230,11 @@ void MsckfVio::pruneLastCamStateBuffer()
stack_cntr += H_xj.rows(); stack_cntr += H_xj.rows();
pruned_cntr++; pruned_cntr++;
} }
/*
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows(); pstack_cntr += pH_xj.rows();
} }
*/
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);
} }
@ -2245,7 +2247,7 @@ void MsckfVio::pruneLastCamStateBuffer()
// Perform measurement update. // Perform measurement update.
measurementUpdate(H_x, r); measurementUpdate(H_x, r);
//photometricMeasurementUpdate(pH_x, pr); photometricMeasurementUpdate(pH_x, pr);
//reduction //reduction
int cam_sequence = std::distance(state_server.cam_states.begin(), int cam_sequence = std::distance(state_server.cam_states.begin(),
@ -2372,7 +2374,7 @@ void MsckfVio::pruneCamStateBuffer() {
if (involved_cam_state_ids.size() == 0) continue; if (involved_cam_state_ids.size() == 0) continue;
//PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j); PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j);
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j); featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) { if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
@ -2380,13 +2382,13 @@ void MsckfVio::pruneCamStateBuffer() {
r.segment(stack_cntr, r_j.rows()) = r_j; r.segment(stack_cntr, r_j.rows()) = r_j;
stack_cntr += H_xj.rows(); stack_cntr += H_xj.rows();
} }
/*
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) { if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj; pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
pr.segment(pstack_cntr, pr_j.rows()) = pr_j; pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
pstack_cntr += pH_xj.rows(); pstack_cntr += pH_xj.rows();
} }
*/
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);
} }
@ -2399,7 +2401,7 @@ void MsckfVio::pruneCamStateBuffer() {
// Perform measurement update. // Perform measurement update.
measurementUpdate(H_x, r); measurementUpdate(H_x, r);
//photometricMeasurementUpdate(pH_x, pr); photometricMeasurementUpdate(pH_x, pr);
//reduction //reduction
for (const auto& cam_id : rm_cam_state_ids) { for (const auto& cam_id : rm_cam_state_ids) {