From 6ae83f0db786c989932c7513189d5ed98745edcb Mon Sep 17 00:00:00 2001 From: g-spacewhale Date: Wed, 17 Apr 2019 10:54:54 +0200 Subject: [PATCH] added saving exposure time from the frame ID, where the TUM dataset saves it --- include/msckf_vio/cam_state.h | 10 ++++++++-- include/msckf_vio/feature.hpp | 4 ++-- include/msckf_vio/msckf_vio.h | 4 ++-- src/msckf_vio.cpp | 27 ++++++++++++++++----------- 4 files changed, 28 insertions(+), 17 deletions(-) diff --git a/include/msckf_vio/cam_state.h b/include/msckf_vio/cam_state.h index e56b6c9..a58da92 100644 --- a/include/msckf_vio/cam_state.h +++ b/include/msckf_vio/cam_state.h @@ -30,6 +30,11 @@ struct CameraCalibration{ cv::Vec4d distortion_coeffs; }; +struct Frame{ + cv::Mat image; + double exposureTime_ms; +}; + /* * @brief CAMState Stored camera state in order to * form measurement model. @@ -50,6 +55,7 @@ struct CAMState { // Position of the camera frame in the world frame. Eigen::Vector3d position; + // Illumination Information of the frame IlluminationParameter illumination; // These two variables should have the same physical @@ -80,9 +86,9 @@ typedef std::map, Eigen::aligned_allocator< std::pair > > CamStateServer; -typedef std::map, +typedef std::map, Eigen::aligned_allocator< - std::pair > > movingWindow; + std::pair > > movingWindow; } // namespace msckf_vio #endif // MSCKF_VIO_CAM_STATE_H diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 8215cad..5026c3d 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -361,7 +361,7 @@ bool Feature::IrradianceOfAnchorPatch( for (auto point : anchorPatch_3d) { cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point); - uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second); + uint8_t irradiance = Irradiance(p_in_c0 , cam0_moving_window.find(cam_state_id)->second.image); anchorPatch_measurement.push_back(irradiance); } } @@ -425,7 +425,7 @@ bool Feature::initializeAnchor( if(cam0_moving_window.find(anchor->first) == cam0_moving_window.end()) return false; - cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second; + cv::Mat anchorImage = cam0_moving_window.find(anchor->first)->second.image; auto u = anchor->second(0)*cam.intrinsics[0] + cam.intrinsics[2]; auto v = anchor->second(1)*cam.intrinsics[1] + cam.intrinsics[3]; int count = 0; diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h index b87349b..07da6bd 100644 --- a/include/msckf_vio/msckf_vio.h +++ b/include/msckf_vio/msckf_vio.h @@ -145,8 +145,8 @@ class MsckfVio { std_srvs::Trigger::Response& res); void 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); // Filter related functions diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index 095b71b..3625541 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -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)