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
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
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
// the cam0 and cam1 frame.
@@ -983,12 +983,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
//estimate photometric measurement
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;
//calculate photom. residual
//for(int i = 0; i < photo_z.size(); i++)
// photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
for(int i = 0; i < photo_z.size(); i++)
photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
// printf("-----\n");
// for(int i = 0; i < photo_z.size(); i++)
@@ -1341,11 +1341,11 @@ void MsckfVio::removeLostFeatures() {
continue;
}
}
/*if(!feature.initializeAnchor(cam0_moving_window, cam0))
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
invalid_feature_ids.push_back(feature.id);
continue;
}*/
}
}
jacobian_row_size += 4*feature.observations.size() - 3;
@@ -1494,12 +1494,12 @@ void MsckfVio::pruneCamStateBuffer() {
continue;
}
}
/*if(!feature.initializeAnchor(cam0_moving_window, cam0))
if(!feature.initializeAnchor(cam0_moving_window, cam0))
{
for (const auto& cam_id : involved_cam_state_ids)
feature.observations.erase(cam_id);
continue;
}*/
}
}
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;