From 866135fb47bb5573bf01200cd37741fa9a474f3f Mon Sep 17 00:00:00 2001 From: Richard Elvira Date: Fri, 31 Jul 2020 10:56:37 +0200 Subject: [PATCH] Solve compatibility with Kitti dataset --- Examples/Monocular/KITTI00-02.yaml | 1 + Examples/Monocular/KITTI03.yaml | 1 + Examples/Monocular/KITTI04-12.yaml | 1 + Examples/RGB-D/TUM1.yaml | 1 + Examples/RGB-D/TUM2.yaml | 1 + Examples/RGB-D/TUM3.yaml | 1 + Examples/ROS/ORB_SLAM3/Asus.yaml | 1 + Examples/Stereo/KITTI00-02.yaml | 1 + Examples/Stereo/KITTI03.yaml | 1 + Examples/Stereo/KITTI04-12.yaml | 1 + include/Tracking.h | 5 +++-- src/Tracking.cc | 24 ++++++++++++++++++------ 12 files changed, 31 insertions(+), 8 deletions(-) diff --git a/Examples/Monocular/KITTI00-02.yaml b/Examples/Monocular/KITTI00-02.yaml index e4faba1633..e32d24a74c 100644 --- a/Examples/Monocular/KITTI00-02.yaml +++ b/Examples/Monocular/KITTI00-02.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 718.856 diff --git a/Examples/Monocular/KITTI03.yaml b/Examples/Monocular/KITTI03.yaml index b081bd08a2..28a18361cd 100644 --- a/Examples/Monocular/KITTI03.yaml +++ b/Examples/Monocular/KITTI03.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 721.5377 diff --git a/Examples/Monocular/KITTI04-12.yaml b/Examples/Monocular/KITTI04-12.yaml index e0e18c7b7e..df2bb6fdc8 100644 --- a/Examples/Monocular/KITTI04-12.yaml +++ b/Examples/Monocular/KITTI04-12.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 707.0912 diff --git a/Examples/RGB-D/TUM1.yaml b/Examples/RGB-D/TUM1.yaml index d049a68fcc..717bc36de0 100644 --- a/Examples/RGB-D/TUM1.yaml +++ b/Examples/RGB-D/TUM1.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 517.306408 diff --git a/Examples/RGB-D/TUM2.yaml b/Examples/RGB-D/TUM2.yaml index 4c309ee98d..13c326b213 100644 --- a/Examples/RGB-D/TUM2.yaml +++ b/Examples/RGB-D/TUM2.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 520.908620 diff --git a/Examples/RGB-D/TUM3.yaml b/Examples/RGB-D/TUM3.yaml index 5a5ff409e4..49235ddbcc 100644 --- a/Examples/RGB-D/TUM3.yaml +++ b/Examples/RGB-D/TUM3.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 535.4 diff --git a/Examples/ROS/ORB_SLAM3/Asus.yaml b/Examples/ROS/ORB_SLAM3/Asus.yaml index 9ffd2c8627..4d0c08ec7d 100644 --- a/Examples/ROS/ORB_SLAM3/Asus.yaml +++ b/Examples/ROS/ORB_SLAM3/Asus.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 535.4 diff --git a/Examples/Stereo/KITTI00-02.yaml b/Examples/Stereo/KITTI00-02.yaml index 3325ef5675..b84e639c2d 100644 --- a/Examples/Stereo/KITTI00-02.yaml +++ b/Examples/Stereo/KITTI00-02.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 718.856 diff --git a/Examples/Stereo/KITTI03.yaml b/Examples/Stereo/KITTI03.yaml index 14014f32e9..40c6e7cb0c 100644 --- a/Examples/Stereo/KITTI03.yaml +++ b/Examples/Stereo/KITTI03.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 721.5377 diff --git a/Examples/Stereo/KITTI04-12.yaml b/Examples/Stereo/KITTI04-12.yaml index 98f762a5f4..f62151c71d 100644 --- a/Examples/Stereo/KITTI04-12.yaml +++ b/Examples/Stereo/KITTI04-12.yaml @@ -3,6 +3,7 @@ #-------------------------------------------------------------------------------------------- # Camera Parameters. Adjust them! #-------------------------------------------------------------------------------------------- +Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 707.0912 diff --git a/include/Tracking.h b/include/Tracking.h index 6ab09cfae7..e92530bd8d 100644 --- a/include/Tracking.h +++ b/include/Tracking.h @@ -155,8 +155,9 @@ class Tracking //TEST-- - cv::Mat M1l, M2l; - cv::Mat M1r, M2r; + bool mbNeedRectify; + //cv::Mat M1l, M2l; + //cv::Mat M1r, M2r; bool mbWriteStats; diff --git a/src/Tracking.cc b/src/Tracking.cc index d6f82fca1a..577b5a1b70 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -268,7 +268,8 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, mbInitWith3KFs = false; - //Test Images + //Rectification parameters + /*mbNeedRectify = false; if((mSensor == System::STEREO || mSensor == System::IMU_STEREO) && sCameraName == "PinHole") { cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r; @@ -289,17 +290,28 @@ Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, int rows_r = fSettings["RIGHT.height"]; int cols_r = fSettings["RIGHT.width"]; - // M1r y M2r son los outputs (igual para l) - // M1r y M2r son las matrices relativas al mapeo correspondiente a la rectificación de mapa en el eje X e Y respectivamente - cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l); - cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r); + if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() + || rows_l==0 || cols_l==0 || rows_r==0 || cols_r==0) + { + mbNeedRectify = false; + } + else + { + mbNeedRectify = true; + // M1r y M2r son los outputs (igual para l) + // M1r y M2r son las matrices relativas al mapeo correspondiente a la rectificación de mapa en el eje X e Y respectivamente + //cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l); + //cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r); + } + + } else { int cols = 752; int rows = 480; cv::Mat R_l = cv::Mat::eye(3, 3, CV_32F); - } + }*/ mnNumDataset = 0;