From 5a80f97b68e54bed52f2971e8ad430a4a4ec201f Mon Sep 17 00:00:00 2001 From: Raphael Maenle Date: Fri, 12 Jul 2019 16:36:36 +0200 Subject: [PATCH] fixed visualization issue --- config/camchain-imucam-euroc.yaml | 4 ++-- include/msckf_vio/feature.hpp | 3 +-- launch/image_processor_tinytum.launch | 6 +++--- launch/msckf_vio_euroc.launch | 4 ++-- src/image_processor.cpp | 12 ++++++++++-- src/msckf_vio.cpp | 16 ++++++++++------ 6 files changed, 28 insertions(+), 17 deletions(-) diff --git a/config/camchain-imucam-euroc.yaml b/config/camchain-imucam-euroc.yaml index de4c321..ad2572c 100644 --- a/config/camchain-imucam-euroc.yaml +++ b/config/camchain-imucam-euroc.yaml @@ -9,7 +9,7 @@ cam0: 0, 0, 0, 1.000000000000000] camera_model: pinhole distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] - distortion_model: radtan + distortion_model: pre-radtan intrinsics: [458.654, 457.296, 367.215, 248.375] resolution: [752, 480] timeshift_cam_imu: 0.0 @@ -26,7 +26,7 @@ cam1: 0, 0, 0, 1.000000000000000] camera_model: pinhole distortion_coeffs: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05] - distortion_model: radtan + distortion_model: pre-radtan intrinsics: [457.587, 456.134, 379.999, 255.238] resolution: [752, 480] timeshift_cam_imu: 0.0 diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp index 5cc997f..44f2a9a 100644 --- a/include/msckf_vio/feature.hpp +++ b/include/msckf_vio/feature.hpp @@ -823,11 +823,10 @@ bool Feature::VisualizePatch( cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA); - cv::Point2f p_f(observations.find(cam_state_id)->second(0)*cam.intrinsics[0] + cam.intrinsics[2],observations.find(cam_state_id)->second(1)*cam.intrinsics[1] + cam.intrinsics[3]); + cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1)); // move to real pixels p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs); - for(int i = 0; i - - - + + + diff --git a/launch/msckf_vio_euroc.launch b/launch/msckf_vio_euroc.launch index 864f15e..8f59c97 100644 --- a/launch/msckf_vio_euroc.launch +++ b/launch/msckf_vio_euroc.launch @@ -19,11 +19,11 @@ - + - + diff --git a/src/image_processor.cpp b/src/image_processor.cpp index 65acd43..9242b15 100644 --- a/src/image_processor.cpp +++ b/src/image_processor.cpp @@ -226,8 +226,16 @@ 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); + + cv::Mat new_cam0; + cv::Mat new_cam1; + + image_handler::undistortImage(cam0_curr_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1_curr_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs); + + new_cam0.copyTo(cam0_curr_img_ptr->image); + new_cam1.copyTo(cam1_curr_img_ptr->image); + //ROS_INFO("Publishing: %f", // (ros::Time::now()-start_time).toSec()); diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp index f6fa93f..c9cb9e9 100644 --- a/src/msckf_vio.cpp +++ b/src/msckf_vio.cpp @@ -544,13 +544,14 @@ void MsckfVio::manageMovingWindow( 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 new_cam0; + cv::Mat new_cam1; + image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs); + image_handler::undistortImage(cam1_img_ptr->image, new_cam1, 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 = new_cam0.clone(); + cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone(); cv::Mat xder; cv::Mat yder; @@ -1704,7 +1705,8 @@ bool MsckfVio::PhotometricPatchPointJacobian( dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0]; dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1]; - //cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl; + cout << dI_dhj(1, 2) << ", " << dI_dhj(1, 3) << endl; // add jacobian @@ -1782,6 +1784,8 @@ bool MsckfVio::PhotometricMeasurementJacobian( if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo)) return false; + cout << "-\n" << r_photo << "-" << endl; + //cout << "r\n" << r_photo << endl; // calculate jacobian for patch int count = 0;