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

added saving exposure time from the frame ID, where the TUM dataset saves it

Browse Source
This commit is contained in:
Raphael Maenle
2019-04-17 10:54:54 +02:00
parent 819e43bb3b
commit 6ae83f0db7
4 changed files with 28 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/msckf_vio.cpp
View File

@ -310,12 +310,6 @@ void MsckfVio::imageCallback(
// Return if the gravity vector has not been set.
if (!is_gravity_set) return;
// Get the current image.
cv_bridge::CvImageConstPtr cam0_image_ptr = cv_bridge::toCvShare(cam0_img,
sensor_msgs::image_encodings::MONO8);
cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
sensor_msgs::image_encodings::MONO8);
// Start the system if the first image is received.
// The frame where the first image is received will be
// the origin.
@ -350,7 +344,7 @@ void MsckfVio::imageCallback(
// Add new images to moving window
start_time = ros::Time::now();
manageMovingWindow(cam0_image_ptr, cam1_img_ptr, feature_msg);
manageMovingWindow(cam0_img, cam1_img, feature_msg);
double manage_moving_window_time = (
ros::Time::now()-start_time).toSec();
@ -399,12 +393,23 @@ void MsckfVio::imageCallback(
}
void MsckfVio::manageMovingWindow(
const cv_bridge::CvImageConstPtr& cam0_image_ptr,
const cv_bridge::CvImageConstPtr& cam1_image_ptr,
const sensor_msgs::ImageConstPtr& cam0_img,
const sensor_msgs::ImageConstPtr& cam1_img,
const CameraMeasurementConstPtr& feature_msg) {
cam0_moving_window[state_server.imu_state.id] = cam0_image_ptr->image.clone();
cam1_moving_window[state_server.imu_state.id] = cam1_image_ptr->image.clone();
//save exposure Time into moving window
cam0_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam0_img->header.frame_id.data(), NULL) / 1000;
cam1_moving_window[state_server.imu_state.id].exposureTime_ms = strtod(cam1_img->header.frame_id.data(), NULL) / 1000;
printf("exposure: %f\n", cam0_moving_window[state_server.imu_state.id].exposureTime_ms);
// Get the current image.
cv_bridge::CvImageConstPtr cam0_img_ptr = cv_bridge::toCvShare(cam0_img,
sensor_msgs::image_encodings::MONO8);
cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
sensor_msgs::image_encodings::MONO8);
// save image information into moving window
cam0_moving_window[state_server.imu_state.id].image = cam0_img_ptr->image.clone();
cam1_moving_window[state_server.imu_state.id].image = cam1_img_ptr->image.clone();
//TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
while(cam0_moving_window.size() > 100)