Merge pull request #32 from richard-elvira/master

Solve compatibility with Kitti and TUM dataset

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

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

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

Examples/Monocular/TUM1.yaml

@@ -3,6 +3,7 @@
 #--------------------------------------------------------------------------------------------
 # Camera Parameters. Adjust them!
 #--------------------------------------------------------------------------------------------
+Camera.type: "PinHole"
 
 # Camera calibration and distortion parameters (OpenCV) 
 Camera.fx: 517.306408

Examples/Monocular/TUM2.yaml

@@ -3,6 +3,7 @@
 #--------------------------------------------------------------------------------------------
 # Camera Parameters. Adjust them!
 #--------------------------------------------------------------------------------------------
+Camera.type: "PinHole"
 
 # Camera calibration and distortion parameters (OpenCV) 
 Camera.fx: 520.908620

Examples/Monocular/TUM3.yaml

@@ -3,6 +3,7 @@
 #--------------------------------------------------------------------------------------------
 # Camera Parameters. Adjust them!
 #--------------------------------------------------------------------------------------------
+Camera.type: "PinHole"
 
 # Camera calibration and distortion parameters (OpenCV) 
 Camera.fx: 535.4

Examples/Monocular/TUM_512.yaml

@@ -4,6 +4,7 @@
 # Camera Parameters. Adjust them!
 #--------------------------------------------------------------------------------------------
 Camera.type: "KannalaBrandt8"
+
 # Camera calibration and distortion parameters (OpenCV) 
 Camera.fx: 190.978477 # 190.97847715128717
 Camera.fy: 190.973307 # 190.9733070521226

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

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

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

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

include/Tracking.h

@@ -155,8 +155,9 @@ public:
 
 
     //TEST--
-    cv::Mat M1l, M2l;
-    cv::Mat M1r, M2r;
+    bool mbNeedRectify;
+    //cv::Mat M1l, M2l;
+    //cv::Mat M1r, M2r;
 
     bool mbWriteStats;
 

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;