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

changed data handling in undistortion to work with euroc datset

Browse Source
This commit is contained in:
Raphael Maenle 2019-07-11 16:50:03 +02:00
parent 55669ea2c9
commit 3b6d2c918d
2 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/image_processor.cpp
View File

@ -221,9 +221,13 @@ void ImageProcessor::stereoCallback(
ros::Time start_time = ros::Time::now();
image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
//ROS_INFO("Publishing: %f",
cv::Mat newImage;
image_handler::undistortImage(cam0_curr_img_ptr->image, newImage, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
newImage.copyTo(cam0_curr_img_ptr->image);
image_handler::undistortImage(cam1_curr_img_ptr->image, newImage, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
newImage.copyTo( cam1_curr_img_ptr->image);
//ROS_INFO("Publishing: %f",
// (ros::Time::now()-start_time).toSec());
// Build the image pyramids once since they're used at multiple places
createImagePyramids();

15
src/msckf_vio.cpp
View File

@ -541,14 +541,15 @@ void MsckfVio::manageMovingWindow(
sensor_msgs::image_encodings::MONO8);
cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
sensor_msgs::image_encodings::MONO8);
image_handler::undistortImage(cam0_img_ptr->image, cam0_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
image_handler::undistortImage(cam1_img_ptr->image, cam1_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
cv::Mat newImage0;
cv::Mat newImage1;
image_handler::undistortImage(cam0_img_ptr->image, newImage0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
image_handler::undistortImage(cam1_img_ptr->image, newImage1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
// 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();
cam0.moving_window[state_server.imu_state.id].image = newImage0.clone();
cam1.moving_window[state_server.imu_state.id].image = newImage1.clone();
cv::Mat xder;
cv::Mat yder;