V0.4: Beta version, 21 April 2021

- Changed OpenCV dynamic matrices to static matrices to speed up the code.

- Capability to measure running time of the system threads.

- Compatibility with OpenCV 4.0 (Requires at least OpenCV 3.0)

- Fixed minor bugs.

CMakeLists.txt

@@ -33,12 +33,12 @@ endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
-find_package(OpenCV 3)
+find_package(OpenCV 4.0)
 if(NOT OpenCV_FOUND)
-   find_package(OpenCV 2.4.3 QUIET)
-   if(NOT OpenCV_FOUND)
-      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
-   endif()
+  find_package(OpenCV 3.0)
+  if(NOT OpenCV_FOUND)
+    message(FATAL_ERROR "OpenCV > 3.0 not found.")
+  endif()
 endif()
 
 MESSAGE("OPENCV VERSION:")
@@ -46,6 +46,7 @@ MESSAGE(${OpenCV_VERSION})
 
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
+find_package(realsense2)
 
 include_directories(
 ${PROJECT_SOURCE_DIR}
@@ -72,7 +73,6 @@ src/Atlas.cc
 src/Map.cc
 src/MapDrawer.cc
 src/Optimizer.cc
-src/PnPsolver.cc
 src/Frame.cc
 src/KeyFrameDatabase.cc
 src/Sim3Solver.cc
@@ -84,6 +84,7 @@ src/CameraModels/Pinhole.cpp
 src/CameraModels/KannalaBrandt8.cpp
 src/OptimizableTypes.cpp
 src/MLPnPsolver.cpp
+src/TwoViewReconstruction.cc
 include/System.h
 include/Tracking.h
 include/LocalMapping.h
@@ -98,7 +99,6 @@ include/Atlas.h
 include/Map.h
 include/MapDrawer.h
 include/Optimizer.h
-include/PnPsolver.h
 include/Frame.h
 include/KeyFrameDatabase.h
 include/Sim3Solver.h
@@ -112,7 +112,8 @@ include/CameraModels/KannalaBrandt8.h
 include/OptimizableTypes.h
 include/MLPnPsolver.h
 include/TwoViewReconstruction.h
-src/TwoViewReconstruction.cc)
+include/Config.h
+)
 
 add_subdirectory(Thirdparty/g2o)
 
@@ -127,14 +128,17 @@ ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
 )
 
 
-# Build examples
+### Build examples
 
+# RGB-D examples
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D)
+
 add_executable(rgbd_tum
 Examples/RGB-D/rgbd_tum.cc)
 target_link_libraries(rgbd_tum ${PROJECT_NAME})
 
 
+# Stereo examples
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo)
 
 add_executable(stereo_kitti
@@ -149,7 +153,7 @@ add_executable(stereo_tum_vi
 Examples/Stereo/stereo_tum_vi.cc)
 target_link_libraries(stereo_tum_vi ${PROJECT_NAME})
 
-
+# Monocular examples
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular)
 
 add_executable(mono_tum
@@ -168,7 +172,7 @@ add_executable(mono_tum_vi
 Examples/Monocular/mono_tum_vi.cc)
 target_link_libraries(mono_tum_vi ${PROJECT_NAME})
 
-
+# Monocular-Inertial examples
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular-Inertial)
 
 add_executable(mono_inertial_euroc
@@ -179,16 +183,14 @@ add_executable(mono_inertial_tum_vi
 Examples/Monocular-Inertial/mono_inertial_tum_vi.cc)
 target_link_libraries(mono_inertial_tum_vi ${PROJECT_NAME})
 
-
+# Stereo-Inertial examples
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo-Inertial)
 
 add_executable(stereo_inertial_euroc
 Examples/Stereo-Inertial/stereo_inertial_euroc.cc)
 target_link_libraries(stereo_inertial_euroc ${PROJECT_NAME})
 
-
 add_executable(stereo_inertial_tum_vi
 Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc)
 target_link_libraries(stereo_inertial_tum_vi ${PROJECT_NAME})
 
-

Changelog.md

@@ -1,15 +1,26 @@
 # ORB-SLAM3
 Details of changes between the different versions.
 
+### V0.4: Beta version, 21 April 2021
+
+- Changed OpenCV dynamic matrices to static matrices to speed up the code.
+
+- Capability to measure running time of the system threads.
+
+- Compatibility with OpenCV 4.0 (Requires at least OpenCV 3.0) 
+
+- Fixed minor bugs.
+
+
 ### V0.3: Beta version, 4 Sep 2020
 
-- RGB-D compatibility, the RGB-D examples had been adapted to the new version.
+- RGB-D compatibility, the RGB-D examples have been adapted to the new version.
 
-- Kitti and TUM dataset compatibility, these examples had been adapted to the new version.
+- Kitti and TUM dataset compatibility, these examples have been adapted to the new version.
 
-- ROS compatibility, It had been updated the old references in the code to work with this version.
+- ROS compatibility, updated the old references in the code to work with this version.
 
-- Config file parser, the YAML file contains the session configuration, a wrong parametrization may break the execution without any information to solve it. This version parses the file to read all the fields and give a proper answer if one of the fields have been wrong deffined or don't exist.
+- Config file parser, the YAML file contains the session configuration, a wrong parametrization may break the execution without any information to solve it. This version parses the file to read all the fields and give a proper answer if one of the fields have been wrongly deffined or does not exist.
 
 - Fixed minor bugs.
 
@@ -19,7 +30,7 @@ Initial release. It has these capabilities:
 
 - Multiple-Maps capabilities, it is able to handle multiple maps in the same session and merge them when a common area is detected with a seamless fussion.
 
-- Inertial sensor, the IMU initialization takes a 2 seconds to achieve a scale error less than 5\% and it is reffined in the next 10 seconds until is around 1\%. Inertial measures are integrated at frame rate to estimate the scale, gravity and velocity in order to improve the visual features detection and make the system robust to temporal occlusions.
+- Inertial sensor, the IMU initialization takes 2 seconds to achieve a scale error less than 5\% and it is reffined in the next 10 seconds until it is around 1\%. Inertial measures are integrated at frame rate to estimate the scale, gravity and velocity in order to improve the visual features detection and make the system robust to temporal occlusions.
 
 - Fisheye sensor, the fisheye sensors are now fully supported in monocular and stereo. 
 

Examples/Monocular-Inertial/mono_inertial_euroc.cc

@@ -116,25 +116,19 @@ int main(int argc, char *argv[])
 
     cout.precision(17);
 
-    /*cout << "Start processing sequence ..." << endl;
-    cout << "Images in the sequence: " << nImages << endl;
-    cout << "IMU data in the sequence: " << nImu << endl << endl;*/
-
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
     ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);
 
     int proccIm=0;
     for (seq = 0; seq<num_seq; seq++)
     {
-
-        // Main loop
         cv::Mat im;
         vector<ORB_SLAM3::IMU::Point> vImuMeas;
         proccIm = 0;
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
         {
             // Read image from file
-            im = cv::imread(vstrImageFilenames[seq][ni],CV_LOAD_IMAGE_UNCHANGED);
+            im = cv::imread(vstrImageFilenames[seq][ni],cv::IMREAD_UNCHANGED);
 
             double tframe = vTimestampsCam[seq][ni];
 
@@ -150,8 +144,6 @@ int main(int argc, char *argv[])
 
             if(ni>0)
             {
-                // cout << "t_cam " << tframe << endl;
-
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
                     vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
@@ -161,9 +153,7 @@ int main(int argc, char *argv[])
                 }
             }
 
-            /*cout << "first imu: " << first_imu << endl;
-            cout << "first imu time: " << fixed << vTimestampsImu[first_imu] << endl;
-            cout << "size vImu: " << vImuMeas.size() << endl;*/
+
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     #else
@@ -171,8 +161,7 @@ int main(int argc, char *argv[])
     #endif
 
             // Pass the image to the SLAM system
-            // cout << "tframe = " << tframe << endl;
-            SLAM.TrackMonocular(im,tframe,vImuMeas); // TODO change to monocular_inertial
+            SLAM.TrackMonocular(im,tframe,vImuMeas);
 
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
@@ -180,9 +169,13 @@ int main(int argc, char *argv[])
             std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
     #endif
 
+#ifdef REGISTER_TIMES
+            double t_track = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
+            SLAM.InsertTrackTime(t_track);
+#endif
+
             double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
             ttrack_tot += ttrack;
-            // std::cout << "ttrack: " << ttrack << std::endl;
 
             vTimesTrack[ni]=ttrack;
 
@@ -194,7 +187,7 @@ int main(int argc, char *argv[])
                 T = tframe-vTimestampsCam[seq][ni-1];
 
             if(ttrack<T)
-                usleep((T-ttrack)*1e6); // 1e6
+                usleep((T-ttrack)*1e6);
         }
         if(seq < num_seq - 1)
         {

Examples/Monocular-Inertial/mono_inertial_tum_vi.cc

@@ -110,10 +110,6 @@ int main(int argc, char **argv)
     cout << endl << "-------" << endl;
     cout.precision(17);
 
-    /*cout << "Start processing sequence ..." << endl;
-    cout << "Images in the sequence: " << nImages << endl;
-    cout << "IMU data in the sequence: " << nImu << endl << endl;*/
-
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
     ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true, 0, file_name);
 
@@ -135,8 +131,6 @@ int main(int argc, char **argv)
             // clahe
             clahe->apply(im,im);
 
-
-            // cout << "mat type: " << im.type() << endl;
             double tframe = vTimestampsCam[seq][ni];
 
             if(im.empty())
@@ -152,21 +146,16 @@ int main(int argc, char **argv)
 
             if(ni>0)
             {
-                // cout << "t_cam " << tframe << endl;
 
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
                     vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
-                    // cout << "t_imu = " << fixed << vImuMeas.back().t << endl;
                     first_imu[seq]++;
                 }
             }
 
-            // cout << "first imu: " << first_imu[seq] << endl;
-            /*cout << "first imu time: " << fixed << vTimestampsImu[first_imu] << endl;
-            cout << "size vImu: " << vImuMeas.size() << endl;*/
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     #else
@@ -174,8 +163,7 @@ int main(int argc, char **argv)
     #endif
 
             // Pass the image to the SLAM system
-            // cout << "tframe = " << tframe << endl;
-            SLAM.TrackMonocular(im,tframe,vImuMeas); // TODO change to monocular_inertial
+            SLAM.TrackMonocular(im,tframe,vImuMeas);
 
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
@@ -185,7 +173,6 @@ int main(int argc, char **argv)
 
             double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
             ttrack_tot += ttrack;
-            // std::cout << "ttrack: " << ttrack << std::endl;
 
             vTimesTrack[ni]=ttrack;
 
@@ -197,7 +184,7 @@ int main(int argc, char **argv)
                 T = tframe-vTimestampsCam[seq][ni-1];
 
             if(ttrack<T)
-                usleep((T-ttrack)*1e6); // 1e6
+                usleep((T-ttrack)*1e6);
 
         }
         if(seq < num_seq - 1)
@@ -209,13 +196,9 @@ int main(int argc, char **argv)
 
     }
 
-    // cout << "ttrack_tot = " << ttrack_tot << std::endl;
     // Stop all threads
     SLAM.Shutdown();
 
-
-    // Tracking time statistics
-
     // Save camera trajectory
 
     if (bFileName)
@@ -241,12 +224,6 @@ int main(int argc, char **argv)
     cout << "median tracking time: " << vTimesTrack[nImages[0]/2] << endl;
     cout << "mean tracking time: " << totaltime/proccIm << endl;
 
-    /*const string kf_file =  "kf_" + ss.str() + ".txt";
-    const string f_file =  "f_" + ss.str() + ".txt";
-
-    SLAM.SaveTrajectoryEuRoC(f_file);
-    SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);*/
-
     return 0;
 }
 

Examples/Monocular/mono_euroc.cc

@@ -91,7 +91,7 @@ int main(int argc, char **argv)
         {
 
             // Read image from file
-            im = cv::imread(vstrImageFilenames[seq][ni],CV_LOAD_IMAGE_UNCHANGED);
+            im = cv::imread(vstrImageFilenames[seq][ni],cv::IMREAD_UNCHANGED);
             double tframe = vTimestampsCam[seq][ni];
 
             if(im.empty())
@@ -108,8 +108,7 @@ int main(int argc, char **argv)
     #endif
 
             // Pass the image to the SLAM system
-            // cout << "tframe = " << tframe << endl;
-            SLAM.TrackMonocular(im,tframe); // TODO change to monocular_inertial
+            SLAM.TrackMonocular(im,tframe);
 
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
@@ -117,6 +116,11 @@ int main(int argc, char **argv)
             std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
     #endif
 
+#ifdef REGISTER_TIMES
+            double t_track = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
+            SLAM.InsertTrackTime(t_track);
+#endif
+
             double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
 
             vTimesTrack[ni]=ttrack;
@@ -129,7 +133,7 @@ int main(int argc, char **argv)
                 T = tframe-vTimestampsCam[seq][ni-1];
 
             if(ttrack<T)
-                usleep((T-ttrack)*1e6); // 1e6
+                usleep((T-ttrack)*1e6);
         }
 
         if(seq < num_seq - 1)

Examples/Monocular/mono_kitti.cc

@@ -53,10 +53,6 @@ int main(int argc, char **argv)
     vector<float> vTimesTrack;
     vTimesTrack.resize(nImages);
 
-    cout << endl << "-------" << endl;
-    cout << "Start processing sequence ..." << endl;
-    cout << "Images in the sequence: " << nImages << endl << endl;
-
     // Main loop
     cv::Mat im;
     for(int ni=0; ni<nImages; ni++)

Examples/Monocular/mono_tum.cc

@@ -55,10 +55,6 @@ int main(int argc, char **argv)
     vector<float> vTimesTrack;
     vTimesTrack.resize(nImages);
 
-    cout << endl << "-------" << endl;
-    cout << "Start processing sequence ..." << endl;
-    cout << "Images in the sequence: " << nImages << endl << endl;
-
     // Main loop
     cv::Mat im;
     for(int ni=0; ni<nImages; ni++)

Examples/Monocular/mono_tum_vi.cc

@@ -103,7 +103,7 @@ int main(int argc, char **argv)
         {
 
             // Read image from file
-            im = cv::imread(vstrImageFilenames[seq][ni],cv::IMREAD_GRAYSCALE);
+            im = cv::imread(vstrImageFilenames[seq][ni],cv::IMREAD_UNCHANGED);
 
             // clahe
             clahe->apply(im,im);
@@ -146,7 +146,7 @@ int main(int argc, char **argv)
                 T = tframe-vTimestampsCam[seq][ni-1];
 
             if(ttrack<T)
-                usleep((T-ttrack)*1e6); // 1e6
+                usleep((T-ttrack)*1e6);
 
         }
         if(seq < num_seq - 1)
@@ -158,13 +158,9 @@ int main(int argc, char **argv)
 
     }
 
-    // cout << "ttrack_tot = " << ttrack_tot << std::endl;
     // Stop all threads
     SLAM.Shutdown();
 
-
-    // Tracking time statistics
-
     // Save camera trajectory
 
     if (bFileName)

Examples/RGB-D/rgbd_tum.cc

@@ -65,17 +65,13 @@ int main(int argc, char **argv)
     vector<float> vTimesTrack;
     vTimesTrack.resize(nImages);
 
-    cout << endl << "-------" << endl;
-    cout << "Start processing sequence ..." << endl;
-    cout << "Images in the sequence: " << nImages << endl << endl;
-
     // Main loop
     cv::Mat imRGB, imD;
     for(int ni=0; ni<nImages; ni++)
     {
         // Read image and depthmap from file
-        imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],cv::IMREAD_UNCHANGED);
-        imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],cv::IMREAD_UNCHANGED);
+        imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],cv::IMREAD_UNCHANGED); //,cv::IMREAD_UNCHANGED);
+        imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],cv::IMREAD_UNCHANGED); //,cv::IMREAD_UNCHANGED);
         double tframe = vTimestamps[ni];
 
         if(imRGB.empty())

Examples/Stereo-Inertial/stereo_inertial_euroc.cc

@@ -167,6 +167,7 @@ int main(int argc, char **argv)
         // Seq loop
         vector<ORB_SLAM3::IMU::Point> vImuMeas;
         double t_rect = 0;
+        double t_track = 0;
         int num_rect = 0;
         int proccIm = 0;
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
@@ -190,28 +191,32 @@ int main(int argc, char **argv)
             }
 
 
+#ifdef REGISTER_TIMES
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t_Start_Rect = std::chrono::steady_clock::now();
     #else
             std::chrono::monotonic_clock::time_point t_Start_Rect = std::chrono::monotonic_clock::now();
     #endif
+#endif
             cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
             cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);
 
+#ifdef REGISTER_TIMES
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t_End_Rect = std::chrono::steady_clock::now();
     #else
             std::chrono::monotonic_clock::time_point t_End_Rect = std::chrono::monotonic_clock::now();
     #endif
-
-            t_rect = std::chrono::duration_cast<std::chrono::duration<double> >(t_End_Rect - t_Start_Rect).count();
+            t_rect = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Rect - t_Start_Rect).count();
+            SLAM.InsertRectTime(t_rect);
+#endif
             double tframe = vTimestampsCam[seq][ni];
 
             // Load imu measurements from previous frame
             vImuMeas.clear();
 
             if(ni>0)
-                while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) // while(vTimestampsImu[first_imu]<=vTimestampsCam[ni])
+                while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
                     vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
@@ -234,6 +239,11 @@ int main(int argc, char **argv)
             std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
     #endif
 
+#ifdef REGISTER_TIMES
+            t_track = t_rect + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
+            SLAM.InsertTrackTime(t_track);
+#endif
+
             double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
 
             vTimesTrack[ni]=ttrack;

Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc

@@ -114,10 +114,6 @@ int main(int argc, char **argv)
     cout << endl << "-------" << endl;
     cout.precision(17);
 
-    /*cout << "Start processing sequence ..." << endl;
-    cout << "Images in the sequence: " << nImages << endl;
-    cout << "IMU data in the sequence: " << nImu << endl << endl;*/
-
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
     ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO, true, 0, file_name);
 
@@ -156,23 +152,16 @@ int main(int argc, char **argv)
 
             if(ni>0)
             {
-                // cout << "t_cam " << tframe << endl;
 
                 while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni])
                 {
-                    // vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[first_imu],vGyro[first_imu],vTimestampsImu[first_imu]));
                     vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
                                                              vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
                                                              vTimestampsImu[seq][first_imu[seq]]));
-                    // cout << "t_imu = " << fixed << vImuMeas.back().t << endl;
                     first_imu[seq]++;
                 }
             }
 
-            /*cout << "first imu: " << first_imu[seq] << endl;
-            cout << "first imu time: " << fixed << vTimestampsImu[seq][0] << endl;
-            cout << "size vImu: " << vImuMeas.size() << endl;*/
-
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     #else
@@ -190,7 +179,6 @@ int main(int argc, char **argv)
 
             double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
             ttrack_tot += ttrack;
-            // std::cout << "ttrack: " << ttrack << std::endl;
 
             vTimesTrack[ni]=ttrack;
 
@@ -202,7 +190,7 @@ int main(int argc, char **argv)
                 T = tframe-vTimestampsCam[seq][ni-1];
 
             if(ttrack<T)
-                usleep((T-ttrack)*1e6); // 1e6
+                usleep((T-ttrack)*1e6);
         }
         if(seq < num_seq - 1)
         {
@@ -216,8 +204,6 @@ int main(int argc, char **argv)
     // Stop all threads
     SLAM.Shutdown();
 
-    // Tracking time statistics
-
     // Save camera trajectory
     std::chrono::system_clock::time_point scNow = std::chrono::system_clock::now();
     std::time_t now = std::chrono::system_clock::to_time_t(scNow);

Examples/Stereo/stereo_euroc.cc

@@ -135,6 +135,7 @@ int main(int argc, char **argv)
         // Seq loop
 
         double t_rect = 0;
+        double t_track = 0;
         int num_rect = 0;
         int proccIm = 0;
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
@@ -157,25 +158,26 @@ int main(int argc, char **argv)
                 return 1;
             }
 
-
+#ifdef REGISTER_TIMES
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t_Start_Rect = std::chrono::steady_clock::now();
     #else
             std::chrono::monotonic_clock::time_point t_Start_Rect = std::chrono::monotonic_clock::now();
     #endif
+#endif
             cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
             cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);
 
+#ifdef REGISTER_TIMES
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t_End_Rect = std::chrono::steady_clock::now();
     #else
             std::chrono::monotonic_clock::time_point t_End_Rect = std::chrono::monotonic_clock::now();
     #endif
-
-            t_rect = std::chrono::duration_cast<std::chrono::duration<double> >(t_End_Rect - t_Start_Rect).count();
+            t_rect = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Rect - t_Start_Rect).count();
+            SLAM.InsertRectTime(t_rect);
+#endif
             double tframe = vTimestampsCam[seq][ni];
-
-
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     #else
@@ -191,6 +193,11 @@ int main(int argc, char **argv)
             std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
     #endif
 
+#ifdef REGISTER_TIMES
+            t_track = t_rect + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
+            SLAM.InsertTrackTime(t_track);
+#endif
+
             double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
 
             vTimesTrack[ni]=ttrack;
@@ -203,7 +210,7 @@ int main(int argc, char **argv)
                 T = tframe-vTimestampsCam[seq][ni-1];
 
             if(ttrack<T)
-                usleep((T-ttrack)*1e6); // 1e6
+                usleep((T-ttrack)*1e6);
         }
 
         if(seq < num_seq - 1)

Examples/Stereo/stereo_kitti.cc

@@ -52,11 +52,7 @@ int main(int argc, char **argv)
 
     // Vector for tracking time statistics
     vector<float> vTimesTrack;
-    vTimesTrack.resize(nImages);
-
-    cout << endl << "-------" << endl;
-    cout << "Start processing sequence ..." << endl;
-    cout << "Images in the sequence: " << nImages << endl << endl;   
+    vTimesTrack.resize(nImages);  
 
     // Main loop
     cv::Mat imLeft, imRight;

Examples/Stereo/stereo_tum_vi.cc

@@ -137,7 +137,6 @@ int main(int argc, char **argv)
 
             double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
             ttrack_tot += ttrack;
-            // std::cout << "ttrack: " << ttrack << std::endl;
 
             vTimesTrack[ni]=ttrack;
 
@@ -149,7 +148,7 @@ int main(int argc, char **argv)
                 T = tframe-vTimestampsCam[seq][ni-1];
 
             if(ttrack<T)
-                usleep((T-ttrack)*1e6); // 1e6
+                usleep((T-ttrack)*1e6);
         }
         if(seq < num_seq - 1)
         {
@@ -163,8 +162,6 @@ int main(int argc, char **argv)
     // Stop all threads
     SLAM.Shutdown();
 
-    // Tracking time statistics
-
     // Save camera trajectory
     std::chrono::system_clock::time_point scNow = std::chrono::system_clock::now();
     std::time_t now = std::chrono::system_clock::to_time_t(scNow);

Examples/euroc_eval_examples.sh

@@ -0,0 +1,36 @@
+#!/bin/bash
+pathDatasetEuroc='/Datasets/EuRoC' #Example, it is necesary to change it by the dataset path
+
+# Single Session Example (Pure visual)
+echo "Launching MH01 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo
+echo "------------------------------------"
+echo "Evaluation of MH01 trajectory with Stereo sensor"
+python ../evaluation/evaluate_ate_scale.py ../evaluation/Ground_truth/EuRoC_left_cam/MH01_GT.txt f_dataset-MH01_stereo.txt --plot MH01_stereo.pdf
+
+
+
+# MultiSession Example (Pure visual)
+echo "Launching Machine Hall with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereo
+echo "------------------------------------"
+echo "Evaluation of MAchine Hall trajectory with Stereo sensor"
+python ../evaluation/evaluate_ate_scale.py ../evaluation/Ground_truth/EuRoC_left_cam/MH_GT.txt f_dataset-MH01_to_MH05_stereo.txt --plot MH01_to_MH05_stereo.pdf
+
+
+# Single Session Example (Visual-Inertial)
+echo "Launching V102 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_monoi
+echo "------------------------------------"
+echo "Evaluation of V102 trajectory with Monocular-Inertial sensor"
+python ../evaluation/evaluate_ate_scale.py "$pathDatasetEuroc"/V102/mav0/state_groundtruth_estimate0/data.csv f_dataset-V102_monoi.txt --plot V102_monoi.pdf
+
+
+# MultiSession Monocular Examples
+
+echo "Launching Vicon Room 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_monoi
+echo "------------------------------------"
+echo "Evaluation of Vicon Room 2 trajectory with Stereo sensor"
+python ../evaluation/evaluate_ate_scale.py ../evaluation/Ground_truth/EuRoC_imu/V2_GT.txt f_dataset-V201_to_V203_monoi.txt --plot V201_to_V203_monoi.pdf
+

Examples/euroc_examples.sh

@@ -0,0 +1,182 @@
+#!/bin/bash
+pathDatasetEuroc='/Datasets/EuRoC' #Example, it is necesary to change it by the dataset path
+
+#------------------------------------
+# Monocular Examples
+echo "Launching MH01 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular/EuRoC_TimeStamps/MH01.txt dataset-MH01_mono
+
+echo "Launching MH02 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Monocular/EuRoC_TimeStamps/MH02.txt dataset-MH02_mono
+
+echo "Launching MH03 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Monocular/EuRoC_TimeStamps/MH03.txt dataset-MH03_mono
+
+echo "Launching MH04 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Monocular/EuRoC_TimeStamps/MH04.txt dataset-MH04_mono
+
+echo "Launching MH05 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH05_mono
+
+echo "Launching V101 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular/EuRoC_TimeStamps/V101.txt dataset-V101_mono
+
+echo "Launching V102 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular/EuRoC_TimeStamps/V102.txt dataset-V102_mono
+
+echo "Launching V103 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Monocular/EuRoC_TimeStamps/V103.txt dataset-V103_mono
+
+echo "Launching V201 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular/EuRoC_TimeStamps/V201.txt dataset-V201_mono
+
+echo "Launching V202 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Monocular/EuRoC_TimeStamps/V202.txt dataset-V202_mono
+
+echo "Launching V203 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Monocular/EuRoC_TimeStamps/V203.txt dataset-V203_mono
+
+# MultiSession Monocular Examples
+echo "Launching Machine Hall with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Monocular/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Monocular/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Monocular/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_mono
+
+echo "Launching Vicon Room 1 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Monocular/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Monocular/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_mono
+
+echo "Launching Vicon Room 2 with Monocular sensor"
+./Monocular/mono_euroc ../Vocabulary/ORBvoc.txt ./Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_mono
+
+#------------------------------------
+# Stereo Examples
+echo "Launching MH01 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo
+
+echo "Launching MH02 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereo
+
+echo "Launching MH03 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereo
+
+echo "Launching MH04 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereo
+
+echo "Launching MH05 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereo
+
+echo "Launching V101 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo/EuRoC_TimeStamps/V101.txt dataset-V101_stereo
+
+echo "Launching V102 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Stereo/EuRoC_TimeStamps/V102.txt dataset-V102_stereo
+
+echo "Launching V103 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Stereo/EuRoC_TimeStamps/V103.txt dataset-V103_stereo
+
+echo "Launching V201 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo/EuRoC_TimeStamps/V201.txt dataset-V201_stereo
+
+echo "Launching V202 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Stereo/EuRoC_TimeStamps/V202.txt dataset-V202_stereo
+
+echo "Launching V203 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Stereo/EuRoC_TimeStamps/V203.txt dataset-V203_stereo
+
+# MultiSession Stereo Examples
+echo "Launching Machine Hall with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereo
+
+echo "Launching Vicon Room 1 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Stereo/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Stereo/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereo
+
+echo "Launching Vicon Room 2 with Stereo sensor"
+./Stereo/stereo_euroc ../Vocabulary/ORBvoc.txt ./Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Stereo/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Stereo/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereo
+
+#------------------------------------
+# Monocular-Inertial Examples
+echo "Launching MH01 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_monoi
+
+echo "Launching MH02 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Monocular-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_monoi
+
+echo "Launching MH03 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Monocular-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_monoi
+
+echo "Launching MH04 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Monocular-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_monoi
+
+echo "Launching MH05 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_monoi
+
+echo "Launching V101 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_monoi
+
+echo "Launching V102 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_monoi
+
+echo "Launching V103 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_monoi
+
+echo "Launching V201 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_monoi
+
+echo "Launching V202 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_monoi
+
+echo "Launching V203 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_monoi
+
+# MultiSession Monocular Examples
+echo "Launching Machine Hall with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Monocular-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Monocular-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Monocular-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Monocular-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_monoi
+
+echo "Launching Vicon Room 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Monocular-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Monocular-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_monoi
+
+echo "Launching Vicon Room 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_euroc ../Vocabulary/ORBvoc.txt ./Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Monocular-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Monocular-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_monoi
+
+#------------------------------------
+# Stereo-Inertial Examples
+echo "Launching MH01 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereoi
+
+echo "Launching MH02 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Stereo-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereoi
+
+echo "Launching MH03 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Stereo-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereoi
+
+echo "Launching MH04 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Stereo-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereoi
+
+echo "Launching MH05 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereoi
+
+echo "Launching V101 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_stereoi
+
+echo "Launching V102 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Stereo-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_stereoi
+
+echo "Launching V103 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_stereoi
+
+echo "Launching V201 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_stereoi
+
+echo "Launching V202 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Stereo-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_stereoi
+
+echo "Launching V203 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_stereoi
+
+# MultiSession Stereo-Inertial Examples
+echo "Launching Machine Hall with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Stereo-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Stereo-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Stereo-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Stereo-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereoi
+
+echo "Launching Vicon Room 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Stereo-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Stereo-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereoi
+
+echo "Launching Vicon Room 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_euroc ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Stereo-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Stereo-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereoi

Examples/tum_vi_eval_examples.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+pathDatasetTUM_VI='/Datasets/TUM_VI' #Example, it is necesary to change it by the dataset path
+
+
+# Single Session Example
+
+echo "Launching Magistrale 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt ./Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam1/data ./Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt ./Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt dataset-magistrale1_512_stereoi
+echo "------------------------------------"
+echo "Evaluation of Magistrale 1 trajectory with Stereo-Inertial sensor"
+python ../evaluation/evaluate_ate_scale.py "$pathDatasetTUM_VI"/magistrale1_512_16/mav0/mocap0/data.csv f_dataset-magistrale1_512_stereoi.txt --plot magistrale1_512_stereoi.pdf

Examples/tum_vi_examples.sh

@@ -0,0 +1,240 @@
+#!/bin/bash
+pathDatasetTUM_VI='/Datasets/TUM_VI' #Example, it is necesary to change it by the dataset path
+
+#------------------------------------
+# Monocular Examples
+echo "Launching Room 1 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room1_512.txt dataset-room1_512_mono
+
+echo "Launching Room 2 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room2_512.txt dataset-room2_512_mono
+
+echo "Launching Room 3 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room3_512.txt dataset-room3_512_mono
+
+echo "Launching Room 4 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room4_512.txt dataset-room4_512_mono
+
+echo "Launching Room 5 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room5_512.txt dataset-room5_512_mono
+
+echo "Launching Room 6 with Monocular sensor"
+./Monocular/mono_tum_vi ../Vocabulary/ORBvoc.txt Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data Monocular/TUM_TimeStamps/dataset-room6_512.txt dataset-room6_512_mono
+
+
+#------------------------------------
+# Stereo Examples
+echo "Launching Room 1 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room1_512.txt dataset-room1_512_stereo
+
+echo "Launching Room 2 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room2_512.txt dataset-room2_512_stereo
+
+echo "Launching Room 3 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room3_512.txt dataset-room3_512_stereo
+
+echo "Launching Room 4 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room4_512.txt dataset-room4_512_stereo
+
+echo "Launching Room 5 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room5_512.txt dataset-room5_512_stereo
+
+echo "Launching Room 6 with Stereo sensor"
+./Stereo/stereo_tum_vi ../Vocabulary/ORBvoc.txt Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam1/data Stereo/TUM_TimeStamps/dataset-room6_512.txt dataset-room6_512_stereo
+
+
+#------------------------------------
+# Monocular-Inertial Examples
+echo "Launching Corridor 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor1_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor1_512.txt dataset-corridor1_512_monoi
+
+echo "Launching Corridor 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor2_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor2_512.txt dataset-corridor2_512_monoi
+
+echo "Launching Corridor 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor3_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor3_512.txt dataset-corridor3_512_monoi
+
+echo "Launching Corridor 4 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor4_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor4_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor4_512.txt dataset-corridor4_512_monoi
+
+echo "Launching Corridor 5 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor5_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-corridor5_512.txt Monocular-Inertial/TUM_IMU/dataset-corridor5_512.txt dataset-corridor5_512_monoi
+
+
+echo "Launching Magistrale 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale1_512.txt dataset-magistrale1_512_monoi
+
+echo "Launching Magistrale 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale2_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale2_512.txt dataset-magistrale2_512
+
+echo "Launching Magistrale 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale3_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale3_512.txt dataset-magistrale3_512_monoi
+
+echo "Launching Magistrale 4 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale4_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale4_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale4_512.txt dataset-magistrale4_512_monoi
+
+echo "Launching Magistrale 5 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale5_512.txt dataset-magistrale5_512_monoi
+
+echo "Launching Magistrale 6 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale6_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-magistrale6_512.txt Monocular-Inertial/TUM_IMU/dataset-magistrale6_512.txt dataset-magistrale6_512_monoi
+
+
+echo "Launching Outdoor 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors1_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors1_512.txt dataset-outdoors1_512_monoi
+
+echo "Launching Outdoor 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors2_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors2_512.txt dataset-outdoors2_512_monoi
+
+echo "Launching Outdoor 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors3_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors3_512.txt dataset-outdoors3_512_monoi
+
+echo "Launching Outdoor 4 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors4_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors4_512.txt dataset-outdoors4_512_monoi
+
+echo "Launching Outdoor 5 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors5_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors5_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors5_512.txt dataset-outdoors5_512_monoi
+
+echo "Launching Outdoor 6 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors6_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors6_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors6_512.txt dataset-outdoors6_512_monoi
+
+echo "Launching Outdoor 7 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors7_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors7_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors7_512.txt dataset-outdoors7_512_monoi
+
+echo "Launching Outdoor 8 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors8_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-outdoors8_512.txt Monocular-Inertial/TUM_IMU/dataset-outdoors8_512.txt dataset-outdoors8_512_monoi
+
+
+echo "Launching Room 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room1_512.txt Monocular-Inertial/TUM_IMU/dataset-room1_512.txt dataset-room1_512_monoi
+
+echo "Launching Room 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room2_512.txt Monocular-Inertial/TUM_IMU/dataset-room2_512.txt dataset-room2_512_monoi
+
+echo "Launching Room 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room3_512.txt Monocular-Inertial/TUM_IMU/dataset-room3_512.txt dataset-room3_512_monoi
+
+echo "Launching Room 4 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room4_512.txt Monocular-Inertial/TUM_IMU/dataset-room4_512.txt dataset-room4_512_monoi
+
+echo "Launching Room 5 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room5_512.txt Monocular-Inertial/TUM_IMU/dataset-room5_512.txt dataset-room5_512_monoi
+
+echo "Launching Room 6 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-room6_512.txt Monocular-Inertial/TUM_IMU/dataset-room6_512.txt dataset-room6_512_monoi
+
+
+echo "Launching Slides 1 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Monocular-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-slides1_512_monoi
+
+echo "Launching Slides 2 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides2_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-slides2_512.txt Monocular-Inertial/TUM_IMU/dataset-slides2_512.txt dataset-slides2_512_monoi
+
+echo "Launching Slides 3 with Monocular-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides3_512_16/mav0/cam0/data Monocular-Inertial/TUM_TimeStamps/dataset-slides3_512.txt Monocular-Inertial/TUM_IMU/dataset-slides3_512.txt dataset-slides3_512_monoi
+
+
+# MultiSession Monocular Examples
+echo "Launching Room 1, Magistrale 1, Magistrale 5 and Slides 1 in the same session with Stereo-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale5_512.txt "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Stereo-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-room1_mag1_mag5_slides1_monoi
+
+echo "Launching all Rooms (1-6) in the same session with Stereo-Inertial sensor"
+./Monocular-Inertial/mono_inertial_tum_vi ../Vocabulary/ORBvoc.txt Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room2_512.txt Stereo-Inertial/TUM_IMU/dataset-room2_512.txt "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room3_512.txt Stereo-Inertial/TUM_IMU/dataset-room3_512.txt "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room4_512.txt Stereo-Inertial/TUM_IMU/dataset-room4_512.txt "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room5_512.txt Stereo-Inertial/TUM_IMU/dataset-room5_512.txt "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data Stereo-Inertial/TUM_TimeStamps/dataset-room6_512.txt Stereo-Inertial/TUM_IMU/dataset-room6_512.txt dataset-rooms123456_monoi
+
+#------------------------------------
+# Stereo-Inertial Examples
+echo "Launching Corridor 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor1_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor1_512.txt dataset-corridor1_512_stereoi
+
+echo "Launching Corridor 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor2_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor2_512.txt dataset-corridor2_512_stereoi
+
+echo "Launching Corridor 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor3_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor3_512.txt dataset-corridor3_512_stereoi
+
+echo "Launching Corridor 4 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor4_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor4_512.txt dataset-corridor4_512_stereoi
+
+echo "Launching Corridor 5 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-corridor5_512.txt Stereo-Inertial/TUM_IMU/dataset-corridor5_512.txt dataset-corridor5_512_stereoi
+
+
+echo "Launching Magistrale 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt dataset-magistrale1_512_stereoi
+
+echo "Launching Magistrale 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale2_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale2_512.txt dataset-magistrale2_512_stereoi
+
+echo "Launching Magistrale 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale3_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale3_512.txt dataset-magistrale3_512_stereoi
+
+echo "Launching Magistrale 4 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale4_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale4_512.txt dataset-magistrale4_512_stereoi
+
+echo "Launching Magistrale 5 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale5_512.txt dataset-magistrale5_512_stereoi
+
+echo "Launching Magistrale 6 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale6_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale6_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale6_512.txt dataset-magistrale6_512_stereoi
+
+
+echo "Launching Outdoor 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors1_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors1_512.txt outdoors1_512_stereoi
+
+echo "Launching Outdoor 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors2_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors2_512.txt outdoors2_512_stereoi
+
+echo "Launching Outdoor 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors3_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors3_512.txt outdoors3_512
+
+echo "Launching Outdoor 4 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors4_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors4_512.txt outdoors4_512
+
+echo "Launching Outdoor 5 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors5_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors5_512.txt outdoors5_512_stereoi
+
+echo "Launching Outdoor 6 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors6_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors6_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors6_512.txt outdoors6_512_stereoi
+
+echo "Launching Outdoor 7 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors7_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors7_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors7_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors7_512.txt outdoors7_512_stereoi
+
+echo "Launching Outdoor 8 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors8_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors8_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-outdoors8_512.txt Stereo-Inertial/TUM_IMU/dataset-outdoors8_512.txt outdoors8_512_stereoi
+
+
+echo "Launching Room 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt dataset-room1_512_stereoi
+
+echo "Launching Room 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room2_512.txt Stereo-Inertial/TUM_IMU/dataset-room2_512.txt dataset-room2_512_stereoi
+
+echo "Launching Room 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room3_512.txt Stereo-Inertial/TUM_IMU/dataset-room3_512.txt dataset-room3_512_stereoi
+
+echo "Launching Room 4 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room4_512.txt Stereo-Inertial/TUM_IMU/dataset-room4_512.txt dataset-room4_512_stereoi
+
+echo "Launching Room 5 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room5_512.txt Stereo-Inertial/TUM_IMU/dataset-room5_512.txt dataset-room5_512_stereoi
+
+echo "Launching Room 6 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room6_512.txt Stereo-Inertial/TUM_IMU/dataset-room6_512.txt dataset-room6_512_stereoi
+
+
+echo "Launching Slides 1 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Stereo-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-slides1_512_stereoi
+
+echo "Launching Slides 2 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-slides2_512.txt Stereo-Inertial/TUM_IMU/dataset-slides2_512.txt dataset-slides2_512_stereoi
+
+echo "Launching Slides 3 with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-slides3_512.txt Stereo-Inertial/TUM_IMU/dataset-slides3_512.txt dataset-slides3_512_stereoi
+
+
+# MultiSession Stereo-Inertial Examples
+echo "Launching Room 1, Magistrale 1, Magistrale 5 and Slides 1 in the same session with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Stereo-Inertial/TUM_IMU/dataset-magistrale5_512.txt "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Stereo-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-room1_mag1_mag5_slides1_stereoi
+
+echo "Launching all Rooms (1-6) in the same session with Stereo-Inertial sensor"
+./Stereo-Inertial/stereo_inertial_tum_vi ../Vocabulary/ORBvoc.txt Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room2_512.txt Stereo-Inertial/TUM_IMU/dataset-room2_512.txt "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room3_512.txt Stereo-Inertial/TUM_IMU/dataset-room3_512.txt "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room4_512.txt Stereo-Inertial/TUM_IMU/dataset-room4_512.txt "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room5_512.txt Stereo-Inertial/TUM_IMU/dataset-room5_512.txt "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam1/data Stereo-Inertial/TUM_TimeStamps/dataset-room6_512.txt Stereo-Inertial/TUM_IMU/dataset-room6_512.txt dataset-rooms123456_stereoi

README.md

@@ -1,6 +1,6 @@
 # ORB-SLAM3
 
-### V0.3: Beta version, 4 Sep 2020
+### V0.4: Beta version, 21 April 2021
 **Authors:** Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, [José M. M. Montiel](http://webdiis.unizar.es/~josemari/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/).
 
 The [Changelog](https://github.com/UZ-SLAMLab/ORB_SLAM3/Changelog.md) describes the features of each version.
@@ -16,7 +16,7 @@ alt="ORB-SLAM3" width="240" height="180" border="10" /></a>
 
 ### Related Publications:
 
-[ORB-SLAM3] Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M. M. Montiel and Juan D. Tardós, **ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM**, Under review. **[PDF](https://arxiv.org/pdf/2007.11898.pdf)**.
+[ORB-SLAM3] Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M. M. Montiel and Juan D. Tardós, **ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM**, *IEEE Transactions on Robotics, 2021* **[PDF](https://arxiv.org/abs/2007.11898)**.
 
 [IMU-Initialization] Carlos Campos, J. M. M. Montiel and Juan D. Tardós, **Inertial-Only Optimization for Visual-Inertial Initialization**, *ICRA 2020*. **[PDF](https://arxiv.org/pdf/2003.05766.pdf)**
 
@@ -209,5 +209,6 @@ Once ORB-SLAM3 has loaded the vocabulary, press space in the rosbag tab.
   rosrun rosbag fastrebag.py dataset-room1_512_16.bag dataset-room1_512_16_small_chunks.bag
   ```
 
-
+# 7. Time analysis
+A flag in `include\Config.h` activates time measurements. It is necessary to uncomment the line `#define REGISTER_TIMES` to obtain the time stats of one execution which is shown at the terminal and stored in a text file(`ExecTimeMean.txt`).
 

Thirdparty/DBoW2/CMakeLists.txt

@@ -24,11 +24,11 @@ set(SRCS_DUTILS
   DUtils/Random.cpp
   DUtils/Timestamp.cpp)
 
-find_package(OpenCV 3.0 QUIET)
+find_package(OpenCV 4.0 QUIET)
 if(NOT OpenCV_FOUND)
-   find_package(OpenCV 2.4.3 QUIET)
+   find_package(OpenCV 3.0 QUIET)
    if(NOT OpenCV_FOUND)
-      message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
+      message(FATAL_ERROR "OpenCV > 3.0 not found.")
    endif()
 endif()
 

euroc_eval_examples.sh

@@ -1,36 +0,0 @@
-#!/bin/bash
-pathDatasetEuroc='/Datasets/EuRoC' #Example, it is necesary to change it by the dataset path
-
-# Single Session Example (Pure visual)
-echo "Launching MH01 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Stereo/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo
-echo "------------------------------------"
-echo "Evaluation of MH01 trajectory with Stereo sensor"
-python evaluation/evaluate_ate_scale.py evaluation/Ground_truth/EuRoC_left_cam/MH01_GT.txt f_dataset-MH01_stereo.txt --plot MH01_stereo.pdf
-
-
-
-# MultiSession Example (Pure visual)
-echo "Launching Machine Hall with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Stereo/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Examples/Stereo/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Examples/Stereo/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Examples/Stereo/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Examples/Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereo
-echo "------------------------------------"
-echo "Evaluation of MAchine Hall trajectory with Stereo sensor"
-python evaluation/evaluate_ate_scale.py evaluation/Ground_truth/EuRoC_left_cam/MH_GT.txt f_dataset-MH01_to_MH05_stereo.txt --plot MH01_to_MH05_stereo.pdf
-
-
-# Single Session Example (Visual-Inertial)
-echo "Launching V102 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_monoi
-echo "------------------------------------"
-echo "Evaluation of V102 trajectory with Monocular-Inertial sensor"
-python evaluation/evaluate_ate_scale.py "$pathDatasetEuroc"/V102/mav0/state_groundtruth_estimate0/data.csv f_dataset-V102_monoi.txt --plot V102_monoi.pdf
-
-
-# MultiSession Monocular Examples
-
-echo "Launching Vicon Room 2 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_monoi
-echo "------------------------------------"
-echo "Evaluation of Vicon Room 2 trajectory with Stereo sensor"
-python evaluation/evaluate_ate_scale.py evaluation/Ground_truth/EuRoC_imu/V2_GT.txt f_dataset-V201_to_V203_monoi.txt --plot V201_to_V203_monoi.pdf
-

euroc_examples.sh

@@ -1,182 +0,0 @@
-#!/bin/bash
-pathDatasetEuroc='/Datasets/EuRoC' #Example, it is necesary to change it by the dataset path
-
-#------------------------------------
-# Monocular Examples
-echo "Launching MH01 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Monocular/EuRoC_TimeStamps/MH01.txt dataset-MH01_mono
-
-echo "Launching MH02 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Examples/Monocular/EuRoC_TimeStamps/MH02.txt dataset-MH02_mono
-
-echo "Launching MH03 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Examples/Monocular/EuRoC_TimeStamps/MH03.txt dataset-MH03_mono
-
-echo "Launching MH04 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Examples/Monocular/EuRoC_TimeStamps/MH04.txt dataset-MH04_mono
-
-echo "Launching MH05 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Examples/Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH05_mono
-
-echo "Launching V101 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Examples/Monocular/EuRoC_TimeStamps/V101.txt dataset-V101_mono
-
-echo "Launching V102 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Examples/Monocular/EuRoC_TimeStamps/V102.txt dataset-V102_mono
-
-echo "Launching V103 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Examples/Monocular/EuRoC_TimeStamps/V103.txt dataset-V103_mono
-
-echo "Launching V201 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Examples/Monocular/EuRoC_TimeStamps/V201.txt dataset-V201_mono
-
-echo "Launching V202 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Examples/Monocular/EuRoC_TimeStamps/V202.txt dataset-V202_mono
-
-echo "Launching V203 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Examples/Monocular/EuRoC_TimeStamps/V203.txt dataset-V203_mono
-
-# MultiSession Monocular Examples
-echo "Launching Machine Hall with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Monocular/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Examples/Monocular/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Examples/Monocular/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Examples/Monocular/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Examples/Monocular/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_mono
-
-echo "Launching Vicon Room 1 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Examples/Monocular/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Examples/Monocular/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Examples/Monocular/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_mono
-
-echo "Launching Vicon Room 2 with Monocular sensor"
-./Examples/Monocular/mono_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Examples/Monocular/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Examples/Monocular/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Examples/Monocular/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_mono
-
-#------------------------------------
-# Stereo Examples
-echo "Launching MH01 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Stereo/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo
-
-echo "Launching MH02 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Examples/Stereo/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereo
-
-echo "Launching MH03 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Examples/Stereo/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereo
-
-echo "Launching MH04 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Examples/Stereo/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereo
-
-echo "Launching MH05 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Examples/Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereo
-
-echo "Launching V101 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Examples/Stereo/EuRoC_TimeStamps/V101.txt dataset-V101_stereo
-
-echo "Launching V102 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Examples/Stereo/EuRoC_TimeStamps/V102.txt dataset-V102_stereo
-
-echo "Launching V103 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Examples/Stereo/EuRoC_TimeStamps/V103.txt dataset-V103_stereo
-
-echo "Launching V201 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Examples/Stereo/EuRoC_TimeStamps/V201.txt dataset-V201_stereo
-
-echo "Launching V202 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Examples/Stereo/EuRoC_TimeStamps/V202.txt dataset-V202_stereo
-
-echo "Launching V203 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Examples/Stereo/EuRoC_TimeStamps/V203.txt dataset-V203_stereo
-
-# MultiSession Stereo Examples
-echo "Launching Machine Hall with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Stereo/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Examples/Stereo/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Examples/Stereo/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Examples/Stereo/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Examples/Stereo/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereo
-
-echo "Launching Vicon Room 1 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Examples/Stereo/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Examples/Stereo/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Examples/Stereo/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereo
-
-echo "Launching Vicon Room 2 with Stereo sensor"
-./Examples/Stereo/stereo_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Examples/Stereo/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Examples/Stereo/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Examples/Stereo/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereo
-
-#------------------------------------
-# Monocular-Inertial Examples
-echo "Launching MH01 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_monoi
-
-echo "Launching MH02 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_monoi
-
-echo "Launching MH03 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_monoi
-
-echo "Launching MH04 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_monoi
-
-echo "Launching MH05 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_monoi
-
-echo "Launching V101 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_monoi
-
-echo "Launching V102 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_monoi
-
-echo "Launching V103 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_monoi
-
-echo "Launching V201 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_monoi
-
-echo "Launching V202 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_monoi
-
-echo "Launching V203 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_monoi
-
-# MultiSession Monocular Examples
-echo "Launching Machine Hall with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_monoi
-
-echo "Launching Vicon Room 1 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_monoi
-
-echo "Launching Vicon Room 2 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Examples/Monocular-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_monoi
-
-#------------------------------------
-# Stereo-Inertial Examples
-echo "Launching MH01 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereoi
-
-echo "Launching MH02 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH02 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH02.txt dataset-MH02_stereoi
-
-echo "Launching MH03 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH03 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH03.txt dataset-MH03_stereoi
-
-echo "Launching MH04 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH04 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH04.txt dataset-MH04_stereoi
-
-echo "Launching MH05 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH05 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH05_stereoi
-
-echo "Launching V101 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V101.txt dataset-V101_stereoi
-
-echo "Launching V102 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V102 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V102.txt dataset-V102_stereoi
-
-echo "Launching V103 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V103 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V103_stereoi
-
-echo "Launching V201 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V201.txt dataset-V201_stereoi
-
-echo "Launching V202 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V202 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V202.txt dataset-V202_stereoi
-
-echo "Launching V203 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V203 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V203_stereoi
-
-# MultiSession Stereo-Inertial Examples
-echo "Launching Machine Hall with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/MH01 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH01.txt "$pathDatasetEuroc"/MH02 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH02.txt "$pathDatasetEuroc"/MH03 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH03.txt "$pathDatasetEuroc"/MH04 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH04.txt "$pathDatasetEuroc"/MH05 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH05.txt dataset-MH01_to_MH05_stereoi
-
-echo "Launching Vicon Room 1 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V101 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V101.txt "$pathDatasetEuroc"/V102 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V102.txt "$pathDatasetEuroc"/V103 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V103.txt dataset-V101_to_V103_stereoi
-
-echo "Launching Vicon Room 2 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_euroc ./Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml "$pathDatasetEuroc"/V201 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V201.txt "$pathDatasetEuroc"/V202 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V202.txt "$pathDatasetEuroc"/V203 ./Examples/Stereo-Inertial/EuRoC_TimeStamps/V203.txt dataset-V201_to_V203_stereoi

include/Atlas.h

@@ -43,33 +43,8 @@ class Frame;
 class KannalaBrandt8;
 class Pinhole;
 
-//BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")
-//BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")
-
 class Atlas
 {
-    friend class boost::serialization::access;
-
-    template<class Archive>
-    void serialize(Archive &ar, const unsigned int version)
-    {
-        //ar.template register_type<Pinhole>();
-        //ar.template register_type<KannalaBrandt8>();
-
-        // Save/load the set of maps, the set is broken in libboost 1.58 for ubuntu 16.04
-        //ar & mspMaps;
-        ar & mvpBackupMaps;
-        ar & mvpCameras;
-        //ar & mvpBackupCamPin;
-        //ar & mvpBackupCamKan;
-        // Need to save/load the static Id from Frame, KeyFrame, MapPoint and Map
-        ar & Map::nNextId;
-        ar & Frame::nNextId;
-        ar & KeyFrame::nNextId;
-        ar & MapPoint::nNextId;
-        ar & GeometricCamera::nNextId;
-        ar & mnLastInitKFidMap;
-    }
 
 public:
     Atlas();
@@ -86,8 +61,6 @@ public:
     // Method for change components in the current map
     void AddKeyFrame(KeyFrame* pKF);
     void AddMapPoint(MapPoint* pMP);
-    //void EraseMapPoint(MapPoint* pMP);
-    //void EraseKeyFrame(KeyFrame* pKF);
 
     void AddCamera(GeometricCamera* pCam);
 
@@ -122,10 +95,6 @@ public:
     void SetImuInitialized();
     bool isImuInitialized();
 
-    // Function for garantee the correction of serialization of this object
-    void PreSave();
-    void PostLoad();
-
     void SetKeyFrameDababase(KeyFrameDatabase* pKFDB);
     KeyFrameDatabase* GetKeyFrameDatabase();
 
@@ -140,15 +109,12 @@ protected:
 
     std::set<Map*> mspMaps;
     std::set<Map*> mspBadMaps;
-    // Its necessary change the container from set to vector because libboost 1.58 and Ubuntu 16.04 have an error with this cointainer
-    std::vector<Map*> mvpBackupMaps;
     Map* mpCurrentMap;
 
     std::vector<GeometricCamera*> mvpCameras;
     std::vector<KannalaBrandt8*> mvpBackupCamKan;
     std::vector<Pinhole*> mvpBackupCamPin;
 
-    //Pinhole testCam;
     std::mutex mMutexAtlas;
 
     unsigned long int mnLastInitKFidMap;

include/CameraModels/GeometricCamera.h

@@ -36,23 +36,13 @@
 namespace ORB_SLAM3 {
     class GeometricCamera {
 
-        friend class boost::serialization::access;
-
-        template<class Archive>
-        void serialize(Archive& ar, const unsigned int version)
-        {
-            ar & mnId;
-            ar & mnType;
-            ar & mvParameters;
-        }
-
-
     public:
         GeometricCamera() {}
         GeometricCamera(const std::vector<float> &_vParameters) : mvParameters(_vParameters) {}
         ~GeometricCamera() {}
 
         virtual cv::Point2f project(const cv::Point3f &p3D) = 0;
+        virtual cv::Point2f project(const cv::Matx31f &m3D) = 0;
         virtual cv::Point2f project(const cv::Mat& m3D) = 0;
         virtual Eigen::Vector2d project(const Eigen::Vector3d & v3D) = 0;
         virtual cv::Mat projectMat(const cv::Point3f& p3D) = 0;
@@ -61,6 +51,7 @@ namespace ORB_SLAM3 {
 
         virtual cv::Point3f unproject(const cv::Point2f &p2D) = 0;
         virtual cv::Mat unprojectMat(const cv::Point2f &p2D) = 0;
+        virtual cv::Matx31f unprojectMat_(const cv::Point2f &p2D) = 0;
 
         virtual cv::Mat projectJac(const cv::Point3f &p3D) = 0;
         virtual Eigen::Matrix<double,2,3> projectJac(const Eigen::Vector3d& v3D) = 0;
@@ -71,8 +62,10 @@ namespace ORB_SLAM3 {
                                              cv::Mat &R21, cv::Mat &t21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated) = 0;
 
         virtual cv::Mat toK() = 0;
+        virtual cv::Matx33f toK_() = 0;
 
         virtual bool epipolarConstrain(GeometricCamera* otherCamera, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc) = 0;
+        virtual bool epipolarConstrain_(GeometricCamera* otherCamera, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Matx33f& R12, const cv::Matx31f& t12, const float sigmaLevel, const float unc) = 0;
 
         float getParameter(const int i){return mvParameters[i];}
         void setParameter(const float p, const size_t i){mvParameters[i] = p;}

include/CameraModels/KannalaBrandt8.h

@@ -35,15 +35,6 @@
 namespace ORB_SLAM3 {
     class KannalaBrandt8 final : public GeometricCamera {
 
-    friend class boost::serialization::access;
-
-    template<class Archive>
-    void serialize(Archive& ar, const unsigned int version)
-    {
-        ar & boost::serialization::base_object<GeometricCamera>(*this);
-        ar & const_cast<float&>(precision);
-    }
-
     public:
         KannalaBrandt8() : precision(1e-6) {
             mvParameters.resize(8);
@@ -69,6 +60,7 @@ namespace ORB_SLAM3 {
         }
 
         cv::Point2f project(const cv::Point3f &p3D);
+        cv::Point2f project(const cv::Matx31f &m3D);
         cv::Point2f project(const cv::Mat& m3D);
         Eigen::Vector2d project(const Eigen::Vector3d & v3D);
         cv::Mat projectMat(const cv::Point3f& p3D);
@@ -77,6 +69,7 @@ namespace ORB_SLAM3 {
 
         cv::Point3f unproject(const cv::Point2f &p2D);
         cv::Mat unprojectMat(const cv::Point2f &p2D);
+        cv::Matx31f unprojectMat_(const cv::Point2f &p2D);
 
         cv::Mat projectJac(const cv::Point3f &p3D);
         Eigen::Matrix<double,2,3> projectJac(const Eigen::Vector3d& v3D);
@@ -87,10 +80,14 @@ namespace ORB_SLAM3 {
                                      cv::Mat &R21, cv::Mat &t21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated);
 
         cv::Mat toK();
+        cv::Matx33f toK_();
 
         bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc);
+        bool epipolarConstrain_(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Matx33f& R12, const cv::Matx31f& t12, const float sigmaLevel, const float unc);
+
 
         float TriangulateMatches(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc, cv::Mat& p3D);
+        float TriangulateMatches_(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Matx33f& R12, const cv::Matx31f& t12, const float sigmaLevel, const float unc, cv::Matx31f& p3D);
 
         std::vector<int> mvLappingArea;
 
@@ -110,6 +107,7 @@ namespace ORB_SLAM3 {
         TwoViewReconstruction* tvr;
 
         void Triangulate(const cv::Point2f &p1, const cv::Point2f &p2, const cv::Mat &Tcw1, const cv::Mat &Tcw2,cv::Mat &x3D);
+        void Triangulate_(const cv::Point2f &p1, const cv::Point2f &p2, const cv::Matx44f &Tcw1, const cv::Matx44f &Tcw2,cv::Matx31f &x3D);
     };
 }
 

include/CameraModels/Pinhole.h

@@ -35,14 +35,6 @@
 namespace ORB_SLAM3 {
     class Pinhole : public GeometricCamera {
 
-    friend class boost::serialization::access;
-
-    template<class Archive>
-    void serialize(Archive& ar, const unsigned int version)
-    {
-        ar & boost::serialization::base_object<GeometricCamera>(*this);
-    }
-
     public:
         Pinhole() {
             mvParameters.resize(4);
@@ -67,6 +59,7 @@ namespace ORB_SLAM3 {
         }
 
         cv::Point2f project(const cv::Point3f &p3D);
+        cv::Point2f project(const cv::Matx31f &m3D);
         cv::Point2f project(const cv::Mat &m3D);
         Eigen::Vector2d project(const Eigen::Vector3d & v3D);
         cv::Mat projectMat(const cv::Point3f& p3D);
@@ -75,6 +68,8 @@ namespace ORB_SLAM3 {
 
         cv::Point3f unproject(const cv::Point2f &p2D);
         cv::Mat unprojectMat(const cv::Point2f &p2D);
+        cv::Matx31f unprojectMat_(const cv::Point2f &p2D);
+
 
         cv::Mat projectJac(const cv::Point3f &p3D);
         Eigen::Matrix<double,2,3> projectJac(const Eigen::Vector3d& v3D);
@@ -85,8 +80,10 @@ namespace ORB_SLAM3 {
                                              cv::Mat &R21, cv::Mat &t21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated);
 
         cv::Mat toK();
+        cv::Matx33f toK_();
 
         bool epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Mat& R12, const cv::Mat& t12, const float sigmaLevel, const float unc);
+        bool epipolarConstrain_(GeometricCamera* pCamera2, const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, const cv::Matx33f& R12, const cv::Matx31f& t12, const float sigmaLevel, const float unc);
 
         bool matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther,
                                  cv::Mat& Tcw1, cv::Mat& Tcw2,
@@ -97,6 +94,7 @@ namespace ORB_SLAM3 {
         friend std::istream& operator>>(std::istream& os, Pinhole& ph);
     private:
         cv::Mat SkewSymmetricMatrix(const cv::Mat &v);
+        cv::Matx33f SkewSymmetricMatrix_(const cv::Matx31f &v);
 
         //Parameters vector corresponds to
         //      [fx, fy, cx, cy]

include/Config.h

@@ -0,0 +1,6 @@
+#ifndef CONFIG_H
+#define CONFIG_H
+
+//#define REGISTER_TIMES
+
+#endif // CONFIG_H

include/Frame.h

@@ -20,8 +20,6 @@
 #ifndef FRAME_H
 #define FRAME_H
 
-//#define SAVE_TIMES
-
 #include<vector>
 
 #include "Thirdparty/DBoW2/DBoW2/BowVector.h"
@@ -29,6 +27,7 @@
 
 #include "ImuTypes.h"
 #include "ORBVocabulary.h"
+#include "Config.h"
 
 #include <mutex>
 #include <opencv2/opencv.hpp>
@@ -61,9 +60,6 @@ public:
     // Constructor for Monocular cameras.
     Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
 
-    // Destructor
-    // ~Frame();
-
     // Extract ORB on the image. 0 for left image and 1 for right image.
     void ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1);
 
@@ -247,9 +243,10 @@ public:
 
     int mnDataset;
 
-    double mTimeStereoMatch;
+#ifdef REGISTER_TIMES
     double mTimeORB_Ext;
-
+    double mTimeStereoMatch;
+#endif
 
 private:
 
@@ -269,6 +266,10 @@ private:
     cv::Mat mtcw;
     //==mtwc
 
+    cv::Matx31f mOwx;
+    cv::Matx33f mRcwx;
+    cv::Matx31f mtcwx;
+
     bool mbImuPreintegrated;
 
     std::mutex *mpMutexImu;
@@ -295,6 +296,7 @@ public:
     std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];
 
     cv::Mat mTlr, mRlr, mtlr, mTrl;
+    cv::Matx34f mTrlx, mTlrx;
 
     Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, GeometricCamera* pCamera2, cv::Mat& Tlr,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
 

include/KeyFrame.h

@@ -51,233 +51,6 @@ class GeometricCamera;
 class KeyFrame
 {
 
-
-    template<class Archive>
-    void serializeMatrix(Archive& ar, cv::Mat& mat, const unsigned int version)
-    {
-        int cols, rows, type;
-        bool continuous;
-
-        if (Archive::is_saving::value) {
-            cols = mat.cols; rows = mat.rows; type = mat.type();
-            continuous = mat.isContinuous();
-        }
-
-        ar & cols & rows & type & continuous;
-
-        if (Archive::is_loading::value)
-            mat.create(rows, cols, type);
-
-        if (continuous) {
-            const unsigned int data_size = rows * cols * mat.elemSize();
-            ar & boost::serialization::make_array(mat.ptr(), data_size);
-        } else {
-            const unsigned int row_size = cols*mat.elemSize();
-            for (int i = 0; i < rows; i++) {
-                ar & boost::serialization::make_array(mat.ptr(i), row_size);
-            }
-        }
-    }
-
-
-    template<class Archive>
-    void serializeMatrix(Archive& ar, const cv::Mat& mat, const unsigned int version)
-    {
-        cv::Mat matAux = mat;
-
-        serializeMatrix(ar, matAux,version);
-
-        if (Archive::is_loading::value)
-        {
-            cv::Mat* ptr;
-            ptr = (cv::Mat*)( &mat );
-            *ptr = matAux;
-        }
-    }
-
-    friend class boost::serialization::access;
-    template<class Archive>
-    void serializeVectorKeyPoints(Archive& ar, const vector<cv::KeyPoint>& vKP, const unsigned int version)
-    {
-        int NumEl;
-
-        if (Archive::is_saving::value) {
-            NumEl = vKP.size();
-        }
-
-        ar & NumEl;
-
-        vector<cv::KeyPoint> vKPaux = vKP;
-        if (Archive::is_loading::value)
-            vKPaux.reserve(NumEl);
-
-        for(int i=0; i < NumEl; ++i)
-        {
-            cv::KeyPoint KPi;
-
-            if (Archive::is_loading::value)
-                KPi = cv::KeyPoint();
-
-            if (Archive::is_saving::value)
-                KPi = vKPaux[i];
-
-            ar & KPi.angle;
-            ar & KPi.response;
-            ar & KPi.size;
-            ar & KPi.pt.x;
-            ar & KPi.pt.y;
-            ar & KPi.class_id;
-            ar & KPi.octave;
-
-            if (Archive::is_loading::value)
-                vKPaux.push_back(KPi);
-        }
-
-
-        if (Archive::is_loading::value)
-        {
-            vector<cv::KeyPoint> *ptr;
-            ptr = (vector<cv::KeyPoint>*)( &vKP );
-            *ptr = vKPaux;
-        }
-    }
-
-    template<class Archive>
-    void serialize(Archive& ar, const unsigned int version)
-    {
-        ar & mnId;
-        ar & const_cast<long unsigned int&>(mnFrameId);
-        ar & const_cast<double&>(mTimeStamp);
-        // Grid
-        ar & const_cast<int&>(mnGridCols);
-        ar & const_cast<int&>(mnGridRows);
-        ar & const_cast<float&>(mfGridElementWidthInv);
-        ar & const_cast<float&>(mfGridElementHeightInv);
-        // Variables of tracking
-        ar & mnTrackReferenceForFrame;
-        ar & mnFuseTargetForKF;
-        // Variables of local mapping
-        ar & mnBALocalForKF;
-        ar & mnBAFixedForKF;
-        ar & mnNumberOfOpt;
-        // Variables used by KeyFrameDatabase
-        ar & mnLoopQuery;
-        ar & mnLoopWords;
-        ar & mLoopScore;
-        ar & mnRelocQuery;
-        ar & mnRelocWords;
-        ar & mRelocScore;
-        ar & mnMergeQuery;
-        ar & mnMergeWords;
-        ar & mMergeScore;
-        ar & mnPlaceRecognitionQuery;
-        ar & mnPlaceRecognitionWords;
-        ar & mPlaceRecognitionScore;
-        ar & mbCurrentPlaceRecognition;
-        // Variables of loop closing
-        serializeMatrix(ar,mTcwGBA,version);
-        serializeMatrix(ar,mTcwBefGBA,version);
-        serializeMatrix(ar,mVwbGBA,version);
-        serializeMatrix(ar,mVwbBefGBA,version);
-        ar & mBiasGBA;
-        ar & mnBAGlobalForKF;
-        // Variables of Merging
-        serializeMatrix(ar,mTcwMerge,version);
-        serializeMatrix(ar,mTcwBefMerge,version);
-        serializeMatrix(ar,mTwcBefMerge,version);
-        serializeMatrix(ar,mVwbMerge,version);
-        serializeMatrix(ar,mVwbBefMerge,version);
-        ar & mBiasMerge;
-        ar & mnMergeCorrectedForKF;
-        ar & mnMergeForKF;
-        ar & mfScaleMerge;
-        ar & mnBALocalForMerge;
-        // Scale
-        ar & mfScale;
-        // Calibration parameters
-        ar & const_cast<float&>(fx);
-        ar & const_cast<float&>(fy);
-        ar & const_cast<float&>(invfx);
-        ar & const_cast<float&>(invfy);
-        ar & const_cast<float&>(cx);
-        ar & const_cast<float&>(cy);
-        ar & const_cast<float&>(mbf);
-        ar & const_cast<float&>(mb);
-        ar & const_cast<float&>(mThDepth);
-        serializeMatrix(ar,mDistCoef,version);
-        // Number of Keypoints
-        ar & const_cast<int&>(N);
-        // KeyPoints
-        serializeVectorKeyPoints(ar,mvKeys,version);
-        serializeVectorKeyPoints(ar,mvKeysUn,version);
-        ar & const_cast<vector<float>& >(mvuRight);
-        ar & const_cast<vector<float>& >(mvDepth);
-        serializeMatrix(ar,mDescriptors,version);
-        // BOW
-        ar & mBowVec;
-        ar & mFeatVec;
-        // Pose relative to parent
-        serializeMatrix(ar,mTcp,version);
-        // Scale
-        ar & const_cast<int&>(mnScaleLevels);
-        ar & const_cast<float&>(mfScaleFactor);
-        ar & const_cast<float&>(mfLogScaleFactor);
-        ar & const_cast<vector<float>& >(mvScaleFactors);
-        ar & const_cast<vector<float>& >(mvLevelSigma2);
-        ar & const_cast<vector<float>& >(mvInvLevelSigma2);
-        // Image bounds and calibration
-        ar & const_cast<int&>(mnMinX);
-        ar & const_cast<int&>(mnMinY);
-        ar & const_cast<int&>(mnMaxX);
-        ar & const_cast<int&>(mnMaxY);
-        serializeMatrix(ar,mK,version);
-        // Pose
-        serializeMatrix(ar,Tcw,version);
-        // MapPointsId associated to keypoints
-        ar & mvBackupMapPointsId;
-        // Grid
-        ar & mGrid;
-        // Connected KeyFrameWeight
-        ar & mBackupConnectedKeyFrameIdWeights;
-        // Spanning Tree and Loop Edges
-        ar & mbFirstConnection;
-        ar & mBackupParentId;
-        ar & mvBackupChildrensId;
-        ar & mvBackupLoopEdgesId;
-        ar & mvBackupMergeEdgesId;
-        // Bad flags
-        ar & mbNotErase;
-        ar & mbToBeErased;
-        ar & mbBad;
-
-        ar & mHalfBaseline;
-
-        // Camera variables
-        ar & mnBackupIdCamera;
-        ar & mnBackupIdCamera2;
-
-        // Fisheye variables
-        /*ar & mvLeftToRightMatch;
-        ar & mvRightToLeftMatch;
-        ar & NLeft;
-        ar & NRight;
-        serializeMatrix(ar, mTlr, version);
-        //serializeMatrix(ar, mTrl, version);
-        serializeVectorKeyPoints(ar, mvKeysRight, version);
-        ar & mGridRight;
-
-        // Inertial variables
-        ar & mImuBias;
-        ar & mBackupImuPreintegrated;
-        ar & mImuCalib;
-        ar & mBackupPrevKFId;
-        ar & mBackupNextKFId;
-        ar & bImu;
-        serializeMatrix(ar, Vw, version);
-        serializeMatrix(ar, Owb, version);*/
-
-    }
-
 public:
     KeyFrame();
     KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB);
@@ -297,6 +70,16 @@ public:
     cv::Mat GetTranslation();
     cv::Mat GetVelocity();
 
+    cv::Matx33f GetRotation_();
+    cv::Matx31f GetTranslation_();
+    cv::Matx31f GetCameraCenter_();
+    cv::Matx33f GetRightRotation_();
+    cv::Matx31f GetRightTranslation_();
+    cv::Matx44f GetRightPose_();
+    cv::Matx31f GetRightCameraCenter_();
+    cv::Matx44f GetPose_();
+
+
     // Bag of Words Representation
     void ComputeBoW();
 
@@ -343,6 +126,7 @@ public:
     // KeyPoint functions
     std::vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r, const bool bRight = false) const;
     cv::Mat UnprojectStereo(int i);
+    cv::Matx31f UnprojectStereo_(int i);
 
     // Image
     bool IsInImage(const float &x, const float &y) const;
@@ -377,10 +161,6 @@ public:
     bool ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v);
     bool ProjectPointUnDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v);
 
-    void PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP, set<GeometricCamera*>& spCam);
-    void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid, map<unsigned int, GeometricCamera*>& mpCamId);
-
-
     void SetORBVocabulary(ORBVocabulary* pORBVoc);
     void SetKeyFrameDatabase(KeyFrameDatabase* pKFDB);
 
@@ -516,6 +296,9 @@ protected:
     cv::Mat Ow;
     cv::Mat Cw; // Stereo middel point. Only for visualization
 
+    cv::Matx44f Tcw_, Twc_, Tlr_;
+    cv::Matx31f Ow_;
+
     // IMU position
     cv::Mat Owb;
 
@@ -527,8 +310,6 @@ protected:
 
     // MapPoints associated to keypoints
     std::vector<MapPoint*> mvpMapPoints;
-    // For save relation without pointer, this is necessary for save/load function
-    std::vector<long long int> mvBackupMapPointsId;
 
     // BoW
     KeyFrameDatabase* mpKeyFrameDB;
@@ -540,8 +321,6 @@ protected:
     std::map<KeyFrame*,int> mConnectedKeyFrameWeights;
     std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;
     std::vector<int> mvOrderedWeights;
-    // For save relation without pointer, this is necessary for save/load function
-    std::map<long unsigned int, int> mBackupConnectedKeyFrameIdWeights;
 
     // Spanning Tree and Loop Edges
     bool mbFirstConnection;
@@ -549,11 +328,6 @@ protected:
     std::set<KeyFrame*> mspChildrens;
     std::set<KeyFrame*> mspLoopEdges;
     std::set<KeyFrame*> mspMergeEdges;
-    // For save relation without pointer, this is necessary for save/load function
-    long long int mBackupParentId;
-    std::vector<long unsigned int> mvBackupChildrensId;
-    std::vector<long unsigned int> mvBackupLoopEdgesId;
-    std::vector<long unsigned int> mvBackupMergeEdgesId;
 
     // Bad flags
     bool mbNotErase;
@@ -569,14 +343,6 @@ protected:
     std::mutex mMutexFeatures;
     std::mutex mMutexMap;
 
-    // Backup variables for inertial
-    long long int mBackupPrevKFId;
-    long long int mBackupNextKFId;
-    IMU::Preintegrated mBackupImuPreintegrated;
-
-    // Backup for Cameras
-    unsigned int mnBackupIdCamera, mnBackupIdCamera2;
-
 public:
     GeometricCamera* mpCamera, *mpCamera2;
 
@@ -601,7 +367,7 @@ public:
     cv::Mat GetRightRotation();
     cv::Mat GetRightTranslation();
 
-    cv::Mat imgLeft, imgRight; //TODO Backup??
+    cv::Mat imgLeft, imgRight;
 
     void PrintPointDistribution(){
         int left = 0, right = 0;

include/KeyFrameDatabase.h

@@ -46,13 +46,6 @@ class Map;
 
 class KeyFrameDatabase
 {
-    friend class boost::serialization::access;
-
-    template<class Archive>
-    void serialize(Archive& ar, const unsigned int version)
-    {
-        ar & mvBackupInvertedFileId;
-    }
 
 public:
 
@@ -76,8 +69,6 @@ public:
    // Relocalization
    std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F, Map* pMap);
 
-   void PreSave();
-   void PostLoad(map<long unsigned int, KeyFrame*> mpKFid);
    void SetORBVocabulary(ORBVocabulary* pORBVoc);
 
 protected:
@@ -88,9 +79,6 @@ protected:
   // Inverted file
   std::vector<list<KeyFrame*> > mvInvertedFile;
 
-  // For save relation without pointer, this is necessary for save/load function
-  std::vector<list<long unsigned int> > mvBackupInvertedFileId;
-
   // Mutex
   std::mutex mMutex;
 };

include/LocalMapping.h

@@ -95,11 +95,6 @@ public:
     double mFirstTs;
     int mnMatchesInliers;
 
-    // For debugging (erase in normal mode)
-    int mInitFr;
-    int mIdxIteration;
-    string strSequence;
-
     bool mbNotBA1;
     bool mbNotBA2;
     bool mbBadImu;
@@ -109,6 +104,25 @@ public:
     // not consider far points (clouds)
     bool mbFarPoints;
     float mThFarPoints;
+
+#ifdef REGISTER_TIMES
+    vector<double> vdKFInsert_ms;
+    vector<double> vdMPCulling_ms;
+    vector<double> vdMPCreation_ms;
+    vector<double> vdLBA_ms;
+    vector<double> vdKFCulling_ms;
+    vector<double> vdLMTotal_ms;
+
+
+    vector<double> vdLBASync_ms;
+    vector<double> vdKFCullingSync_ms;
+    vector<int> vnLBA_edges;
+    vector<int> vnLBA_KFopt;
+    vector<int> vnLBA_KFfixed;
+    vector<int> vnLBA_MPs;
+    int nLBA_exec;
+    int nLBA_abort;
+#endif
 protected:
 
     bool CheckNewKeyFrames();
@@ -120,8 +134,10 @@ protected:
     void KeyFrameCulling();
 
     cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2);
+    cv::Matx33f ComputeF12_(KeyFrame* &pKF1, KeyFrame* &pKF2);
 
     cv::Mat SkewSymmetricMatrix(const cv::Mat &v);
+    cv::Matx33f SkewSymmetricMatrix_(const cv::Matx31f &v);
 
     System *mpSystem;
 

include/LoopClosing.h

@@ -25,6 +25,7 @@
 #include "Atlas.h"
 #include "ORBVocabulary.h"
 #include "Tracking.h"
+#include "Config.h"
 
 #include "KeyFrameDatabase.h"
 
@@ -48,7 +49,7 @@ public:
 
     typedef pair<set<KeyFrame*>,int> ConsistentGroup;    
     typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
-        Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;
+        Eigen::aligned_allocator<std::pair<KeyFrame* const, g2o::Sim3> > > KeyFrameAndPose;
 
 public:
 
@@ -86,6 +87,26 @@ public:
 
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
+#ifdef REGISTER_TIMES
+    double timeDetectBoW;
+
+    std::vector<double> vTimeBoW_ms;
+    std::vector<double> vTimeSE3_ms;
+    std::vector<double> vTimePRTotal_ms;
+
+    std::vector<double> vTimeLoopFusion_ms;
+    std::vector<double> vTimeLoopEssent_ms;
+    std::vector<double> vTimeLoopTotal_ms;
+
+    std::vector<double> vTimeMergeFusion_ms;
+    std::vector<double> vTimeMergeBA_ms;
+    std::vector<double> vTimeMergeTotal_ms;
+
+    std::vector<double> vTimeFullGBA_ms;
+    std::vector<double> vTimeMapUpdate_ms;
+    std::vector<double> vTimeGBATotal_ms;
+#endif
+
 protected:
 
     bool CheckNewKeyFrames();
@@ -112,9 +133,6 @@ protected:
     void MergeLocal();
     void MergeLocal2();
 
-    void CheckObservations(set<KeyFrame*> &spKFsMap1, set<KeyFrame*> &spKFsMap2);
-    void printReprojectionError(set<KeyFrame*> &spLocalWindowKFs, KeyFrame* mpCurrentKF, string &name);
-
     void ResetIfRequested();
     bool mbResetRequested;
     bool mbResetActiveMapRequested;

include/MLPnPsolver.h

@@ -65,8 +65,6 @@ namespace ORB_SLAM3{
         void SetRansacParameters(double probability = 0.99, int minInliers = 8, int maxIterations = 300, int minSet = 6, float epsilon = 0.4,
                                  float th2 = 5.991);
 
-        //Find metod is necessary?
-
         cv::Mat iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers);
 
         //Type definitions needed by the original code

include/Map.h

@@ -37,39 +37,10 @@ class MapPoint;
 class KeyFrame;
 class Atlas;
 class KeyFrameDatabase;
+class GeometricCamera;
 
 class Map
 {
-    friend class boost::serialization::access;
-
-    template<class Archive>
-    void serialize(Archive &ar, const unsigned int version)
-    {
-        ar & mnId;
-        ar & mnInitKFid;
-        ar & mnMaxKFid;
-        ar & mnBigChangeIdx;
-        // Set of KeyFrames and MapPoints, in this version the set serializator is not working
-        //ar & mspKeyFrames;
-        //ar & mspMapPoints;
-
-        ar & mvpBackupKeyFrames;
-        ar & mvpBackupMapPoints;
-
-        ar & mvBackupKeyFrameOriginsId;
-
-        ar & mnBackupKFinitialID;
-        ar & mnBackupKFlowerID;
-
-        ar & mbImuInitialized;
-        ar & mbIsInertial;
-        ar & mbIMU_BA1;
-        ar & mbIMU_BA2;
-
-        ar & mnInitKFid;
-        ar & mnMaxKFid;
-        ar & mnLastLoopKFid;
-    }
 
 public:
     Map();
@@ -134,11 +105,6 @@ public:
 
     unsigned int GetLowerKFID();
 
-    void PreSave(std::set<GeometricCamera*> &spCams);
-    void PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc, map<long unsigned int, KeyFrame*>& mpKeyFrameId, map<unsigned int, GeometricCamera*> &mpCams);
-
-    void printReprojectionError(list<KeyFrame*> &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder);
-
     vector<KeyFrame*> mvpKeyFrameOrigins;
     vector<unsigned long int> mvBackupKeyFrameOriginsId;
     KeyFrame* mpFirstRegionKF;
@@ -162,15 +128,9 @@ protected:
     std::set<MapPoint*> mspMapPoints;
     std::set<KeyFrame*> mspKeyFrames;
 
-    std::vector<MapPoint*> mvpBackupMapPoints;
-    std::vector<KeyFrame*> mvpBackupKeyFrames;
-
     KeyFrame* mpKFinitial;
     KeyFrame* mpKFlowerID;
 
-    unsigned long int mnBackupKFinitialID;
-    unsigned long int mnBackupKFlowerID;
-
     std::vector<MapPoint*> mvpReferenceMapPoints;
 
     bool mbImuInitialized;

include/MapPoint.h

@@ -40,89 +40,6 @@ class Frame;
 
 class MapPoint
 {
-    template<class Archive>
-    void serializeMatrix(Archive &ar, cv::Mat& mat, const unsigned int version)
-    {
-        int cols, rows, type;
-        bool continuous;
-
-        if (Archive::is_saving::value) {
-            cols = mat.cols; rows = mat.rows; type = mat.type();
-            continuous = mat.isContinuous();
-        }
-
-        ar & cols & rows & type & continuous;
-        if (Archive::is_loading::value)
-            mat.create(rows, cols, type);
-
-        if (continuous) {
-            const unsigned int data_size = rows * cols * mat.elemSize();
-            ar & boost::serialization::make_array(mat.ptr(), data_size);
-        } else {
-            const unsigned int row_size = cols*mat.elemSize();
-            for (int i = 0; i < rows; i++) {
-                ar & boost::serialization::make_array(mat.ptr(i), row_size);
-            }
-        }
-    }
-
-    friend class boost::serialization::access;
-    template<class Archive>
-    void serialize(Archive & ar, const unsigned int version)
-    {
-        ar & mnId;
-        ar & mnFirstKFid;
-        ar & mnFirstFrame;
-        ar & nObs;
-        // Variables used by the tracking
-        ar & mTrackProjX;
-        ar & mTrackProjY;
-        ar & mTrackDepth;
-        ar & mTrackDepthR;
-        ar & mTrackProjXR;
-        ar & mTrackProjYR;
-        ar & mbTrackInView;
-        ar & mbTrackInViewR;
-        ar & mnTrackScaleLevel;
-        ar & mnTrackScaleLevelR;
-        ar & mTrackViewCos;
-        ar & mTrackViewCosR;
-        ar & mnTrackReferenceForFrame;
-        ar & mnLastFrameSeen;
-
-        // Variables used by local mapping
-        ar & mnBALocalForKF;
-        ar & mnFuseCandidateForKF;
-
-        // Variables used by loop closing and merging
-        ar & mnLoopPointForKF;
-        ar & mnCorrectedByKF;
-        ar & mnCorrectedReference;
-        serializeMatrix(ar,mPosGBA,version);
-        ar & mnBAGlobalForKF;
-        ar & mnBALocalForMerge;
-        serializeMatrix(ar,mPosMerge,version);
-        serializeMatrix(ar,mNormalVectorMerge,version);
-
-        // Protected variables
-        serializeMatrix(ar,mWorldPos,version);
-        //ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId);
-        ar & mBackupObservationsId1;
-        ar & mBackupObservationsId2;
-        serializeMatrix(ar,mNormalVector,version);
-        serializeMatrix(ar,mDescriptor,version);
-        ar & mBackupRefKFId;
-        ar & mnVisible;
-        ar & mnFound;
-
-        ar & mbBad;
-        ar & mBackupReplacedId;
-
-        ar & mfMinDistance;
-        ar & mfMaxDistance;
-
-    }
-
 
 public:
     MapPoint();
@@ -136,6 +53,11 @@ public:
     cv::Mat GetWorldPos();
 
     cv::Mat GetNormal();
+
+    cv::Matx31f GetWorldPos2();
+
+    cv::Matx31f GetNormal2();
+
     KeyFrame* GetReferenceKeyFrame();
 
     std::map<KeyFrame*,std::tuple<int,int>> GetObservations();
@@ -175,11 +97,6 @@ public:
     Map* GetMap();
     void UpdateMap(Map* pMap);
 
-    void PrintObservations();
-
-    void PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP);
-    void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid);
-
 public:
     long unsigned int mnId;
     static long unsigned int nNextId;
@@ -231,22 +148,20 @@ protected:
 
      // Position in absolute coordinates
      cv::Mat mWorldPos;
+     cv::Matx31f mWorldPosx;
 
      // Keyframes observing the point and associated index in keyframe
      std::map<KeyFrame*,std::tuple<int,int> > mObservations;
-     // For save relation without pointer, this is necessary for save/load function
-     std::map<long unsigned int, int> mBackupObservationsId1;
-     std::map<long unsigned int, int> mBackupObservationsId2;
 
      // Mean viewing direction
      cv::Mat mNormalVector;
+     cv::Matx31f mNormalVectorx;
 
      // Best descriptor to fast matching
      cv::Mat mDescriptor;
 
      // Reference KeyFrame
      KeyFrame* mpRefKF;
-     long unsigned int mBackupRefKFId;
 
      // Tracking counters
      int mnVisible;
@@ -255,8 +170,6 @@ protected:
      // Bad flag (we do not currently erase MapPoint from memory)
      bool mbBad;
      MapPoint* mpReplaced;
-     // For save relation without pointer, this is necessary for save/load function
-     long long int mBackupReplacedId;
 
      // Scale invariance distances
      float mfMinDistance;

include/ORBextractor.h

@@ -21,7 +21,7 @@
 
 #include <vector>
 #include <list>
-#include <opencv/cv.h>
+#include <opencv2/opencv.hpp>
 
 
 namespace ORB_SLAM3

include/ORBmatcher.h

@@ -74,6 +74,9 @@ public:
     int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, cv::Mat F12,
                                std::vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo, const bool bCoarse = false);
 
+    int SearchForTriangulation_(KeyFrame *pKF1, KeyFrame* pKF2, cv::Matx33f F12,
+                               std::vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo, const bool bCoarse = false);
+
     int SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                            vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo, vector<cv::Mat> &vMatchedPoints);
 

include/Optimizer.h

@@ -55,7 +55,7 @@ public:
     void static FullInertialBA(Map *pMap, int its, const bool bFixLocal=false, const unsigned long nLoopKF=0, bool *pbStopFlag=NULL, bool bInit=false, float priorG = 1e2, float priorA=1e6, Eigen::VectorXd *vSingVal = NULL, bool *bHess=NULL);
 
     void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, vector<KeyFrame*> &vpNonEnoughOptKFs);
-    void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF);
+    void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges);
 
     void static MergeBundleAdjustmentVisual(KeyFrame* pCurrentKF, vector<KeyFrame*> vpWeldingKFs, vector<KeyFrame*> vpFixedKFs, bool *pbStopFlag);
 
@@ -96,7 +96,7 @@ public:
 
     // For inertial systems
 
-    void static LocalInertialBA(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, bool bLarge = false, bool bRecInit = false);
+    void static LocalInertialBA(KeyFrame* pKF, bool *pbStopFlag, Map *pMap, int& num_fixedKF, int& num_OptKF, int& num_MPs, int& num_edges, bool bLarge = false, bool bRecInit = false);
 
     void static MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF, bool *pbStopFlag, Map *pMap, LoopClosing::KeyFrameAndPose &corrPoses);
 

include/System.h

@@ -20,8 +20,6 @@
 #ifndef SYSTEM_H
 #define SYSTEM_H
 
-//#define SAVE_TIMES
-
 #include <unistd.h>
 #include<stdio.h>
 #include<stdlib.h>
@@ -39,6 +37,7 @@
 #include "ORBVocabulary.h"
 #include "Viewer.h"
 #include "ImuTypes.h"
+#include "Config.h"
 
 
 namespace ORB_SLAM3
@@ -151,9 +150,6 @@ public:
     void SaveTrajectoryEuRoC(const string &filename);
     void SaveKeyFrameTrajectoryEuRoC(const string &filename);
 
-    // Save data used for initialization debug
-    void SaveDebugData(const int &iniIdx);
-
     // Save camera trajectory in the KITTI dataset format.
     // Only for stereo and RGB-D. This method does not work for monocular.
     // Call first Shutdown()
@@ -177,13 +173,13 @@ public:
 
     void ChangeDataset();
 
-    //void SaveAtlas(int type);
+#ifdef REGISTER_TIMES
+    void InsertRectTime(double& time);
 
-private:
+    void InsertTrackTime(double& time);
+#endif
 
-    //bool LoadAtlas(string filename, int type);
-
-    //string CalculateCheckSum(string filename, int type);
+private:
 
     // Input sensor
     eSensor mSensor;
@@ -194,8 +190,7 @@ private:
     // KeyFrame database for place recognition (relocalization and loop detection).
     KeyFrameDatabase* mpKeyFrameDatabase;
 
-    // Map structure that stores the pointers to all KeyFrames and MapPoints.
-    //Map* mpMap;
+    // Atlas structure that stores the pointers to all KeyFrames and MapPoints.
     Atlas* mpAtlas;
 
     // Tracker. It receives a frame and computes the associated camera pose.

include/Tracking.h

@@ -158,13 +158,29 @@ public:
 
     vector<MapPoint*> GetLocalMapMPS();
 
+    bool mbWriteStats;
 
-    //TEST--
-    bool mbNeedRectify;
-    //cv::Mat M1l, M2l;
-    //cv::Mat M1r, M2r;
+#ifdef REGISTER_TIMES
+    void LocalMapStats2File();
+    void TrackStats2File();
+    void PrintTimeStats();
 
-    bool mbWriteStats;
+    vector<double> vdRectStereo_ms;
+    vector<double> vdORBExtract_ms;
+    vector<double> vdStereoMatch_ms;
+    vector<double> vdIMUInteg_ms;
+    vector<double> vdPosePred_ms;
+    vector<double> vdLMTrack_ms;
+    vector<double> vdNewKF_ms;
+    vector<double> vdTrackTotal_ms;
+
+    vector<double> vdUpdatedLM_ms;
+    vector<double> vdSearchLP_ms;
+    vector<double> vdPoseOpt_ms;
+#endif
+
+    vector<int> vnKeyFramesLM;
+    vector<int> vnMapPointsLM;
 
 protected:
 
@@ -203,7 +219,6 @@ protected:
     void PreintegrateIMU();
 
     // Reset IMU biases and compute frame velocity
-    void ResetFrameIMU();
     void ComputeGyroBias(const vector<Frame*> &vpFs, float &bwx,  float &bwy, float &bwz);
     void ComputeVelocitiesAccBias(const vector<Frame*> &vpFs, float &bax,  float &bay, float &baz);
 
@@ -294,6 +309,7 @@ protected:
     unsigned int mnLastRelocFrameId;
     double mTimeStampLost;
     double time_recently_lost;
+    double time_recently_lost_visual;
 
     unsigned int mnFirstFrameId;
     unsigned int mnInitialFrameId;

src/Atlas.cc

@@ -259,81 +259,6 @@ bool Atlas::isImuInitialized()
     return mpCurrentMap->isImuInitialized();
 }
 
-void Atlas::PreSave()
-{
-    if(mpCurrentMap){
-        if(!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
-            mnLastInitKFidMap = mpCurrentMap->GetMaxKFid()+1; //The init KF is the next of current maximum
-    }
-
-    struct compFunctor
-    {
-        inline bool operator()(Map* elem1 ,Map* elem2)
-        {
-            return elem1->GetId() < elem2->GetId();
-        }
-    };
-    std::copy(mspMaps.begin(), mspMaps.end(), std::back_inserter(mvpBackupMaps));
-    sort(mvpBackupMaps.begin(), mvpBackupMaps.end(), compFunctor());
-
-    std::set<GeometricCamera*> spCams(mvpCameras.begin(), mvpCameras.end());
-    cout << "There are " << spCams.size() << " cameras in the atlas" << endl;
-    for(Map* pMi : mvpBackupMaps)
-    {
-        cout << "Pre-save of map " << pMi->GetId() << endl;
-        pMi->PreSave(spCams);
-    }
-    cout << "Maps stored" << endl;
-    for(GeometricCamera* pCam : mvpCameras)
-    {
-        cout << "Pre-save of camera " << pCam->GetId() << endl;
-        if(pCam->GetType() == pCam->CAM_PINHOLE)
-        {
-            mvpBackupCamPin.push_back((Pinhole*) pCam);
-        }
-        else if(pCam->GetType() == pCam->CAM_FISHEYE)
-        {
-            mvpBackupCamKan.push_back((KannalaBrandt8*) pCam);
-        }
-    }
-
-}
-
-void Atlas::PostLoad()
-{
-    mvpCameras.clear();
-    map<unsigned int,GeometricCamera*> mpCams;
-    for(Pinhole* pCam : mvpBackupCamPin)
-    {
-        //mvpCameras.push_back((GeometricCamera*)pCam);
-        mvpCameras.push_back(pCam);
-        mpCams[pCam->GetId()] = pCam;
-    }
-    for(KannalaBrandt8* pCam : mvpBackupCamKan)
-    {
-        //mvpCameras.push_back((GeometricCamera*)pCam);
-        mvpCameras.push_back(pCam);
-        mpCams[pCam->GetId()] = pCam;
-    }
-
-    mspMaps.clear();
-    unsigned long int numKF = 0, numMP = 0;
-    map<long unsigned int, KeyFrame*> mpAllKeyFrameId;
-    for(Map* pMi : mvpBackupMaps)
-    {
-        cout << "Map id:" << pMi->GetId() << endl;
-        mspMaps.insert(pMi);
-        map<long unsigned int, KeyFrame*> mpKeyFrameId;
-        pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpKeyFrameId, mpCams);
-        mpAllKeyFrameId.insert(mpKeyFrameId.begin(), mpKeyFrameId.end());
-        numKF += pMi->GetAllKeyFrames().size();
-        numMP += pMi->GetAllMapPoints().size();
-    }
-
-    cout << "Number KF:" << numKF << "; number MP:" << numMP << endl;
-    mvpBackupMaps.clear();
-}
-
 void Atlas::SetKeyFrameDababase(KeyFrameDatabase* pKFDB)
 {
     mpKeyFrameDB = pKFDB;

src/CameraModels/KannalaBrandt8.cpp

@@ -20,10 +20,7 @@
 
 #include <boost/serialization/export.hpp>
 
-//BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::KannalaBrandt8)
-
 namespace ORB_SLAM3 {
-//BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")
 
     cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) {
         const float x2_plus_y2 = p3D.x * p3D.x + p3D.y * p3D.y;
@@ -43,6 +40,10 @@ namespace ORB_SLAM3 {
 
     }
 
+    cv::Point2f KannalaBrandt8::project(const cv::Matx31f &m3D) {
+        return this->project(cv::Point3f(m3D(0),m3D(1),m3D(2)));
+    }
+
     cv::Point2f KannalaBrandt8::project(const cv::Mat &m3D) {
         const float* p3D = m3D.ptr<float>();
 
@@ -67,14 +68,6 @@ namespace ORB_SLAM3 {
         res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3];
 
         return res;
-
-        /*cv::Point2f cvres = this->project(cv::Point3f(v3D[0],v3D[1],v3D[2]));
-
-        Eigen::Vector2d res;
-        res[0] = cvres.x;
-        res[1] = cvres.y;
-
-        return res;*/
     }
 
     cv::Mat KannalaBrandt8::projectMat(const cv::Point3f &p3D) {
@@ -85,12 +78,6 @@ namespace ORB_SLAM3 {
 
     float KannalaBrandt8::uncertainty2(const Eigen::Matrix<double,2,1> &p2D)
     {
-        /*Eigen::Matrix<double,2,1> c;
-        c << mvParameters[2], mvParameters[3];
-        if ((p2D-c).squaredNorm()>57600) // 240*240 (256)
-            return 100.f;
-        else
-            return 1.0f;*/
         return 1.f;
     }
 
@@ -100,6 +87,12 @@ namespace ORB_SLAM3 {
         return ret.clone();
     }
 
+        cv::Matx31f KannalaBrandt8::unprojectMat_(const cv::Point2f &p2D) {
+            cv::Point3f ray = this->unproject(p2D);
+            cv::Matx31f r{ray.x, ray.y, ray.z};
+            return r;
+    }
+
     cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) {
         //Use Newthon method to solve for theta with good precision (err ~ e-6)
         cv::Point2f pw((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1]);
@@ -232,11 +225,22 @@ namespace ORB_SLAM3 {
         return K;
     }
 
+    cv::Matx33f KannalaBrandt8::toK_() {
+        cv::Matx33f K{mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f};
+
+        return K;
+    }
+
     bool KannalaBrandt8::epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc) {
         cv::Mat p3D;
         return this->TriangulateMatches(pCamera2,kp1,kp2,R12,t12,sigmaLevel,unc,p3D) > 0.0001f;
     }
 
+    bool KannalaBrandt8::epipolarConstrain_(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc) {
+        cv::Matx31f p3D;
+        return this->TriangulateMatches_(pCamera2,kp1,kp2,R12,t12,sigmaLevel,unc,p3D) > 0.0001f;
+    }
+
     bool KannalaBrandt8::matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther,
                                              cv::Mat& Tcw1, cv::Mat& Tcw2,
                                             const float sigmaLevel1, const float sigmaLevel2,
@@ -357,8 +361,8 @@ namespace ORB_SLAM3 {
 
         cv::Mat x3D;
         cv::Mat Tcw1 = (cv::Mat_<float>(3,4) << 1.f,0.f,0.f,0.f,
-                                                           0.f,1.f,0.f,0.f,
-                                                           0.f,0.f,1.f,0.f);
+                                                0.f,1.f,0.f,0.f,
+                                                0.f,0.f,1.f,0.f);
         cv::Mat Tcw2;
         cv::Mat R21 = R12.t();
         cv::Mat t21 = -R21*t12;
@@ -402,6 +406,79 @@ namespace ORB_SLAM3 {
         return z1;
     }
 
+    float KannalaBrandt8::TriangulateMatches_(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc, cv::Matx31f& p3D) {
+        cv::Matx31f r1 = this->unprojectMat_(kp1.pt);
+        cv::Matx31f r2 = pCamera2->unprojectMat_(kp2.pt);
+
+        //Check parallax
+        cv::Matx31f r21 = R12*r2;
+
+        const float cosParallaxRays = r1.dot(r21)/(cv::norm(r1)*cv::norm(r21));
+
+        if(cosParallaxRays > 0.9998){
+            return -1;
+        }
+
+        //Parallax is good, so we try to triangulate
+        cv::Point2f p11,p22;
+
+        p11.x = r1(0);
+        p11.y = r1(1);
+
+        p22.x = r2(0);
+        p22.y = r2(1);
+
+        cv::Matx31f x3D;
+        cv::Matx44f Tcw1{1.f,0.f,0.f,0.f,
+                0.f,1.f,0.f,0.f,
+                0.f,0.f,1.f,0.f};
+
+        cv::Matx33f R21 = R12.t();
+        cv::Matx31f t21 = -R21*t12;
+
+        cv::Matx44f Tcw2{R21(0,0),R21(0,1),R21(0,2),t21(0),
+                         R21(1,0),R21(1,1),R21(1,2),t21(1),
+                         R21(2,0),R21(2,1),R21(2,2),t21(2),
+                         0.f,0.f,0.f,1.f};
+
+        Triangulate_(p11,p22,Tcw1,Tcw2,x3D);
+        cv::Matx13f x3Dt = x3D.t();
+
+        float z1 = x3D(2);
+        if(z1 <= 0){
+            return -1;
+        }
+
+        float z2 = R21.row(2).dot(x3Dt)+t21(2);
+        if(z2<=0){
+            return -1;
+        }
+
+        //Check reprojection error
+        cv::Point2f uv1 = this->project(x3D);
+
+        float errX1 = uv1.x - kp1.pt.x;
+        float errY1 = uv1.y - kp1.pt.y;
+
+        if((errX1*errX1+errY1*errY1)>5.991 * sigmaLevel){   //Reprojection error is high
+            return -1;
+        }
+
+        cv::Matx31f x3D2 = R21 * x3D + t21;
+        cv::Point2f uv2 = pCamera2->project(x3D2);
+
+        float errX2 = uv2.x - kp2.pt.x;
+        float errY2 = uv2.y - kp2.pt.y;
+
+        if((errX2*errX2+errY2*errY2)>5.991 * unc){   //Reprojection error is high
+            return -1;
+        }
+
+        p3D = x3D;
+
+        return z1;
+    }
+
     std::ostream & operator<<(std::ostream &os, const KannalaBrandt8 &kb) {
         os << kb.mvParameters[0] << " " << kb.mvParameters[1] << " " << kb.mvParameters[2] << " " << kb.mvParameters[3] << " "
            << kb.mvParameters[4] << " " << kb.mvParameters[5] << " " << kb.mvParameters[6] << " " << kb.mvParameters[7];
@@ -433,4 +510,27 @@ namespace ORB_SLAM3 {
         x3D = vt.row(3).t();
         x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
     }
+
+    void KannalaBrandt8::Triangulate_(const cv::Point2f &p1, const cv::Point2f &p2, const cv::Matx44f &Tcw1, const cv::Matx44f &Tcw2, cv::Matx31f &x3D)
+    {
+        cv::Matx14f A0, A1, A2, A3;
+
+
+        A0 = p1.x * Tcw1.row(2) - Tcw1.row(0);
+        A1 = p1.y * Tcw1.row(2) - Tcw1.row(1);
+        A2 = p2.x * Tcw2.row(2) - Tcw2.row(0);
+        A3 = p2.y * Tcw2.row(2) - Tcw2.row(1);
+        cv::Matx44f A{A0(0), A0(1), A0(2), A0(3),
+                      A1(0), A1(1), A1(2), A1(3),
+                      A2(0), A2(1), A2(2), A2(3),
+                      A3(0), A3(1), A3(2), A3(3)};
+
+        // cv::Mat u,w,vt;
+        cv::Matx44f u, vt;
+        cv::Matx41f w;
+
+        cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
+        cv::Matx41f x3D_h = vt.row(3).t();
+        x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3);
+    }
 }

src/CameraModels/Pinhole.cpp

@@ -20,10 +20,7 @@
 
 #include <boost/serialization/export.hpp>
 
-//BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::Pinhole)
-
 namespace ORB_SLAM3 {
-//BOOST_CLASS_EXPORT_GUID(Pinhole, "Pinhole")
 
     long unsigned int GeometricCamera::nNextId=0;
 
@@ -32,6 +29,10 @@ namespace ORB_SLAM3 {
                            mvParameters[1] * p3D.y / p3D.z + mvParameters[3]);
     }
 
+    cv::Point2f Pinhole::project(const cv::Matx31f &m3D) {
+        return this->project(cv::Point3f(m3D(0),m3D(1),m3D(2)));
+    }
+
     cv::Point2f Pinhole::project(const cv::Mat &m3D) {
         const float* p3D = m3D.ptr<float>();
 
@@ -66,6 +67,12 @@ namespace ORB_SLAM3 {
         return (cv::Mat_<float>(3,1) << ray.x, ray.y, ray.z);
     }
 
+    cv::Matx31f Pinhole::unprojectMat_(const cv::Point2f &p2D) {
+        cv::Point3f ray = this->unproject(p2D);
+        cv::Matx31f r{ray.x, ray.y, ray.z};
+        return r;
+    }
+
     cv::Mat Pinhole::projectJac(const cv::Point3f &p3D) {
         cv::Mat Jac(2, 3, CV_32F);
         Jac.at<float>(0, 0) = mvParameters[0] / p3D.z;
@@ -119,6 +126,12 @@ namespace ORB_SLAM3 {
         return K;
     }
 
+    cv::Matx33f Pinhole::toK_() {
+        cv::Matx33f K{mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f};
+
+        return K;
+    }
+
     bool Pinhole::epipolarConstrain(GeometricCamera* pCamera2,  const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &R12, const cv::Mat &t12, const float sigmaLevel, const float unc) {
         //Compute Fundamental Matrix
         cv::Mat t12x = SkewSymmetricMatrix(t12);
@@ -143,6 +156,30 @@ namespace ORB_SLAM3 {
         return dsqr<3.84*unc;
     }
 
+    bool Pinhole::epipolarConstrain_(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Matx33f &R12, const cv::Matx31f &t12, const float sigmaLevel, const float unc) {
+        //Compute Fundamental Matrix
+        auto t12x = SkewSymmetricMatrix_(t12);
+        auto K1 = this->toK_();
+        auto K2 = pCamera2->toK_();
+        cv::Matx33f F12 = K1.t().inv()*t12x*R12*K2.inv();
+
+        // Epipolar line in second image l = x1'F12 = [a b c]
+        const float a = kp1.pt.x*F12(0,0)+kp1.pt.y*F12(1,0)+F12(2,0);
+        const float b = kp1.pt.x*F12(0,1)+kp1.pt.y*F12(1,1)+F12(2,1);
+        const float c = kp1.pt.x*F12(0,2)+kp1.pt.y*F12(1,2)+F12(2,2);
+
+        const float num = a*kp2.pt.x+b*kp2.pt.y+c;
+
+        const float den = a*a+b*b;
+
+        if(den==0)
+            return false;
+
+        const float dsqr = num*num/den;
+
+        return dsqr<3.84*unc;
+    }
+
     std::ostream & operator<<(std::ostream &os, const Pinhole &ph) {
         os << ph.mvParameters[0] << " " << ph.mvParameters[1] << " " << ph.mvParameters[2] << " " << ph.mvParameters[3];
         return os;
@@ -165,4 +202,13 @@ namespace ORB_SLAM3 {
                 v.at<float>(2),               0,-v.at<float>(0),
                 -v.at<float>(1),  v.at<float>(0),              0);
     }
+
+    cv::Matx33f Pinhole::SkewSymmetricMatrix_(const cv::Matx31f &v)
+    {
+        cv::Matx33f skew{0.f, -v(2), v(1),
+                         v(2), 0.f, -v(0),
+                         -v(1), v(0), 0.f};
+
+        return skew;
+    }
 }

src/Converter.cc

@@ -190,7 +190,7 @@ std::vector<float> Converter::toEuler(const cv::Mat &R)
     assert(isRotationMatrix(R));
     float sy = sqrt(R.at<float>(0,0) * R.at<float>(0,0) +  R.at<float>(1,0) * R.at<float>(1,0) );
 
-    bool singular = sy < 1e-6; // If
+    bool singular = sy < 1e-6;
 
     float x, y, z;
     if (!singular)

src/Frame.cc

@@ -44,6 +44,10 @@ cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING);
 
 Frame::Frame(): mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevFrame(NULL), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false)
 {
+#ifdef REGISTER_TIMES
+    mTimeStereoMatch = 0;
+    mTimeORB_Ext = 0;
+#endif
 }
 
 
@@ -64,7 +68,8 @@ Frame::Frame(const Frame &frame)
      mpCamera(frame.mpCamera), mpCamera2(frame.mpCamera2), Nleft(frame.Nleft), Nright(frame.Nright),
      monoLeft(frame.monoLeft), monoRight(frame.monoRight), mvLeftToRightMatch(frame.mvLeftToRightMatch),
      mvRightToLeftMatch(frame.mvRightToLeftMatch), mvStereo3Dpoints(frame.mvStereo3Dpoints),
-     mTlr(frame.mTlr.clone()), mRlr(frame.mRlr.clone()), mtlr(frame.mtlr.clone()), mTrl(frame.mTrl.clone()), mTimeStereoMatch(frame.mTimeStereoMatch), mTimeORB_Ext(frame.mTimeORB_Ext)
+     mTlr(frame.mTlr.clone()), mRlr(frame.mRlr.clone()), mtlr(frame.mtlr.clone()), mTrl(frame.mTrl.clone()),
+     mTrlx(frame.mTrlx), mTlrx(frame.mTlrx), mOwx(frame.mOwx), mRcwx(frame.mRcwx), mtcwx(frame.mtcwx)
 {
     for(int i=0;i<FRAME_GRID_COLS;i++)
         for(int j=0; j<FRAME_GRID_ROWS; j++){
@@ -82,13 +87,18 @@ Frame::Frame(const Frame &frame)
 
     mmProjectPoints = frame.mmProjectPoints;
     mmMatchedInImage = frame.mmMatchedInImage;
+
+#ifdef REGISTER_TIMES
+    mTimeStereoMatch = frame.mTimeStereoMatch;
+    mTimeORB_Ext = frame.mTimeORB_Ext;
+#endif
 }
 
 
 Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera, Frame* pPrevF, const IMU::Calib &ImuCalib)
     :mpcpi(NULL), mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
      mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false),
-     mpCamera(pCamera) ,mpCamera2(nullptr), mTimeStereoMatch(0), mTimeORB_Ext(0)
+     mpCamera(pCamera) ,mpCamera2(nullptr)
 {
     // Frame ID
     mnId=nNextId++;
@@ -103,14 +113,14 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
     mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
 
     // ORB extraction
-#ifdef SAVE_TIMES
+#ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now();
 #endif
     thread threadLeft(&Frame::ExtractORB,this,0,imLeft,0,0);
     thread threadRight(&Frame::ExtractORB,this,1,imRight,0,0);
     threadLeft.join();
     threadRight.join();
-#ifdef SAVE_TIMES
+#ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now();
 
     mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
@@ -123,11 +133,11 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
 
     UndistortKeyPoints();
 
-#ifdef SAVE_TIMES
+#ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_StartStereoMatches = std::chrono::steady_clock::now();
 #endif
     ComputeStereoMatches();
-#ifdef SAVE_TIMES
+#ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_EndStereoMatches = std::chrono::steady_clock::now();
 
     mTimeStereoMatch = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndStereoMatches - time_StartStereoMatches).count();
@@ -136,7 +146,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
 
     mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
     mvbOutlier = vector<bool>(N,false);
-    mmProjectPoints.clear();// = map<long unsigned int, cv::Point2f>(N, static_cast<cv::Point2f>(NULL));
+    mmProjectPoints.clear();
     mmMatchedInImage.clear();
 
 
@@ -192,7 +202,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeSt
     :mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
      mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
      mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false),
-     mpCamera(pCamera),mpCamera2(nullptr), mTimeStereoMatch(0), mTimeORB_Ext(0)
+     mpCamera(pCamera),mpCamera2(nullptr)
 {
     // Frame ID
     mnId=nNextId++;
@@ -207,11 +217,11 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeSt
     mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
 
     // ORB extraction
-#ifdef SAVE_TIMES
+#ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now();
 #endif
     ExtractORB(0,imGray,0,0);
-#ifdef SAVE_TIMES
+#ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now();
 
     mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
@@ -275,7 +285,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra
     :mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
      mTimeStamp(timeStamp), mK(static_cast<Pinhole*>(pCamera)->toK()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
      mImuCalib(ImuCalib), mpImuPreintegrated(NULL),mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false), mpCamera(pCamera),
-     mpCamera2(nullptr), mTimeStereoMatch(0), mTimeORB_Ext(0)
+     mpCamera2(nullptr)
 {
     // Frame ID
     mnId=nNextId++;
@@ -290,11 +300,11 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra
     mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
 
     // ORB extraction
-#ifdef SAVE_TIMES
+#ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now();
 #endif
     ExtractORB(0,imGray,0,1000);
-#ifdef SAVE_TIMES
+#ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now();
 
     mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
@@ -314,7 +324,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra
 
     mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
 
-    mmProjectPoints.clear();// = map<long unsigned int, cv::Point2f>(N, static_cast<cv::Point2f>(NULL));
+    mmProjectPoints.clear();
     mmMatchedInImage.clear();
 
     mvbOutlier = vector<bool>(N,false);
@@ -353,7 +363,6 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extra
 
     AssignFeaturesToGrid();
 
-    // mVw = cv::Mat::zeros(3,1,CV_32F);
     if(pPrevF)
     {
         if(!pPrevF->mVw.empty())
@@ -453,6 +462,14 @@ void Frame::UpdatePoseMatrices()
     mRwc = mRcw.t();
     mtcw = mTcw.rowRange(0,3).col(3);
     mOw = -mRcw.t()*mtcw;
+
+    // Static matrix
+    mOwx =  cv::Matx31f(mOw.at<float>(0), mOw.at<float>(1), mOw.at<float>(2));
+    mRcwx = cv::Matx33f(mRcw.at<float>(0,0), mRcw.at<float>(0,1), mRcw.at<float>(0,2),
+                        mRcw.at<float>(1,0), mRcw.at<float>(1,1), mRcw.at<float>(1,2),
+                        mRcw.at<float>(2,0), mRcw.at<float>(2,1), mRcw.at<float>(2,2));
+    mtcwx = cv::Matx31f(mtcw.at<float>(0), mtcw.at<float>(1), mtcw.at<float>(2));
+
 }
 
 cv::Mat Frame::GetImuPosition()
@@ -477,56 +494,49 @@ cv::Mat Frame::GetImuPose()
 bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
 {
     if(Nleft == -1){
-        // cout << "\na";
         pMP->mbTrackInView = false;
         pMP->mTrackProjX = -1;
         pMP->mTrackProjY = -1;
 
         // 3D in absolute coordinates
-        cv::Mat P = pMP->GetWorldPos();
+        cv::Matx31f Px = pMP->GetWorldPos2();
 
-        // cout << "b";
 
         // 3D in camera coordinates
-        const cv::Mat Pc = mRcw*P+mtcw;
+        const cv::Matx31f Pc = mRcwx * Px + mtcwx;
         const float Pc_dist = cv::norm(Pc);
 
         // Check positive depth
-        const float &PcZ = Pc.at<float>(2);
+        const float &PcZ = Pc(2);
         const float invz = 1.0f/PcZ;
         if(PcZ<0.0f)
             return false;
 
         const cv::Point2f uv = mpCamera->project(Pc);
 
-        // cout << "c";
-
         if(uv.x<mnMinX || uv.x>mnMaxX)
             return false;
         if(uv.y<mnMinY || uv.y>mnMaxY)
             return false;
 
-        // cout << "d";
         pMP->mTrackProjX = uv.x;
         pMP->mTrackProjY = uv.y;
 
         // Check distance is in the scale invariance region of the MapPoint
         const float maxDistance = pMP->GetMaxDistanceInvariance();
         const float minDistance = pMP->GetMinDistanceInvariance();
-        const cv::Mat PO = P-mOw;
+        const cv::Matx31f PO = Px-mOwx;
         const float dist = cv::norm(PO);
 
         if(dist<minDistance || dist>maxDistance)
             return false;
 
-        // cout << "e";
 
         // Check viewing angle
-        cv::Mat Pn = pMP->GetNormal();
+        cv::Matx31f Pnx = pMP->GetNormal2();
 
-        // cout << "f";
 
-        const float viewCos = PO.dot(Pn)/dist;
+        const float viewCos = PO.dot(Pnx)/dist;
 
         if(viewCos<viewingCosLimit)
             return false;
@@ -534,21 +544,17 @@ bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
         // Predict scale in the image
         const int nPredictedLevel = pMP->PredictScale(dist,this);
 
-        // cout << "g";
-
         // Data used by the tracking
         pMP->mbTrackInView = true;
         pMP->mTrackProjX = uv.x;
         pMP->mTrackProjXR = uv.x - mbf*invz;
 
         pMP->mTrackDepth = Pc_dist;
-        // cout << "h";
 
         pMP->mTrackProjY = uv.y;
         pMP->mnTrackScaleLevel= nPredictedLevel;
         pMP->mTrackViewCos = viewCos;
 
-        // cout << "i";
 
         return true;
     }
@@ -589,8 +595,6 @@ bool Frame::ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float
     u=fx*PcX*invz+cx;
     v=fy*PcY*invz+cy;
 
-    // cout << "c";
-
     if(u<mnMinX || u>mnMaxX)
         return false;
     if(v<mnMinY || v>mnMaxY)
@@ -644,9 +648,6 @@ vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const f
     float factorX = r;
     float factorY = r;
 
-    /*cout << "fX " << factorX << endl;
-    cout << "fY " << factorY << endl;*/
-
     const int nMinCellX = max(0,(int)floor((x-mnMinX-factorX)*mfGridElementWidthInv));
     if(nMinCellX>=FRAME_GRID_COLS)
     {
@@ -891,10 +892,6 @@ void Frame::ComputeStereoMatches()
             // sliding window search
             const int w = 5;
             cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);
-            //IL.convertTo(IL,CV_32F);
-            //IL = IL - IL.at<float>(w,w) *cv::Mat::ones(IL.rows,IL.cols,CV_32F);
-            IL.convertTo(IL,CV_16S);
-            IL = IL - IL.at<short>(w,w);
 
             int bestDist = INT_MAX;
             int bestincR = 0;
@@ -910,10 +907,6 @@ void Frame::ComputeStereoMatches()
             for(int incR=-L; incR<=+L; incR++)
             {
                 cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1);
-                //IR.convertTo(IR,CV_32F);
-                //IR = IR - IR.at<float>(w,w) *cv::Mat::ones(IR.rows,IR.cols,CV_32F);
-                IR.convertTo(IR,CV_16S);
-                IR = IR - IR.at<short>(w,w);
 
                 float dist = cv::norm(IL,IR,cv::NORM_L1);
                 if(dist<bestDist)
@@ -1029,10 +1022,8 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
         :mpcpi(NULL), mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
          mImuCalib(ImuCalib), mpImuPreintegrated(NULL), mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false), mpCamera(pCamera), mpCamera2(pCamera2), mTlr(Tlr)
 {
-    std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
     imgLeft = imLeft.clone();
     imgRight = imRight.clone();
-    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
     // Frame ID
     mnId=nNextId++;
@@ -1047,11 +1038,18 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
     mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
 
     // ORB extraction
+#ifdef REGISTER_TIMES
+    std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now();
+#endif
     thread threadLeft(&Frame::ExtractORB,this,0,imLeft,static_cast<KannalaBrandt8*>(mpCamera)->mvLappingArea[0],static_cast<KannalaBrandt8*>(mpCamera)->mvLappingArea[1]);
     thread threadRight(&Frame::ExtractORB,this,1,imRight,static_cast<KannalaBrandt8*>(mpCamera2)->mvLappingArea[0],static_cast<KannalaBrandt8*>(mpCamera2)->mvLappingArea[1]);
     threadLeft.join();
     threadRight.join();
-    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
+#ifdef REGISTER_TIMES
+    std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now();
+
+    mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
+#endif
 
     Nleft = mvKeys.size();
     Nright = mvKeysRight.size();
@@ -1088,8 +1086,22 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
 
     cv::hconcat(Rrl,trl,mTrl);
 
+    mTrlx = cv::Matx34f(Rrl.at<float>(0,0), Rrl.at<float>(0,1), Rrl.at<float>(0,2), trl.at<float>(0),
+                        Rrl.at<float>(1,0), Rrl.at<float>(1,1), Rrl.at<float>(1,2), trl.at<float>(1),
+                        Rrl.at<float>(2,0), Rrl.at<float>(2,1), Rrl.at<float>(2,2), trl.at<float>(2));
+    mTlrx = cv::Matx34f(mRlr.at<float>(0,0), mRlr.at<float>(0,1), mRlr.at<float>(0,2), mtlr.at<float>(0),
+                        mRlr.at<float>(1,0), mRlr.at<float>(1,1), mRlr.at<float>(1,2), mtlr.at<float>(1),
+                        mRlr.at<float>(2,0), mRlr.at<float>(2,1), mRlr.at<float>(2,2), mtlr.at<float>(2));
+
+#ifdef REGISTER_TIMES
+    std::chrono::steady_clock::time_point time_StartStereoMatches = std::chrono::steady_clock::now();
+#endif
     ComputeStereoFishEyeMatches();
-    std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
+#ifdef REGISTER_TIMES
+    std::chrono::steady_clock::time_point time_EndStereoMatches = std::chrono::steady_clock::now();
+
+    mTimeStereoMatch = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndStereoMatches - time_StartStereoMatches).count();
+#endif
 
     //Put all descriptors in the same matrix
     cv::vconcat(mDescriptors,mDescriptorsRight,mDescriptors);
@@ -1098,25 +1110,10 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeSt
     mvbOutlier = vector<bool>(N,false);
 
     AssignFeaturesToGrid();
-    std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
 
     mpMutexImu = new std::mutex();
 
     UndistortKeyPoints();
-    std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now();
-
-    double t_read = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t1 - t0).count();
-    double t_orbextract = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
-    double t_stereomatches = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t3 - t2).count();
-    double t_assign = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t4 - t3).count();
-    double t_undistort = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t5 - t4).count();
-
-    /*cout << "Reading time: " << t_read << endl;
-    cout << "Extraction time: " << t_orbextract << endl;
-    cout << "Matching time: " << t_stereomatches << endl;
-    cout << "Assignment time: " << t_assign << endl;
-    cout << "Undistortion time: " << t_undistort << endl;*/
-
 }
 
 void Frame::ComputeStereoFishEyeMatches() {
@@ -1163,26 +1160,33 @@ void Frame::ComputeStereoFishEyeMatches() {
 
 bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit, bool bRight) {
     // 3D in absolute coordinates
-    cv::Mat P = pMP->GetWorldPos();
+    cv::Matx31f Px = pMP->GetWorldPos2();
+
+    cv::Matx33f mRx;
+    cv::Matx31f mtx, twcx;
+
+    cv::Matx33f Rcw = mRcwx;
+    cv::Matx33f Rwc = mRcwx.t();
+    cv::Matx31f tcw = mOwx;
 
-    cv::Mat mR, mt, twc;
     if(bRight){
-        cv::Mat Rrl = mTrl.colRange(0,3).rowRange(0,3);
-        cv::Mat trl = mTrl.col(3);
-        mR = Rrl * mRcw;
-        mt = Rrl * mtcw + trl;
-        twc = mRwc * mTlr.rowRange(0,3).col(3) + mOw;
+        cv::Matx33f Rrl = mTrlx.get_minor<3,3>(0,0);
+        cv::Matx31f trl = mTrlx.get_minor<3,1>(0,3);
+        mRx = Rrl * Rcw;
+        mtx = Rrl * tcw + trl;
+        twcx = Rwc * mTlrx.get_minor<3,1>(0,3) + tcw;
     }
     else{
-        mR = mRcw;
-        mt = mtcw;
-        twc = mOw;
+        mRx = mRcwx;
+        mtx = mtcwx;
+        twcx = mOwx;
     }
 
     // 3D in camera coordinates
-    cv::Mat Pc = mR*P+mt;
-    const float Pc_dist = cv::norm(Pc);
-    const float &PcZ = Pc.at<float>(2);
+
+    cv::Matx31f Pcx = mRx * Px + mtx;
+    const float Pc_dist = cv::norm(Pcx);
+    const float &PcZ = Pcx(2);
 
     // Check positive depth
     if(PcZ<0.0f)
@@ -1190,8 +1194,8 @@ bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit, bool bRight)
 
     // Project in image and check it is not outside
     cv::Point2f uv;
-    if(bRight) uv = mpCamera2->project(Pc);
-    else uv = mpCamera->project(Pc);
+    if(bRight) uv = mpCamera2->project(Pcx);
+    else uv = mpCamera->project(Pcx);
 
     if(uv.x<mnMinX || uv.x>mnMaxX)
         return false;
@@ -1201,16 +1205,16 @@ bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit, bool bRight)
     // Check distance is in the scale invariance region of the MapPoint
     const float maxDistance = pMP->GetMaxDistanceInvariance();
     const float minDistance = pMP->GetMinDistanceInvariance();
-    const cv::Mat PO = P-twc;
-    const float dist = cv::norm(PO);
+    const cv::Matx31f POx = Px - twcx;
+    const float dist = cv::norm(POx);
 
     if(dist<minDistance || dist>maxDistance)
         return false;
 
     // Check viewing angle
-    cv::Mat Pn = pMP->GetNormal();
+    cv::Matx31f Pnx = pMP->GetNormal2();
 
-    const float viewCos = PO.dot(Pn)/dist;
+    const float viewCos = POx.dot(Pnx)/dist;
 
     if(viewCos<viewingCosLimit)
         return false;

src/FrameDrawer.cc

@@ -36,7 +36,6 @@ FrameDrawer::FrameDrawer(Atlas* pAtlas):both(false),mpAtlas(pAtlas)
 
 cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
 {
-    // std::cout << "0" << std::endl;
     cv::Mat im;
     vector<cv::KeyPoint> vIniKeys; // Initialization: KeyPoints in reference frame
     vector<int> vMatches; // Initialization: correspondeces with reference keypoints
@@ -45,7 +44,6 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
     vector<pair<cv::Point2f, cv::Point2f> > vTracks;
     int state; // Tracking state
 
-    //
     Frame currentFrame;
     vector<MapPoint*> vpLocalMap;
     vector<cv::KeyPoint> vMatchesKeys;
@@ -71,7 +69,7 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
             vMatches = mvIniMatches;
             vTracks = mvTracks;
         }
-        else if(mState==Tracking::OK /*&& bOldFeatures*/)
+        else if(mState==Tracking::OK)
         {
             vCurrentKeys = mvCurrentKeys;
             vbVO = mvbVO;
@@ -94,7 +92,7 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
     }
 
     if(im.channels()<3) //this should be always true
-        cvtColor(im,im,CV_GRAY2BGR);
+        cvtColor(im,im,cv::COLOR_GRAY2BGR);
 
     //Draw
     if(state==Tracking::NOT_INITIALIZED)
@@ -141,12 +139,8 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
                     mnTrackedVO++;
                 }
             }
-            /*else
-            {
-                cv::circle(im,vCurrentKeys[i].pt,2,cv::Scalar(0,0,255),-1);
-            }*/
+
         }
-        // std::cout << "2.3" << std::endl;
     }
     else if(state==Tracking::OK && !bOldFeatures)
     {
@@ -155,9 +149,6 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
         mnTrackedVO=0;
         int n = vCurrentKeys.size();
 
-        // cout << "----------------------" << endl;
-        // cout << "Number of matches in old method: " << n << endl;
-
         for(int i=0; i < n; ++i)
         {
 
@@ -168,10 +159,6 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
             }
         }
 
-        n = mProjectPoints.size();
-        //cout << "Number of projected points: " << n << endl;
-        n = mMatchedInImage.size();
-        //cout << "Number of matched points: " << n << endl;
         map<long unsigned int, cv::Point2f>::iterator it_match = mMatchedInImage.begin();
         while(it_match != mMatchedInImage.end())
         {
@@ -191,37 +178,9 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
 
 
             it_match++;
-            //it_proj = mProjectPoints.erase(it_proj);
         }
-        //for(int i=0; i < n; ++i)
-        //{
-            /*if(!vpMatchedMPs[i])
-                continue;*/
-
-            //cv::circle(im,vProjectPoints[i],2,cv::Scalar(255,0,0),-1);
-            /*cv::Point2f point3d_proy;
-            float u, v;
-            bool bIsInImage = currentFrame.ProjectPointDistort(vpMatchedMPs[i] , point3d_proy, u, v);
-            if(bIsInImage)
-            {
-                //cout << "-Point is out of the image" << point3d_proy.x << ", " << point3d_proy.y << endl;
-                cv::circle(im,vMatchesKeys[i].pt,2,cv::Scalar(255,0,0),-1);
-                continue;
-            }
-
-            //cout << "+Point CV " << point3d_proy.x << ", " << point3d_proy.y << endl;
-            //cout << "+Point coord " << u << ", " << v << endl;
-            cv::Point2f point_im = vMatchesKeys[i].pt;
-
-            cv::line(im,cv::Point2f(u, v), point_im,cv::Scalar(0, 255, 0), 1);*/
-
-        //}
-
-        /*cout << "Number of tracker in old method: " << mnTracked << endl;
-        cout << "Number of tracker in new method: " << nTracked2 << endl;*/
 
         n = vOutlierKeys.size();
-        //cout << "Number of outliers: " << n << endl;
         for(int i=0; i < n; ++i)
         {
             cv::Point2f point3d_proy;
@@ -233,34 +192,7 @@ cv::Mat FrameDrawer::DrawFrame(bool bOldFeatures)
             cv::line(im,cv::Point2f(u, v), point_im,cv::Scalar(0, 0, 255), 1);
         }
 
-//        for(int i=0;i<n;i++)
-//        {
-//            if(vbVO[i] || vbMap[i])
-//            {
-//                cv::Point2f pt1,pt2;
-//                pt1.x=vCurrentKeys[i].pt.x-r;
-//                pt1.y=vCurrentKeys[i].pt.y-r;
-//                pt2.x=vCurrentKeys[i].pt.x+r;
-//                pt2.y=vCurrentKeys[i].pt.y+r;
-
-//                // This is a match to a MapPoint in the map
-//                if(vbMap[i])
-//                {
-//                    cv::rectangle(im,pt1,pt2,cv::Scalar(0,255,0));
-//                    cv::circle(im,vCurrentKeys[i].pt,2,cv::Scalar(0,255,0),-1);
-//                    mnTracked++;
-//                }
-//                else // This is match to a "visual odometry" MapPoint created in the last frame
-//                {
-//                    cv::rectangle(im,pt1,pt2,cv::Scalar(255,0,0));
-//                    cv::circle(im,vCurrentKeys[i].pt,2,cv::Scalar(255,0,0),-1);
-//                    mnTrackedVO++;
-//                }
-//            }
-//        }
-
     }
-    // std::cout << "3" << std::endl;
 
     cv::Mat imWithInfo;
     DrawTextInfo(im,state, imWithInfo);
@@ -305,7 +237,7 @@ cv::Mat FrameDrawer::DrawRightFrame()
     } // destroy scoped mutex -> release mutex
 
     if(im.channels()<3) //this should be always true
-        cvtColor(im,im,CV_GRAY2BGR);
+        cvtColor(im,im,cv::COLOR_GRAY2BGR);
 
     //Draw
     if(state==Tracking::NOT_INITIALIZED) //INITIALIZING
@@ -416,8 +348,6 @@ void FrameDrawer::Update(Tracking *pTracker)
         N = mvCurrentKeys.size();
     }
 
-    //cout << "Number of matches in frame: " << N << endl;
-    // cout << "Number of matches in frame: " << N << endl;
     mvbVO = vector<bool>(N,false);
     mvbMap = vector<bool>(N,false);
     mbOnlyTracking = pTracker->mbOnlyTracking;
@@ -425,7 +355,6 @@ void FrameDrawer::Update(Tracking *pTracker)
     //Variables for the new visualization
     mCurrentFrame = pTracker->mCurrentFrame;
     mmProjectPoints = mCurrentFrame.mmProjectPoints;
-    //mmMatchedInImage = mCurrentFrame.mmMatchedInImage;
     mmMatchedInImage.clear();
 
     mvpLocalMap = pTracker->GetLocalMapMPS();
@@ -437,8 +366,6 @@ void FrameDrawer::Update(Tracking *pTracker)
     mvOutlierKeys.reserve(N);
     mvpOutlierMPs.clear();
     mvpOutlierMPs.reserve(N);
-    //mvProjectPoints.clear();
-    //mvProjectPoints.reserve(N);
 
     if(pTracker->mLastProcessedState==Tracking::NOT_INITIALIZED)
     {
@@ -459,18 +386,8 @@ void FrameDrawer::Update(Tracking *pTracker)
                     else
                         mvbVO[i]=true;
 
-                    //mvpMatchedMPs.push_back(pMP);
-                    //mvMatchedKeys.push_back(mvCurrentKeys[i]);
                     mmMatchedInImage[pMP->mnId] = mvCurrentKeys[i].pt;
 
-                    //cv::Point2f point3d_proy;
-                    //float u, v;
-                    //bool bIsInImage = mCurrentFrame.ProjectPointDistort(pMP, point3d_proy, u, v);
-                    //if(bIsInImage)
-                    //{
-                        //mvMatchedKeys.push_back(mvCurrentKeys[i]);
-                        //mvProjectPoints.push_back(cv::Point2f(u, v));
-                    //}
                 }
                 else
                 {

src/G2oTypes.cc

@@ -424,61 +424,6 @@ void EdgeStereo::linearizeOplus()
             y ,  -x , 0.0, 0.0, 0.0, 1.0;
 
     _jacobianOplusXj = proj_jac * Rcb * SE3deriv;
-
-
-    /*const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[1]);
-    const g2o::VertexSBAPointXYZ* VPoint = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
-
-    const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw;
-    const Eigen::Vector3d &tcw = VPose->estimate().tcw;
-    const Eigen::Vector3d Xc = Rcw*VPoint->estimate() + tcw;
-    const double &xc = Xc[0];
-    const double &yc = Xc[1];
-    const double invzc = 1.0/Xc[2];
-    const double invzc2 = invzc*invzc;
-    const double &fx = VPose->estimate().fx;
-    const double &fy = VPose->estimate().fy;
-    const double &bf = VPose->estimate().bf;
-    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb;
-
-    // Jacobian wrt Point
-    _jacobianOplusXi(0,0) = -fx*invzc*Rcw(0,0)+fx*xc*invzc2*Rcw(2,0);
-    _jacobianOplusXi(0,1) = -fx*invzc*Rcw(0,1)+fx*xc*invzc2*Rcw(2,1);
-    _jacobianOplusXi(0,2) = -fx*invzc*Rcw(0,2)+fx*xc*invzc2*Rcw(2,2);
-
-    _jacobianOplusXi(1,0) = -fy*invzc*Rcw(1,0)+fy*yc*invzc2*Rcw(2,0);
-    _jacobianOplusXi(1,1) = -fy*invzc*Rcw(1,1)+fy*yc*invzc2*Rcw(2,1);
-    _jacobianOplusXi(1,2) = -fy*invzc*Rcw(1,2)+fy*yc*invzc2*Rcw(2,2);
-
-    _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*invzc2*Rcw(2,0);
-    _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)-bf*invzc2*Rcw(2,1);
-    _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2)-bf*invzc2*Rcw(2,2);
-
-    const Eigen::Vector3d Xb = VPose->estimate().Rbc*Xc + VPose->estimate().tbc;
-    const Eigen::Matrix3d RS = VPose->estimate().Rcb*Skew(Xb);
-
-    // Jacobian wrt Imu Pose
-    _jacobianOplusXj(0,0) = -fx*invzc*RS(0,0)+fx*xc*invzc2*RS(2,0);
-    _jacobianOplusXj(0,1) = -fx*invzc*RS(0,1)+fx*xc*invzc2*RS(2,1);
-    _jacobianOplusXj(0,2) = -fx*invzc*RS(0,2)+fx*xc*invzc2*RS(2,2);
-    _jacobianOplusXj(0,3) = fx*invzc*Rcb(0,0)-fx*xc*invzc2*Rcb(2,0);
-    _jacobianOplusXj(0,4) = fx*invzc*Rcb(0,1)-fx*xc*invzc2*Rcb(2,1);
-    _jacobianOplusXj(0,5) = fx*invzc*Rcb(0,2)-fx*xc*invzc2*Rcb(2,2);
-
-    _jacobianOplusXj(1,0) = -fy*invzc*RS(1,0)+fy*yc*invzc2*RS(2,0);
-    _jacobianOplusXj(1,1) = -fy*invzc*RS(1,1)+fy*yc*invzc2*RS(2,1);
-    _jacobianOplusXj(1,2) = -fy*invzc*RS(1,2)+fy*yc*invzc2*RS(2,2);
-    _jacobianOplusXj(1,3) = fy*invzc*Rcb(1,0)-fy*yc*invzc2*Rcb(2,0);
-    _jacobianOplusXj(1,4) = fy*invzc*Rcb(1,1)-fy*yc*invzc2*Rcb(2,1);
-    _jacobianOplusXj(1,5) = fy*invzc*Rcb(1,2)-fy*yc*invzc2*Rcb(2,2);
-
-    _jacobianOplusXj(2,0) = _jacobianOplusXj(0,0) - bf*invzc2*RS(2,0);
-    _jacobianOplusXj(2,1) = _jacobianOplusXj(0,1) - bf*invzc2*RS(2,1);
-    _jacobianOplusXj(2,2) = _jacobianOplusXj(0,2) - bf*invzc2*RS(2,2);
-    _jacobianOplusXj(2,3) = _jacobianOplusXj(0,3) + bf*invzc2*Rcb(2,0);
-    _jacobianOplusXj(2,4) = _jacobianOplusXj(0,4) + bf*invzc2*Rcb(2,1);
-    _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5) + bf*invzc2*Rcb(2,2);
-    */
 }
 
 void EdgeStereoOnlyPose::linearizeOplus()
@@ -506,156 +451,8 @@ void EdgeStereoOnlyPose::linearizeOplus()
             -z , 0.0, x, 0.0, 1.0, 0.0,
             y ,  -x , 0.0, 0.0, 0.0, 1.0;
     _jacobianOplusXi = proj_jac * Rcb * SE3deriv;
-
-    /*const VertexPose* VPose = static_cast<const VertexPose*>(_vertices[0]);
-    const Eigen::Matrix3d &Rcw = VPose->estimate().Rcw;
-    const Eigen::Vector3d &tcw = VPose->estimate().tcw;
-    const Eigen::Vector3d Xc = Rcw*Xw + tcw;
-    const double &xc = Xc[0];
-    const double &yc = Xc[1];
-    const double invzc = 1.0/Xc[2];
-    const double invzc2 = invzc*invzc;
-    const double &fx = VPose->estimate().fx;
-    const double &fy = VPose->estimate().fy;
-    const double &bf = VPose->estimate().bf;
-    const Eigen::Matrix3d &Rcb = VPose->estimate().Rcb;
-
-    const Eigen::Vector3d Xb = VPose->estimate().Rbc*Xc + VPose->estimate().tbc;
-    const Eigen::Matrix3d RS = VPose->estimate().Rcb*Skew(Xb);
-
-    // Jacobian wrt Imu Pose
-    _jacobianOplusXi(0,0) = -fx*invzc*RS(0,0)+fx*xc*invzc2*RS(2,0);
-    _jacobianOplusXi(0,1) = -fx*invzc*RS(0,1)+fx*xc*invzc2*RS(2,1);
-    _jacobianOplusXi(0,2) = -fx*invzc*RS(0,2)+fx*xc*invzc2*RS(2,2);
-    _jacobianOplusXi(0,3) = fx*invzc*Rcb(0,0)-fx*xc*invzc2*Rcb(2,0);
-    _jacobianOplusXi(0,4) = fx*invzc*Rcb(0,1)-fx*xc*invzc2*Rcb(2,1);
-    _jacobianOplusXi(0,5) = fx*invzc*Rcb(0,2)-fx*xc*invzc2*Rcb(2,2);
-
-    _jacobianOplusXi(1,0) = -fy*invzc*RS(1,0)+fy*yc*invzc2*RS(2,0);
-    _jacobianOplusXi(1,1) = -fy*invzc*RS(1,1)+fy*yc*invzc2*RS(2,1);
-    _jacobianOplusXi(1,2) = -fy*invzc*RS(1,2)+fy*yc*invzc2*RS(2,2);
-    _jacobianOplusXi(1,3) = fy*invzc*Rcb(1,0)-fy*yc*invzc2*Rcb(2,0);
-    _jacobianOplusXi(1,4) = fy*invzc*Rcb(1,1)-fy*yc*invzc2*Rcb(2,1);
-    _jacobianOplusXi(1,5) = fy*invzc*Rcb(1,2)-fy*yc*invzc2*Rcb(2,2);
-
-    _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0) - bf*invzc2*RS(2,0);
-    _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1) - bf*invzc2*RS(2,1);
-    _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2) - bf*invzc2*RS(2,2);
-    _jacobianOplusXi(2,3) = _jacobianOplusXi(0,3) + bf*invzc2*Rcb(2,0);
-    _jacobianOplusXi(2,4) = _jacobianOplusXi(0,4) + bf*invzc2*Rcb(2,1);
-    _jacobianOplusXi(2,5) = _jacobianOplusXi(0,5) + bf*invzc2*Rcb(2,2);
-    */
 }
 
-/*Eigen::Vector2d EdgeMonoInvdepth::cam_project(const Eigen::Vector3d & trans_xyz) const{
-  double invZ = 1./trans_xyz[2];
-  Eigen::Vector2d res;
-  res[0] = invZ*trans_xyz[0]*fx + cx;
-  res[1] = invZ*trans_xyz[1]*fy + cy;
-  return res;
-}
-
-Eigen::Vector3d EdgeMonoInvdepth::cam_unproject(const double u, const double v, const double invDepth) const{
-  Eigen::Vector3d res;
-  res[2] = 1./invDepth;
-  double z_x=res[2]/fx;
-  double z_y=res[2]/fy;
-  res[0] = (u-cx)*z_x;
-  res[1] = (v-cy)*z_y;
-
-  return res;
-}
-
-void EdgeMonoInvdepth::linearizeOplus()
-{
-    VertexInvDepth *vPt = static_cast<VertexInvDepth*>(_vertices[0]);
-    g2o::VertexSE3Expmap *vHost = static_cast<g2o::VertexSE3Expmap*>(_vertices[1]);
-    g2o::VertexSE3Expmap *vObs = static_cast<g2o::VertexSE3Expmap*>(_vertices[2]);
-
-    //
-    g2o::SE3Quat Tiw(vObs->estimate());
-    g2o::SE3Quat T0w(vHost->estimate());
-    g2o::SE3Quat Ti0 = Tiw*T0w.inverse();
-    double o_rho_j = vPt->estimate().rho;
-    Eigen::Vector3d o_X_j = cam_unproject(vPt->estimate().u,vPt->estimate().v,o_rho_j);
-    Eigen::Vector3d i_X_j = Ti0.map(o_X_j);
-    double i_rho_j = 1./i_X_j[2];
-    Eigen::Vector2d i_proj_j = cam_project(i_X_j);
-
-    // i_rho_j*C_ij matrix
-    Eigen::Matrix<double,2,3> rhoC_ij;
-    rhoC_ij(0,0) = i_rho_j*fx;
-    rhoC_ij(0,1) = 0.0;
-    rhoC_ij(0,2) = i_rho_j*(cx-i_proj_j[0]);
-    rhoC_ij(1,0) = 0.0;
-    rhoC_ij(1,1) = i_rho_j*fy;
-    rhoC_ij(1,2) = i_rho_j*(cy-i_proj_j[1]);
-
-    // o_rho_j^{-2}*K^{-1}*0_proj_j vector
-    Eigen::Matrix3d Ri0 = Ti0.rotation().toRotationMatrix();
-    Eigen::Matrix<double, 2, 3> tmp;
-    tmp = rhoC_ij*Ri0;
-
-    // Skew matrices
-    Eigen::Matrix3d skew_0_X_j;
-    Eigen::Matrix3d skew_i_X_j;
-
-    skew_0_X_j=Eigen::MatrixXd::Zero(3,3);
-    skew_i_X_j=Eigen::MatrixXd::Zero(3,3);
-
-    skew_0_X_j(0,1) = -o_X_j[2];
-    skew_0_X_j(1,0) = -skew_0_X_j(0,1);
-    skew_0_X_j(0,2) = o_X_j[1];
-    skew_0_X_j(2,0) = -skew_0_X_j(0,2);
-    skew_0_X_j(1,2) = -o_X_j[0];
-    skew_0_X_j(2,1) = -skew_0_X_j(1,2);
-
-    skew_i_X_j(0,1) = -i_X_j[2];
-    skew_i_X_j(1,0) = -skew_i_X_j(0,1);
-    skew_i_X_j(0,2) = i_X_j[1];
-    skew_i_X_j(2,0) = -skew_i_X_j(0,2);
-    skew_i_X_j(1,2) = -i_X_j[0];
-    skew_i_X_j(2,1) = -skew_i_X_j(1,2);
-
-    // Jacobians w.r.t inverse depth in the host frame
-    _jacobianOplus[0].setZero();
-    _jacobianOplus[0] = (1./o_rho_j)*rhoC_ij*Ri0*o_X_j;
-
-    // Jacobians w.r.t pose host frame
-    _jacobianOplus[1].setZero();
-    // Rotation
-    _jacobianOplus[1].block<2,3>(0,0) = -tmp*skew_0_X_j;
-    // translation
-    _jacobianOplus[1].block<2,3>(0,3) = tmp;
-
-    // Jacobians w.r.t pose observer frame
-    _jacobianOplus[2].setZero();
-    // Rotation
-    _jacobianOplus[2].block<2,3>(0,0) = rhoC_ij*skew_i_X_j;
-    // translation
-    _jacobianOplus[2].block<2,3>(0,3) = -rhoC_ij;
-}
-
-
-Eigen::Vector2d EdgeMonoInvdepthBody::cam_project(const Eigen::Vector3d & trans_xyz) const{
-  double invZ = 1./trans_xyz[2];
-  Eigen::Vector2d res;
-  res[0] = invZ*trans_xyz[0]*fx + cx;
-  res[1] = invZ*trans_xyz[1]*fy + cy;
-  return res;
-}
-
-Eigen::Vector3d EdgeMonoInvdepthBody::cam_unproject(const double u, const double v, const double invDepth) const{
-  Eigen::Vector3d res;
-  res[2] = 1./invDepth;
-  double z_x=res[2]/fx;
-  double z_y=res[2]/fy;
-  res[0] = (u-cx)*z_x;
-  res[1] = (v-cy)*z_y;
-
-  return res;
-}*/
-
 VertexVelocity::VertexVelocity(KeyFrame* pKF)
 {
     setEstimate(Converter::toVector3d(pKF->GetVelocity()));

src/ImuTypes.cc

@@ -31,7 +31,6 @@ cv::Mat NormalizeRotation(const cv::Mat &R)
 {
     cv::Mat U,w,Vt;
     cv::SVDecomp(R,w,U,Vt,cv::SVD::FULL_UV);
-    // assert(cv::determinant(U*Vt)>0);
     return U*Vt;
 }
 

src/Initializer.cc

@@ -103,8 +103,6 @@ bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatc
     thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
     thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
 
-    //cout << "5" << endl;
-
     // Wait until both threads have finished
     threadH.join();
     threadF.join();
@@ -112,20 +110,16 @@ bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatc
     // Compute ratio of scores
     float RH = SH/(SH+SF);
 
-    //cout << "6" << endl;
-
     float minParallax = 1.0; // 1.0 originally
 
     cv::Mat K = static_cast<Pinhole*>(mpCamera)->toK();
     // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
-    if(RH>0.40) // if(RH>0.40)
+    if(RH>0.40)
     {
-        //cout << "Initialization from Homography" << endl;
         return ReconstructH(vbMatchesInliersH,H, K,R21,t21,vP3D,vbTriangulated,minParallax,50);
     }
-    else //if(pF_HF>0.6)
+    else
     {
-        //cout << "Initialization from Fundamental" << endl;
         return ReconstructF(vbMatchesInliersF,F,K,R21,t21,vP3D,vbTriangulated,minParallax,50);
     }
 

src/KeyFrame.cc

@@ -95,8 +95,13 @@ KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
     SetPose(F.mTcw);
 
     mnOriginMapId = pMap->GetId();
-}
 
+    this->Tlr_ = cv::Matx44f(mTlr.at<float>(0,0),mTlr.at<float>(0,1),mTlr.at<float>(0,2),mTlr.at<float>(0,3),
+                             mTlr.at<float>(1,0),mTlr.at<float>(1,1),mTlr.at<float>(1,2),mTlr.at<float>(1,3),
+                             mTlr.at<float>(2,0),mTlr.at<float>(2,1),mTlr.at<float>(2,2),mTlr.at<float>(2,3),
+                             mTlr.at<float>(3,0),mTlr.at<float>(3,1),mTlr.at<float>(3,2),mTlr.at<float>(3,3));
+
+}
 void KeyFrame::ComputeBoW()
 {
     if(mBowVec.empty() || mFeatVec.empty())
@@ -125,6 +130,19 @@ void KeyFrame::SetPose(const cv::Mat &Tcw_)
     Ow.copyTo(Twc.rowRange(0,3).col(3));
     cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1);
     Cw = Twc*center;
+
+    //Static matrices
+    this->Tcw_ = cv::Matx44f(Tcw.at<float>(0,0),Tcw.at<float>(0,1),Tcw.at<float>(0,2),Tcw.at<float>(0,3),
+                       Tcw.at<float>(1,0),Tcw.at<float>(1,1),Tcw.at<float>(1,2),Tcw.at<float>(1,3),
+                       Tcw.at<float>(2,0),Tcw.at<float>(2,1),Tcw.at<float>(2,2),Tcw.at<float>(2,3),
+                       Tcw.at<float>(3,0),Tcw.at<float>(3,1),Tcw.at<float>(3,2),Tcw.at<float>(3,3));
+
+    this->Twc_ = cv::Matx44f(Twc.at<float>(0,0),Twc.at<float>(0,1),Twc.at<float>(0,2),Twc.at<float>(0,3),
+                             Twc.at<float>(1,0),Twc.at<float>(1,1),Twc.at<float>(1,2),Twc.at<float>(1,3),
+                             Twc.at<float>(2,0),Twc.at<float>(2,1),Twc.at<float>(2,2),Twc.at<float>(2,3),
+                                     Twc.at<float>(3,0),Twc.at<float>(3,1),Twc.at<float>(3,2),Twc.at<float>(3,3));
+
+    this->Ow_ = cv::Matx31f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
 }
 
 void KeyFrame::SetVelocity(const cv::Mat &Vw_)
@@ -276,7 +294,7 @@ vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w)
     else
     {
         int n = it-mvOrderedWeights.begin();
-        //cout << "n = " << n << endl;
+
         return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n);
     }
 }
@@ -467,41 +485,12 @@ void KeyFrame::UpdateConnections(bool upParent)
     {
         unique_lock<mutex> lockCon(mMutexConnections);
 
-        // mspConnectedKeyFrames = spConnectedKeyFrames;
         mConnectedKeyFrameWeights = KFcounter;
         mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
         mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
 
-//        if(mbFirstConnection && mnId!=mpMap->GetInitKFid())
-//        {
-//            mpParent = mvpOrderedConnectedKeyFrames.front();
-//            mpParent->AddChild(this);
-//            mbFirstConnection = false;
-//        }
-
         if(mbFirstConnection && mnId!=mpMap->GetInitKFid())
         {
-            /*if(!mpParent || mpParent->GetParent() != this)
-            {
-                KeyFrame* pBestParent = static_cast<KeyFrame*>(NULL);
-                for(KeyFrame* pKFi : mvpOrderedConnectedKeyFrames)
-                {
-                    if(pKFi->GetParent() || pKFi->mnId == mpMap->GetInitKFid())
-                    {
-                        pBestParent = pKFi;
-                        break;
-                    }
-                }
-                if(!pBestParent)
-                {
-                    cout << "It can't be a covisible KF without Parent" << endl << endl;
-                    return;
-                }
-                mpParent = pBestParent;
-                mpParent->AddChild(this);
-                mbFirstConnection = false;
-            }*/
-            // cout << "udt.conn.id: " << mnId << endl;
             mpParent = mvpOrderedConnectedKeyFrames.front();
             mpParent->AddChild(this);
             mbFirstConnection = false;
@@ -525,8 +514,7 @@ void KeyFrame::EraseChild(KeyFrame *pKF)
 void KeyFrame::ChangeParent(KeyFrame *pKF)
 {
     unique_lock<mutex> lockCon(mMutexConnections);
-//    if(!mpParent && mpParent != this)
-//        mpParent->EraseChild(this);
+
     if(pKF == this)
     {
         cout << "ERROR: Change parent KF, the parent and child are the same KF" << endl;
@@ -610,45 +598,32 @@ void KeyFrame::SetErase()
 }
 
 void KeyFrame::SetBadFlag()
-{   
-    // std::cout << "Erasing KF..." << std::endl;
+{
     {
         unique_lock<mutex> lock(mMutexConnections);
         if(mnId==mpMap->GetInitKFid())
         {
-            //std::cout << "KF.BADFLAG-> KF 0!!" << std::endl;
             return;
         }
         else if(mbNotErase)
         {
-            //std::cout << "KF.BADFLAG-> mbNotErase!!" << std::endl;
             mbToBeErased = true;
             return;
         }
-        if(!mpParent)
-        {
-            //cout << "KF.BADFLAG-> There is not parent, but it is not the first KF in the map" << endl;
-            //cout << "KF.BADFLAG-> KF: " << mnId << "; first KF: " << mpMap->GetInitKFid() << endl;
-        }
     }
-    //std::cout << "KF.BADFLAG-> Erasing KF..." << std::endl;
 
     for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
     {
         mit->first->EraseConnection(this);
     }
-    //std::cout << "KF.BADFLAG-> Connection erased..." << std::endl;
 
     for(size_t i=0; i<mvpMapPoints.size(); i++)
     {
         if(mvpMapPoints[i])
         {
             mvpMapPoints[i]->EraseObservation(this);
-            // nDeletedPoints++;
         }
     }
-    // cout << "nDeletedPoints: " << nDeletedPoints << endl;
-    //std::cout << "KF.BADFLAG-> Observations deleted..." << std::endl;
 
     {
         unique_lock<mutex> lock(mMutexConnections);
@@ -661,7 +636,6 @@ void KeyFrame::SetBadFlag()
         set<KeyFrame*> sParentCandidates;
         if(mpParent)
             sParentCandidates.insert(mpParent);
-        //std::cout << "KF.BADFLAG-> Initially there are " << sParentCandidates.size() << " candidates" << std::endl;
 
         // Assign at each iteration one children with a parent (the pair with highest covisibility weight)
         // Include that children as new parent candidate for the rest
@@ -699,16 +673,9 @@ void KeyFrame::SetBadFlag()
                     }
                 }
             }
-            //std::cout << "KF.BADFLAG-> Find most similar children" << std::endl;
 
             if(bContinue)
             {
-                if(pC->mnId == pP->mnId)
-                {
-                    /*cout << "ERROR: The parent and son can't be the same KF. ID: " << pC->mnId << endl;
-                    cout << "Current KF: " << mnId << endl;
-                    cout << "Parent of the map: " << endl;*/
-                }
                 pC->ChangeParent(pP);
                 sParentCandidates.insert(pC);
                 mspChildrens.erase(pC);
@@ -716,7 +683,6 @@ void KeyFrame::SetBadFlag()
             else
                 break;
         }
-        //std::cout << "KF.BADFLAG-> Apply change of parent to children" << std::endl;
 
         // If a children has no covisibility links with any parent candidate, assign to the original parent of this KF
         if(!mspChildrens.empty())
@@ -726,16 +692,11 @@ void KeyFrame::SetBadFlag()
                 (*sit)->ChangeParent(mpParent);
             }
         }
-        //std::cout << "KF.BADFLAG-> Apply change to its parent" << std::endl;
 
         if(mpParent){
             mpParent->EraseChild(this);
             mTcp = Tcw*mpParent->GetPoseInverse();
         }
-        else
-        {
-            //cout << "Error: KF haven't got a parent, it is imposible reach this code point without him" << endl;
-        }
         mbBad = true;
     }
 
@@ -906,171 +867,6 @@ void KeyFrame::UpdateMap(Map* pMap)
     mpMap = pMap;
 }
 
-void KeyFrame::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP, set<GeometricCamera*>& spCam)
-{
-    // Save the id of each MapPoint in this KF, there can be null pointer in the vector
-    mvBackupMapPointsId.clear();
-    mvBackupMapPointsId.reserve(N);
-    for(int i = 0; i < N; ++i)
-    {
-
-        if(mvpMapPoints[i] && spMP.find(mvpMapPoints[i]) != spMP.end()) // Checks if the element is not null
-            mvBackupMapPointsId.push_back(mvpMapPoints[i]->mnId);
-        else // If the element is null his value is -1 because all the id are positives
-            mvBackupMapPointsId.push_back(-1);
-    }
-    //cout << "KeyFrame: ID from MapPoints stored" << endl;
-    // Save the id of each connected KF with it weight
-    mBackupConnectedKeyFrameIdWeights.clear();
-    for(std::map<KeyFrame*,int>::const_iterator it = mConnectedKeyFrameWeights.begin(), end = mConnectedKeyFrameWeights.end(); it != end; ++it)
-    {
-        if(spKF.find(it->first) != spKF.end())
-            mBackupConnectedKeyFrameIdWeights[it->first->mnId] = it->second;
-    }
-    //cout << "KeyFrame: ID from connected KFs stored" << endl;
-    // Save the parent id
-    mBackupParentId = -1;
-    if(mpParent && spKF.find(mpParent) != spKF.end())
-        mBackupParentId = mpParent->mnId;
-    //cout << "KeyFrame: ID from Parent KF stored" << endl;
-    // Save the id of the childrens KF
-    mvBackupChildrensId.clear();
-    mvBackupChildrensId.reserve(mspChildrens.size());
-    for(KeyFrame* pKFi : mspChildrens)
-    {
-        if(spKF.find(pKFi) != spKF.end())
-            mvBackupChildrensId.push_back(pKFi->mnId);
-    }
-    //cout << "KeyFrame: ID from Children KFs stored" << endl;
-    // Save the id of the loop edge KF
-    mvBackupLoopEdgesId.clear();
-    mvBackupLoopEdgesId.reserve(mspLoopEdges.size());
-    for(KeyFrame* pKFi : mspLoopEdges)
-    {
-        if(spKF.find(pKFi) != spKF.end())
-            mvBackupLoopEdgesId.push_back(pKFi->mnId);
-    }
-    //cout << "KeyFrame: ID from Loop KFs stored" << endl;
-    // Save the id of the merge edge KF
-    mvBackupMergeEdgesId.clear();
-    mvBackupMergeEdgesId.reserve(mspMergeEdges.size());
-    for(KeyFrame* pKFi : mspMergeEdges)
-    {
-        if(spKF.find(pKFi) != spKF.end())
-            mvBackupMergeEdgesId.push_back(pKFi->mnId);
-    }
-    //cout << "KeyFrame: ID from Merge KFs stored" << endl;
-
-    //Camera data
-    mnBackupIdCamera = -1;
-    if(mpCamera && spCam.find(mpCamera) != spCam.end())
-        mnBackupIdCamera = mpCamera->GetId();
-    //cout << "KeyFrame: ID from Camera1 stored; " << mnBackupIdCamera << endl;
-
-    mnBackupIdCamera2 = -1;
-    if(mpCamera2 && spCam.find(mpCamera2) != spCam.end())
-        mnBackupIdCamera2 = mpCamera2->GetId();
-    //cout << "KeyFrame: ID from Camera2 stored; " << mnBackupIdCamera2 << endl;
-
-    //Inertial data
-    mBackupPrevKFId = -1;
-    if(mPrevKF && spKF.find(mPrevKF) != spKF.end())
-        mBackupPrevKFId = mPrevKF->mnId;
-    //cout << "KeyFrame: ID from Prev KF stored" << endl;
-    mBackupNextKFId = -1;
-    if(mNextKF && spKF.find(mNextKF) != spKF.end())
-        mBackupNextKFId = mNextKF->mnId;
-    //cout << "KeyFrame: ID from NextKF stored" << endl;
-    if(mpImuPreintegrated)
-        mBackupImuPreintegrated.CopyFrom(mpImuPreintegrated);
-    //cout << "KeyFrame: Imu Preintegrated stored" << endl;
-}
-
-void KeyFrame::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid, map<unsigned int, GeometricCamera*>& mpCamId){
-    // Rebuild the empty variables
-
-    // Pose
-    SetPose(Tcw);
-
-    // Reference reconstruction
-    // Each MapPoint sight from this KeyFrame
-    mvpMapPoints.clear();
-    mvpMapPoints.resize(N);
-    for(int i=0; i<N; ++i)
-    {
-        if(mvBackupMapPointsId[i] != -1)
-            mvpMapPoints[i] = mpMPid[mvBackupMapPointsId[i]];
-        else
-            mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
-    }
-
-    // Conected KeyFrames with him weight
-    mConnectedKeyFrameWeights.clear();
-    for(map<long unsigned int, int>::const_iterator it = mBackupConnectedKeyFrameIdWeights.begin(), end = mBackupConnectedKeyFrameIdWeights.end();
-        it != end; ++it)
-    {
-        KeyFrame* pKFi = mpKFid[it->first];
-        mConnectedKeyFrameWeights[pKFi] = it->second;
-    }
-
-    // Restore parent KeyFrame
-    if(mBackupParentId>=0)
-        mpParent = mpKFid[mBackupParentId];
-
-    // KeyFrame childrens
-    mspChildrens.clear();
-    for(vector<long unsigned int>::const_iterator it = mvBackupChildrensId.begin(), end = mvBackupChildrensId.end(); it!=end; ++it)
-    {
-        mspChildrens.insert(mpKFid[*it]);
-    }
-
-    // Loop edge KeyFrame
-    mspLoopEdges.clear();
-    for(vector<long unsigned int>::const_iterator it = mvBackupLoopEdgesId.begin(), end = mvBackupLoopEdgesId.end(); it != end; ++it)
-    {
-        mspLoopEdges.insert(mpKFid[*it]);
-    }
-
-    // Merge edge KeyFrame
-    mspMergeEdges.clear();
-    for(vector<long unsigned int>::const_iterator it = mvBackupMergeEdgesId.begin(), end = mvBackupMergeEdgesId.end(); it != end; ++it)
-    {
-        mspMergeEdges.insert(mpKFid[*it]);
-    }
-
-    //Camera data
-    if(mnBackupIdCamera >= 0)
-    {
-        mpCamera = mpCamId[mnBackupIdCamera];
-    }
-    if(mnBackupIdCamera2 >= 0)
-    {
-        mpCamera2 = mpCamId[mnBackupIdCamera2];
-    }
-
-    //Inertial data
-    if(mBackupPrevKFId != -1)
-    {
-        mPrevKF = mpKFid[mBackupPrevKFId];
-    }
-    if(mBackupNextKFId != -1)
-    {
-        mNextKF = mpKFid[mBackupNextKFId];
-    }
-    mpImuPreintegrated = &mBackupImuPreintegrated;
-
-
-    // Remove all backup container
-    mvBackupMapPointsId.clear();
-    mBackupConnectedKeyFrameIdWeights.clear();
-    mvBackupChildrensId.clear();
-    mvBackupLoopEdgesId.clear();
-
-    UpdateBestCovisibles();
-
-    //ComputeSceneMedianDepth();
-}
-
 bool KeyFrame::ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v)
 {
 
@@ -1271,4 +1067,94 @@ void KeyFrame::SetKeyFrameDatabase(KeyFrameDatabase* pKFDB)
     mpKeyFrameDB = pKFDB;
 }
 
+cv::Matx33f KeyFrame::GetRotation_() {
+    unique_lock<mutex> lock(mMutexPose);
+    return Tcw_.get_minor<3,3>(0,0);
+}
+
+cv::Matx31f KeyFrame::GetTranslation_() {
+    unique_lock<mutex> lock(mMutexPose);
+    return Tcw_.get_minor<3,1>(0,3);
+}
+
+cv::Matx31f KeyFrame::GetCameraCenter_() {
+    unique_lock<mutex> lock(mMutexPose);
+    return Ow_;
+}
+
+cv::Matx33f KeyFrame::GetRightRotation_() {
+    unique_lock<mutex> lock(mMutexPose);
+    cv::Matx33f Rrl = Tlr_.get_minor<3,3>(0,0).t();
+    cv::Matx33f Rlw = Tcw_.get_minor<3,3>(0,0);
+    cv::Matx33f Rrw = Rrl * Rlw;
+
+    return Rrw;
+}
+
+cv::Matx31f KeyFrame::GetRightTranslation_() {
+    unique_lock<mutex> lock(mMutexPose);
+    cv::Matx33f Rrl = Tlr_.get_minor<3,3>(0,0).t();
+    cv::Matx31f tlw = Tcw_.get_minor<3,1>(0,3);
+    cv::Matx31f trl = - Rrl * Tlr_.get_minor<3,1>(0,3);
+
+    cv::Matx31f trw = Rrl * tlw + trl;
+
+    return trw;
+}
+
+cv::Matx44f KeyFrame::GetRightPose_() {
+    unique_lock<mutex> lock(mMutexPose);
+
+    cv::Matx33f Rrl = Tlr_.get_minor<3,3>(0,0).t();
+    cv::Matx33f Rlw = Tcw_.get_minor<3,3>(0,0);
+    cv::Matx33f Rrw = Rrl * Rlw;
+
+    cv::Matx31f tlw = Tcw_.get_minor<3,1>(0,3);
+    cv::Matx31f trl = - Rrl * Tlr_.get_minor<3,1>(0,3);
+
+    cv::Matx31f trw = Rrl * tlw + trl;
+
+    cv::Matx44f Trw{Rrw(0,0),Rrw(0,1),Rrw(0,2),trw(0),
+                    Rrw(1,0),Rrw(1,1),Rrw(1,2),trw(1),
+                    Rrw(2,0),Rrw(2,1),Rrw(2,2),trw(2),
+                    0.f,0.f,0.f,1.f};
+
+    return Trw;
+}
+
+cv::Matx31f KeyFrame::GetRightCameraCenter_() {
+    unique_lock<mutex> lock(mMutexPose);
+    cv::Matx33f Rwl = Tcw_.get_minor<3,3>(0,0).t();
+    cv::Matx31f tlr = Tlr_.get_minor<3,1>(0,3);
+
+    cv::Matx31f twr = Rwl * tlr + Ow_;
+
+    return twr;
+}
+
+cv::Matx31f KeyFrame::UnprojectStereo_(int i) {
+    const float z = mvDepth[i];
+    if(z>0)
+    {
+        const float u = mvKeys[i].pt.x;
+        const float v = mvKeys[i].pt.y;
+        const float x = (u-cx)*z*invfx;
+        const float y = (v-cy)*z*invfy;
+        cv::Matx31f x3Dc(x,y,z);
+
+        unique_lock<mutex> lock(mMutexPose);
+        return Twc_.get_minor<3,3>(0,0) * x3Dc + Twc_.get_minor<3,1>(0,3);
+    }
+    else
+        return cv::Matx31f::zeros();
+}
+
+cv::Matx44f KeyFrame::GetPose_()
+{
+    unique_lock<mutex> lock(mMutexPose);
+    return Tcw_;
+}
+
+
+
 } //namespace ORB_SLAM

src/KeyFrameDatabase.cc

@@ -365,7 +365,6 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,vector<Key
 
     if(!lKFsSharingWordsMerge.empty())
     {
-        //cout << "BoW candidates: " << lKFsSharingWordsMerge.size() << endl;
         list<pair<float,KeyFrame*> > lScoreAndMatch;
 
         // Only compare against those keyframes that share enough words
@@ -375,7 +374,6 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,vector<Key
             if((*lit)->mnMergeWords>maxCommonWords)
                 maxCommonWords=(*lit)->mnMergeWords;
         }
-        //cout << "Max common words: " << maxCommonWords << endl;
 
         int minCommonWords = maxCommonWords*0.8f;
 
@@ -391,14 +389,12 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,vector<Key
                 nscores++;
 
                 float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
-                //cout << "KF score: " << si << endl;
 
                 pKFi->mMergeScore = si;
                 if(si>=minScore)
                     lScoreAndMatch.push_back(make_pair(si,pKFi));
             }
         }
-        //cout << "BoW candidates2: " << lScoreAndMatch.size() << endl;
 
         if(!lScoreAndMatch.empty())
         {
@@ -436,8 +432,6 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,vector<Key
             // Return all those keyframes with a score higher than 0.75*bestScore
             float minScoreToRetain = 0.75f*bestAccScore;
 
-            //cout << "Min score to retain: " << minScoreToRetain << endl;
-
             set<KeyFrame*> spAlreadyAddedKF;
             vpMergeCand.reserve(lAccScoreAndMatch.size());
 
@@ -453,12 +447,10 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,vector<Key
                     }
                 }
             }
-            //cout << "Candidates: " << vpMergeCand.size() << endl;
         }
 
     }
 
-    //----
     for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
     {
         list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
@@ -612,7 +604,6 @@ bool compFirst(const pair<float, KeyFrame*> & a, const pair<float, KeyFrame*> &
 void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &vpLoopCand, vector<KeyFrame*> &vpMergeCand, int nNumCandidates)
 {
     list<KeyFrame*> lKFsSharingWords;
-    //set<KeyFrame*> spInsertedKFsSharing;
     set<KeyFrame*> spConnectedKF;
 
     // Search all keyframes that share a word with current frame
@@ -628,10 +619,6 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
             for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
             {
                 KeyFrame* pKFi=*lit;
-                /*if(spConnectedKF.find(pKFi) != spConnectedKF.end())
-                {
-                    continue;
-                }*/
                 if(pKFi->mnPlaceRecognitionQuery!=pKF->mnId)
                 {
                     pKFi->mnPlaceRecognitionWords=0;
@@ -643,11 +630,6 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
                     }
                 }
                 pKFi->mnPlaceRecognitionWords++;
-                /*if(spInsertedKFsSharing.find(pKFi) == spInsertedKFsSharing.end())
-                {
-                    lKFsSharingWords.push_back(pKFi);
-                    spInsertedKFsSharing.insert(pKFi);
-                }*/
 
             }
         }
@@ -717,20 +699,15 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
             bestAccScore=accScore;
     }
 
-    //cout << "Amount of candidates: " << lAccScoreAndMatch.size() << endl;
-
     lAccScoreAndMatch.sort(compFirst);
 
     vpLoopCand.reserve(nNumCandidates);
     vpMergeCand.reserve(nNumCandidates);
     set<KeyFrame*> spAlreadyAddedKF;
-    //cout << "Candidates in score order " << endl;
-    //for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
     int i = 0;
     list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin();
     while(i < lAccScoreAndMatch.size() && (vpLoopCand.size() < nNumCandidates || vpMergeCand.size() < nNumCandidates))
     {
-        //cout << "Accum score: " << it->first << endl;
         KeyFrame* pKFi = it->second;
         if(pKFi->isBad())
             continue;
@@ -750,33 +727,6 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame*> &v
         i++;
         it++;
     }
-    //-------
-
-    // Return all those keyframes with a score higher than 0.75*bestScore
-    /*float minScoreToRetain = 0.75f*bestAccScore;
-    set<KeyFrame*> spAlreadyAddedKF;
-    vpLoopCand.reserve(lAccScoreAndMatch.size());
-    vpMergeCand.reserve(lAccScoreAndMatch.size());
-    for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
-    {
-        const float &si = it->first;
-        if(si>minScoreToRetain)
-        {
-            KeyFrame* pKFi = it->second;
-            if(!spAlreadyAddedKF.count(pKFi))
-            {
-                if(pKF->GetMap() == pKFi->GetMap())
-                {
-                    vpLoopCand.push_back(pKFi);
-                }
-                else
-                {
-                    vpMergeCand.push_back(pKFi);
-                }
-                spAlreadyAddedKF.insert(pKFi);
-            }
-        }
-    }*/
 }
 
 
@@ -894,36 +844,6 @@ vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F, Map
     return vpRelocCandidates;
 }
 
-void KeyFrameDatabase::PreSave()
-{
-    //Save the information about the inverted index of KF to node
-    mvBackupInvertedFileId.resize(mvInvertedFile.size());
-    for(int i = 0, numEl = mvInvertedFile.size(); i < numEl; ++i)
-    {
-        for(std::list<KeyFrame*>::const_iterator it = mvInvertedFile[i].begin(), end = mvInvertedFile[i].end(); it != end; ++it)
-        {
-            mvBackupInvertedFileId[i].push_back((*it)->mnId);
-        }
-    }
-}
-
-void KeyFrameDatabase::PostLoad(map<long unsigned int, KeyFrame*> mpKFid)
-{
-    mvInvertedFile.clear();
-    mvInvertedFile.resize(mpVoc->size());
-    for(unsigned int i = 0; i < mvBackupInvertedFileId.size(); ++i)
-    {
-        for(long unsigned int KFid : mvBackupInvertedFileId[i])
-        {
-            if(mpKFid.find(KFid) != mpKFid.end())
-            {
-                mvInvertedFile[i].push_back(mpKFid[KFid]);
-            }
-        }
-    }
-
-}
-
 void KeyFrameDatabase::SetORBVocabulary(ORBVocabulary* pORBVoc)
 {
     ORBVocabulary** ptr;

src/LocalMapping.cc

@@ -22,6 +22,7 @@
 #include "ORBmatcher.h"
 #include "Optimizer.h"
 #include "Converter.h"
+#include "Config.h"
 
 #include<mutex>
 #include<chrono>
@@ -32,7 +33,7 @@ namespace ORB_SLAM3
 LocalMapping::LocalMapping(System* pSys, Atlas *pAtlas, const float bMonocular, bool bInertial, const string &_strSeqName):
     mpSystem(pSys), mbMonocular(bMonocular), mbInertial(bInertial), mbResetRequested(false), mbResetRequestedActiveMap(false), mbFinishRequested(false), mbFinished(true), mpAtlas(pAtlas), bInitializing(false),
     mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true),
-    mbNewInit(false), mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), mIdxIteration(0), infoInertial(Eigen::MatrixXd::Zero(9,9))
+    mbNewInit(false), mIdxInit(0), mScale(1.0), mInitSect(0), mbNotBA1(true), mbNotBA2(true), infoInertial(Eigen::MatrixXd::Zero(9,9))
 {
     mnMatchesInliers = 0;
 
@@ -43,15 +44,11 @@ LocalMapping::LocalMapping(System* pSys, Atlas *pAtlas, const float bMonocular,
     mNumLM = 0;
     mNumKFCulling=0;
 
-    //DEBUG: times and data from LocalMapping in each frame
+#ifdef REGISTER_TIMES
+    nLBA_exec = 0;
+    nLBA_abort = 0;
+#endif
 
-    strSequence = "";//_strSeqName;
-
-    //f_lm.open("localMapping_times" + strSequence + ".txt");
-    /*f_lm.open("localMapping_times.txt");
-
-    f_lm << "# Timestamp KF, Num CovKFs, Num KFs, Num RecentMPs, Num MPs, processKF, MPCulling, CreateMP, SearchNeigh, BA, KFCulling, [numFixKF_LBA]" << endl;
-    f_lm << fixed;*/
 }
 
 void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser)
@@ -77,28 +74,32 @@ void LocalMapping::Run()
         // Check if there are keyframes in the queue
         if(CheckNewKeyFrames() && !mbBadImu)
         {
-            // std::cout << "LM" << std::endl;
-            std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
 
+#ifdef REGISTER_TIMES
+            double timeLBA_ms = 0;
+            double timeKFCulling_ms = 0;
+
+            std::chrono::steady_clock::time_point time_StartProcessKF = std::chrono::steady_clock::now();
+#endif
             // BoW conversion and insertion in Map
             ProcessNewKeyFrame();
-            std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+#ifdef REGISTER_TIMES
+            std::chrono::steady_clock::time_point time_EndProcessKF = std::chrono::steady_clock::now();
+
+            double timeProcessKF = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndProcessKF - time_StartProcessKF).count();
+            vdKFInsert_ms.push_back(timeProcessKF);
+#endif
 
             // Check recent MapPoints
             MapPointCulling();
-            std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
+#ifdef REGISTER_TIMES
+            std::chrono::steady_clock::time_point time_EndMPCulling = std::chrono::steady_clock::now();
 
+            double timeMPCulling = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndMPCulling - time_EndProcessKF).count();
+            vdMPCulling_ms.push_back(timeMPCulling);
+#endif
             // Triangulate new MapPoints
             CreateNewMapPoints();
-            std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
-
-            // Save here:
-            // # Cov KFs
-            // # tot Kfs
-            // # recent added MPs
-            // # tot MPs
-            // # localMPs in LBA
-            // # fixedKFs in LBA
 
             mbAbortBA = false;
 
@@ -108,24 +109,24 @@ void LocalMapping::Run()
                 SearchInNeighbors();
             }
 
-            std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
-            std::chrono::steady_clock::time_point t5 = t4, t6 = t4;
-            // mbAbortBA = false;
-
-            //DEBUG--
-            /*f_lm << setprecision(0);
-            f_lm << mpCurrentKeyFrame->mTimeStamp*1e9 << ",";
-            f_lm << mpCurrentKeyFrame->GetVectorCovisibleKeyFrames().size() << ",";
-            f_lm << mpCurrentKeyFrame->GetMap()->GetAllKeyFrames().size() << ",";
-            f_lm << mlpRecentAddedMapPoints.size() << ",";
-            f_lm << mpCurrentKeyFrame->GetMap()->GetAllMapPoints().size() << ",";*/
-            //--
+#ifdef REGISTER_TIMES
+            std::chrono::steady_clock::time_point time_EndMPCreation = std::chrono::steady_clock::now();
+
+            double timeMPCreation = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndMPCreation - time_EndMPCulling).count();
+            vdMPCreation_ms.push_back(timeMPCreation);
+#endif
+
+            bool b_doneLBA = false;
             int num_FixedKF_BA = 0;
+            int num_OptKF_BA = 0;
+            int num_MPs_BA = 0;
+            int num_edges_BA = 0;
 
             if(!CheckNewKeyFrames() && !stopRequested())
             {
                 if(mpAtlas->KeyFramesInMap()>2)
                 {
+
                     if(mbInertial && mpCurrentKeyFrame->GetMap()->isImuInitialized())
                     {
                         float dist = cv::norm(mpCurrentKeyFrame->mPrevKF->GetCameraCenter() - mpCurrentKeyFrame->GetCameraCenter()) +
@@ -146,17 +147,36 @@ void LocalMapping::Run()
                         }
 
                         bool bLarge = ((mpTracker->GetMatchesInliers()>75)&&mbMonocular)||((mpTracker->GetMatchesInliers()>100)&&!mbMonocular);
-                        Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(), bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2());
+                        Optimizer::LocalInertialBA(mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA, bLarge, !mpCurrentKeyFrame->GetMap()->GetIniertialBA2());
+                        b_doneLBA = true;
                     }
                     else
                     {
-                        std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
-                        Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA);
-                        std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
+                        Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpCurrentKeyFrame->GetMap(),num_FixedKF_BA,num_OptKF_BA,num_MPs_BA,num_edges_BA);
+                        b_doneLBA = true;
+                    }
+
+                }
+#ifdef REGISTER_TIMES
+                std::chrono::steady_clock::time_point time_EndLBA = std::chrono::steady_clock::now();
+
+                if(b_doneLBA)
+                {
+                    timeLBA_ms = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndLBA - time_EndMPCreation).count();
+                    vdLBASync_ms.push_back(timeLBA_ms);
+
+                    nLBA_exec += 1;
+                    if(mbAbortBA)
+                    {
+                        nLBA_abort += 1;
                     }
+                    vnLBA_edges.push_back(num_edges_BA);
+                    vnLBA_KFopt.push_back(num_OptKF_BA);
+                    vnLBA_KFfixed.push_back(num_FixedKF_BA);
+                    vnLBA_MPs.push_back(num_MPs_BA);
                 }
 
-                t5 = std::chrono::steady_clock::now();
+#endif
 
                 // Initialize IMU here
                 if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial)
@@ -171,11 +191,16 @@ void LocalMapping::Run()
                 // Check redundant local Keyframes
                 KeyFrameCulling();
 
-                t6 = std::chrono::steady_clock::now();
+#ifdef REGISTER_TIMES
+                std::chrono::steady_clock::time_point time_EndKFCulling = std::chrono::steady_clock::now();
+
+                timeKFCulling_ms = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndKFCulling - time_EndLBA).count();
+                vdKFCullingSync_ms.push_back(timeKFCulling_ms);
+#endif
 
                 if ((mTinit<100.0f) && mbInertial)
                 {
-                    if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-mapping is called
+                    if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK)
                     {
                         if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){
                             if (mTinit>5.0f)
@@ -183,20 +208,19 @@ void LocalMapping::Run()
                                 cout << "start VIBA 1" << endl;
                                 mpCurrentKeyFrame->GetMap()->SetIniertialBA1();
                                 if (mbMonocular)
-                                    InitializeIMU(1.f, 1e5, true); // 1.f, 1e5
+                                    InitializeIMU(1.f, 1e5, true);
                                 else
-                                    InitializeIMU(1.f, 1e5, true); // 1.f, 1e5
+                                    InitializeIMU(1.f, 1e5, true);
 
                                 cout << "end VIBA 1" << endl;
                             }
                         }
-                        //else if (mbNotBA2){
                         else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){
-                            if (mTinit>15.0f){ // 15.0f
+                            if (mTinit>15.0f){
                                 cout << "start VIBA 2" << endl;
                                 mpCurrentKeyFrame->GetMap()->SetIniertialBA2();
                                 if (mbMonocular)
-                                    InitializeIMU(0.f, 0.f, true); // 0.f, 0.f
+                                    InitializeIMU(0.f, 0.f, true);
                                 else
                                     InitializeIMU(0.f, 0.f, true);
 
@@ -221,37 +245,27 @@ void LocalMapping::Run()
                 }
             }
 
-            std::chrono::steady_clock::time_point t7 = std::chrono::steady_clock::now();
+#ifdef REGISTER_TIMES
+            vdLBA_ms.push_back(timeLBA_ms);
+            vdKFCulling_ms.push_back(timeKFCulling_ms);
+#endif
+
 
             mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
-            std::chrono::steady_clock::time_point t8 = std::chrono::steady_clock::now();
-
-            double t_procKF = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t1 - t0).count();
-            double t_MPcull = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
-            double t_CheckMP = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t3 - t2).count();
-            double t_searchNeigh = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t4 - t3).count();
-            double t_Opt = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t5 - t4).count();
-            double t_KF_cull = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t6 - t5).count();
-            double t_Insert = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t8 - t7).count();
-
-            //DEBUG--
-            /*f_lm << setprecision(6);
-            f_lm << t_procKF << ",";
-            f_lm << t_MPcull << ",";
-            f_lm << t_CheckMP << ",";
-            f_lm << t_searchNeigh << ",";
-            f_lm << t_Opt << ",";
-            f_lm << t_KF_cull << ",";
-            f_lm << setprecision(0) << num_FixedKF_BA << "\n";*/
-            //--
 
+
+#ifdef REGISTER_TIMES
+            std::chrono::steady_clock::time_point time_EndLocalMap = std::chrono::steady_clock::now();
+
+            double timeLocalMap = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndLocalMap - time_StartProcessKF).count();
+            vdLMTotal_ms.push_back(timeLocalMap);
+#endif
         }
         else if(Stop() && !mbBadImu)
         {
             // Safe area to stop
             while(isStopped() && !CheckFinish())
             {
-                // cout << "LM: usleep if is stopped" << endl;
                 usleep(3000);
             }
             if(CheckFinish())
@@ -266,12 +280,9 @@ void LocalMapping::Run()
         if(CheckFinish())
             break;
 
-        // cout << "LM: normal usleep" << endl;
         usleep(3000);
     }
 
-    //f_lm.close();
-
     SetFinish();
 }
 
@@ -291,7 +302,6 @@ bool LocalMapping::CheckNewKeyFrames()
 
 void LocalMapping::ProcessNewKeyFrame()
 {
-    //cout << "ProcessNewKeyFrame: " << mlNewKeyFrames.size() << endl;
     {
         unique_lock<mutex> lock(mMutexNewKFs);
         mpCurrentKeyFrame = mlNewKeyFrames.front();
@@ -348,7 +358,7 @@ void LocalMapping::MapPointCulling()
     if(mbMonocular)
         nThObs = 2;
     else
-        nThObs = 3; // MODIFICATION_STEREO_IMU here 3
+        nThObs = 3;
     const int cnThObs = nThObs;
 
     int borrar = mlpRecentAddedMapPoints.size();
@@ -406,13 +416,15 @@ void LocalMapping::CreateNewMapPoints()
 
     ORBmatcher matcher(th,false);
 
-    cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();
-    cv::Mat Rwc1 = Rcw1.t();
-    cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
-    cv::Mat Tcw1(3,4,CV_32F);
-    Rcw1.copyTo(Tcw1.colRange(0,3));
-    tcw1.copyTo(Tcw1.col(3));
-    cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
+    auto Rcw1 = mpCurrentKeyFrame->GetRotation_();
+    auto Rwc1 = Rcw1.t();
+    auto tcw1 = mpCurrentKeyFrame->GetTranslation_();
+    cv::Matx44f Tcw1{Rcw1(0,0),Rcw1(0,1),Rcw1(0,2),tcw1(0),
+                     Rcw1(1,0),Rcw1(1,1),Rcw1(1,2),tcw1(1),
+                     Rcw1(2,0),Rcw1(2,1),Rcw1(2,2),tcw1(2),
+                     0.f,0.f,0.f,1.f};
+
+    auto Ow1 = mpCurrentKeyFrame->GetCameraCenter_();
 
     const float &fx1 = mpCurrentKeyFrame->fx;
     const float &fy1 = mpCurrentKeyFrame->fy;
@@ -426,7 +438,7 @@ void LocalMapping::CreateNewMapPoints()
     // Search matches with epipolar restriction and triangulate
     for(size_t i=0; i<vpNeighKFs.size(); i++)
     {
-        if(i>0 && CheckNewKeyFrames())// && (mnMatchesInliers>50))
+        if(i>0 && CheckNewKeyFrames())
             return;
 
         KeyFrame* pKF2 = vpNeighKFs[i];
@@ -434,8 +446,8 @@ void LocalMapping::CreateNewMapPoints()
         GeometricCamera* pCamera1 = mpCurrentKeyFrame->mpCamera, *pCamera2 = pKF2->mpCamera;
 
         // Check first that baseline is not too short
-        cv::Mat Ow2 = pKF2->GetCameraCenter();
-        cv::Mat vBaseline = Ow2-Ow1;
+        auto Ow2 = pKF2->GetCameraCenter_();
+        auto vBaseline = Ow2-Ow1;
         const float baseline = cv::norm(vBaseline);
 
         if(!mbMonocular)
@@ -453,21 +465,23 @@ void LocalMapping::CreateNewMapPoints()
         }
 
         // Compute Fundamental Matrix
-        cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
+        auto F12 = ComputeF12_(mpCurrentKeyFrame,pKF2);
 
         // Search matches that fullfil epipolar constraint
         vector<pair<size_t,size_t> > vMatchedIndices;
         bool bCoarse = mbInertial &&
                 ((!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() && mpCurrentKeyFrame->GetMap()->GetIniertialBA1())||
                  mpTracker->mState==Tracking::RECENTLY_LOST);
-        matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false,bCoarse);
 
-        cv::Mat Rcw2 = pKF2->GetRotation();
-        cv::Mat Rwc2 = Rcw2.t();
-        cv::Mat tcw2 = pKF2->GetTranslation();
-        cv::Mat Tcw2(3,4,CV_32F);
-        Rcw2.copyTo(Tcw2.colRange(0,3));
-        tcw2.copyTo(Tcw2.col(3));
+        matcher.SearchForTriangulation_(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false,bCoarse);
+
+        auto Rcw2 = pKF2->GetRotation_();
+        auto Rwc2 = Rcw2.t();
+        auto tcw2 = pKF2->GetTranslation_();
+        cv::Matx44f Tcw2{Rcw2(0,0),Rcw2(0,1),Rcw2(0,2),tcw2(0),
+                         Rcw2(1,0),Rcw2(1,1),Rcw2(1,2),tcw2(1),
+                         Rcw2(2,0),Rcw2(2,1),Rcw2(2,2),tcw2(2),
+                         0.f,0.f,0.f,1.f};
 
         const float &fx2 = pKF2->fx;
         const float &fy2 = pKF2->fy;
@@ -502,65 +516,65 @@ void LocalMapping::CreateNewMapPoints()
 
             if(mpCurrentKeyFrame->mpCamera2 && pKF2->mpCamera2){
                 if(bRight1 && bRight2){
-                    Rcw1 = mpCurrentKeyFrame->GetRightRotation();
+                    Rcw1 = mpCurrentKeyFrame->GetRightRotation_();
                     Rwc1 = Rcw1.t();
-                    tcw1 = mpCurrentKeyFrame->GetRightTranslation();
-                    Tcw1 = mpCurrentKeyFrame->GetRightPose();
-                    Ow1 = mpCurrentKeyFrame->GetRightCameraCenter();
+                    tcw1 = mpCurrentKeyFrame->GetRightTranslation_();
+                    Tcw1 = mpCurrentKeyFrame->GetRightPose_();
+                    Ow1 = mpCurrentKeyFrame->GetRightCameraCenter_();
 
-                    Rcw2 = pKF2->GetRightRotation();
+                    Rcw2 = pKF2->GetRightRotation_();
                     Rwc2 = Rcw2.t();
-                    tcw2 = pKF2->GetRightTranslation();
-                    Tcw2 = pKF2->GetRightPose();
-                    Ow2 = pKF2->GetRightCameraCenter();
+                    tcw2 = pKF2->GetRightTranslation_();
+                    Tcw2 = pKF2->GetRightPose_();
+                    Ow2 = pKF2->GetRightCameraCenter_();
 
                     pCamera1 = mpCurrentKeyFrame->mpCamera2;
                     pCamera2 = pKF2->mpCamera2;
                 }
                 else if(bRight1 && !bRight2){
-                    Rcw1 = mpCurrentKeyFrame->GetRightRotation();
+                    Rcw1 = mpCurrentKeyFrame->GetRightRotation_();
                     Rwc1 = Rcw1.t();
-                    tcw1 = mpCurrentKeyFrame->GetRightTranslation();
-                    Tcw1 = mpCurrentKeyFrame->GetRightPose();
-                    Ow1 = mpCurrentKeyFrame->GetRightCameraCenter();
+                    tcw1 = mpCurrentKeyFrame->GetRightTranslation_();
+                    Tcw1 = mpCurrentKeyFrame->GetRightPose_();
+                    Ow1 = mpCurrentKeyFrame->GetRightCameraCenter_();
 
-                    Rcw2 = pKF2->GetRotation();
+                    Rcw2 = pKF2->GetRotation_();
                     Rwc2 = Rcw2.t();
-                    tcw2 = pKF2->GetTranslation();
-                    Tcw2 = pKF2->GetPose();
-                    Ow2 = pKF2->GetCameraCenter();
+                    tcw2 = pKF2->GetTranslation_();
+                    Tcw2 = pKF2->GetPose_();
+                    Ow2 = pKF2->GetCameraCenter_();
 
                     pCamera1 = mpCurrentKeyFrame->mpCamera2;
                     pCamera2 = pKF2->mpCamera;
                 }
                 else if(!bRight1 && bRight2){
-                    Rcw1 = mpCurrentKeyFrame->GetRotation();
+                    Rcw1 = mpCurrentKeyFrame->GetRotation_();
                     Rwc1 = Rcw1.t();
-                    tcw1 = mpCurrentKeyFrame->GetTranslation();
-                    Tcw1 = mpCurrentKeyFrame->GetPose();
-                    Ow1 = mpCurrentKeyFrame->GetCameraCenter();
+                    tcw1 = mpCurrentKeyFrame->GetTranslation_();
+                    Tcw1 = mpCurrentKeyFrame->GetPose_();
+                    Ow1 = mpCurrentKeyFrame->GetCameraCenter_();
 
-                    Rcw2 = pKF2->GetRightRotation();
+                    Rcw2 = pKF2->GetRightRotation_();
                     Rwc2 = Rcw2.t();
-                    tcw2 = pKF2->GetRightTranslation();
-                    Tcw2 = pKF2->GetRightPose();
-                    Ow2 = pKF2->GetRightCameraCenter();
+                    tcw2 = pKF2->GetRightTranslation_();
+                    Tcw2 = pKF2->GetRightPose_();
+                    Ow2 = pKF2->GetRightCameraCenter_();
 
                     pCamera1 = mpCurrentKeyFrame->mpCamera;
                     pCamera2 = pKF2->mpCamera2;
                 }
                 else{
-                    Rcw1 = mpCurrentKeyFrame->GetRotation();
+                    Rcw1 = mpCurrentKeyFrame->GetRotation_();
                     Rwc1 = Rcw1.t();
-                    tcw1 = mpCurrentKeyFrame->GetTranslation();
-                    Tcw1 = mpCurrentKeyFrame->GetPose();
-                    Ow1 = mpCurrentKeyFrame->GetCameraCenter();
+                    tcw1 = mpCurrentKeyFrame->GetTranslation_();
+                    Tcw1 = mpCurrentKeyFrame->GetPose_();
+                    Ow1 = mpCurrentKeyFrame->GetCameraCenter_();
 
-                    Rcw2 = pKF2->GetRotation();
+                    Rcw2 = pKF2->GetRotation_();
                     Rwc2 = Rcw2.t();
-                    tcw2 = pKF2->GetTranslation();
-                    Tcw2 = pKF2->GetPose();
-                    Ow2 = pKF2->GetCameraCenter();
+                    tcw2 = pKF2->GetTranslation_();
+                    Tcw2 = pKF2->GetPose_();
+                    Ow2 = pKF2->GetCameraCenter_();
 
                     pCamera1 = mpCurrentKeyFrame->mpCamera;
                     pCamera2 = pKF2->mpCamera;
@@ -568,11 +582,11 @@ void LocalMapping::CreateNewMapPoints()
             }
 
             // Check parallax between rays
-            cv::Mat xn1 = pCamera1->unprojectMat(kp1.pt);
-            cv::Mat xn2 = pCamera2->unprojectMat(kp2.pt);
+            auto xn1 = pCamera1->unprojectMat_(kp1.pt);
+            auto xn2 = pCamera2->unprojectMat_(kp2.pt);
 
-            cv::Mat ray1 = Rwc1*xn1;
-            cv::Mat ray2 = Rwc2*xn2;
+            auto ray1 = Rwc1*xn1;
+            auto ray2 = Rwc2*xn2;
             const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
 
             float cosParallaxStereo = cosParallaxRays+1;
@@ -586,58 +600,66 @@ void LocalMapping::CreateNewMapPoints()
 
             cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
 
-            cv::Mat x3D;
+            cv::Matx31f x3D;
+            bool bEstimated = false;
             if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 ||
                (cosParallaxRays<0.9998 && mbInertial) || (cosParallaxRays<0.9998 && !mbInertial)))
             {
                 // Linear Triangulation Method
-                cv::Mat A(4,4,CV_32F);
-                A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
-                A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
-                A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
-                A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);
-
-                cv::Mat w,u,vt;
+                cv::Matx14f A_r0 = xn1(0) * Tcw1.row(2) - Tcw1.row(0);
+                cv::Matx14f A_r1 = xn1(1) * Tcw1.row(2) - Tcw1.row(1);
+                cv::Matx14f A_r2 = xn2(0) * Tcw2.row(2) - Tcw2.row(0);
+                cv::Matx14f A_r3 = xn2(1) * Tcw2.row(2) - Tcw2.row(1);
+                cv::Matx44f A{A_r0(0), A_r0(1), A_r0(2), A_r0(3),
+                              A_r1(0), A_r1(1), A_r1(2), A_r1(3),
+                              A_r2(0), A_r2(1), A_r2(2), A_r2(3),
+                              A_r3(0), A_r3(1), A_r3(2), A_r3(3)};
+
+                cv::Matx44f u,vt;
+                cv::Matx41f w;
                 cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
 
-                x3D = vt.row(3).t();
+                cv::Matx41f x3D_h = vt.row(3).t();
 
-                if(x3D.at<float>(3)==0)
+                if(x3D_h(3)==0)
                     continue;
 
                 // Euclidean coordinates
-                x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
+                x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3);
+                bEstimated = true;
 
             }
             else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)
             {
-                x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);
+                x3D = mpCurrentKeyFrame->UnprojectStereo_(idx1);
+                bEstimated = true;
             }
             else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)
             {
-                x3D = pKF2->UnprojectStereo(idx2);
+                x3D = pKF2->UnprojectStereo_(idx2);
+                bEstimated = true;
             }
             else
             {
                 continue; //No stereo and very low parallax
             }
 
-            cv::Mat x3Dt = x3D.t();
+            cv::Matx13f x3Dt = x3D.t();
 
-            if(x3Dt.empty()) continue;
+            if(!bEstimated) continue;
             //Check triangulation in front of cameras
-            float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
+            float z1 = Rcw1.row(2).dot(x3Dt)+tcw1(2);
             if(z1<=0)
                 continue;
 
-            float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
+            float z2 = Rcw2.row(2).dot(x3Dt)+tcw2(2);
             if(z2<=0)
                 continue;
 
             //Check reprojection error in first keyframe
             const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
-            const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
-            const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
+            const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1(0);
+            const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1(1);
             const float invz1 = 1.0/z1;
 
             if(!bStereo1)
@@ -664,8 +686,8 @@ void LocalMapping::CreateNewMapPoints()
 
             //Check reprojection error in second keyframe
             const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
-            const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
-            const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
+            const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2(0);
+            const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2(1);
             const float invz2 = 1.0/z2;
             if(!bStereo2)
             {
@@ -688,16 +710,16 @@ void LocalMapping::CreateNewMapPoints()
             }
 
             //Check scale consistency
-            cv::Mat normal1 = x3D-Ow1;
+            auto normal1 = x3D-Ow1;
             float dist1 = cv::norm(normal1);
 
-            cv::Mat normal2 = x3D-Ow2;
+            auto normal2 = x3D-Ow2;
             float dist2 = cv::norm(normal2);
 
             if(dist1==0 || dist2==0)
                 continue;
 
-            if(mbFarPoints && (dist1>=mThFarPoints||dist2>=mThFarPoints)) // MODIFICATION
+            if(mbFarPoints && (dist1>=mThFarPoints||dist2>=mThFarPoints))
                 continue;
 
             const float ratioDist = dist2/dist1;
@@ -707,7 +729,8 @@ void LocalMapping::CreateNewMapPoints()
                 continue;
 
             // Triangulation is succesfull
-            MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpAtlas->GetCurrentMap());
+            cv::Mat x3D_(x3D);
+            MapPoint* pMP = new MapPoint(x3D_,mpCurrentKeyFrame,mpAtlas->GetCurrentMap());
 
             pMP->AddObservation(mpCurrentKeyFrame,idx1);            
             pMP->AddObservation(pKF2,idx2);
@@ -855,6 +878,25 @@ cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
     return K1.t().inv()*t12x*R12*K2.inv();
 }
 
+cv::Matx33f LocalMapping::ComputeF12_(KeyFrame *&pKF1, KeyFrame *&pKF2)
+{
+    auto R1w = pKF1->GetRotation_();
+    auto t1w = pKF1->GetTranslation_();
+    auto R2w = pKF2->GetRotation_();
+    auto t2w = pKF2->GetTranslation_();
+
+    auto R12 = R1w*R2w.t();
+    auto t12 = -R1w*R2w.t()*t2w+t1w;
+
+    auto t12x = SkewSymmetricMatrix_(t12);
+
+    const auto &K1 = pKF1->mpCamera->toK_();
+    const auto &K2 = pKF2->mpCamera->toK_();
+
+
+    return K1.t().inv()*t12x*R12*K2.inv();
+}
+
 void LocalMapping::RequestStop()
 {
     unique_lock<mutex> lock(mMutexStop);
@@ -938,7 +980,7 @@ void LocalMapping::KeyFrameCulling()
     // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
     // in at least other 3 keyframes (in the same or finer scale)
     // We only consider close stereo points
-    const int Nd = 21; // MODIFICATION_STEREO_IMU 20 This should be the same than that one from LIBA
+    const int Nd = 21; // This should be the same than that one from LIBA
     mpCurrentKeyFrame->UpdateBestCovisibles();
     vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
 
@@ -1079,7 +1121,7 @@ void LocalMapping::KeyFrameCulling()
                 pKF->SetBadFlag();
             }
         }
-        if((count > 20 && mbAbortBA) || count>100) // MODIFICATION originally 20 for mbabortBA check just 10 keyframes
+        if((count > 20 && mbAbortBA) || count>100)
         {
             break;
         }
@@ -1094,6 +1136,15 @@ cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v)
             -v.at<float>(1),  v.at<float>(0),              0);
 }
 
+cv::Matx33f LocalMapping::SkewSymmetricMatrix_(const cv::Matx31f &v)
+{
+    cv::Matx33f skew{0.f, -v(2), v(1),
+                     v(2), 0.f, -v(0),
+                     -v(1), v(0), 0.f};
+
+    return skew;
+}
+
 void LocalMapping::RequestReset()
 {
     {
@@ -1306,11 +1357,6 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
     Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA);
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
-    /*cout << "scale after inertial-only optimization: " << mScale << endl;
-    cout << "bg after inertial-only optimization: " << mbg << endl;
-    cout << "ba after inertial-only optimization: " << mba << endl;*/
-
-
     if (mScale<1e-1)
     {
         cout << "scale too small" << endl;
@@ -1339,10 +1385,6 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
             pKF2->bImu = true;
         }
 
-    /*cout << "Before GIBA: " << endl;
-    cout << "ba: " << mpCurrentKeyFrame->GetAccBias() << endl;
-    cout << "bg: " << mpCurrentKeyFrame->GetGyroBias() << endl;*/
-
     std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
     if (bFIBA)
     {
@@ -1379,15 +1421,6 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
     mpTracker->mState=Tracking::OK;
     bInitializing = false;
 
-
-    /*cout << "After GIBA: " << endl;
-    cout << "ba: " << mpCurrentKeyFrame->GetAccBias() << endl;
-    cout << "bg: " << mpCurrentKeyFrame->GetGyroBias() << endl;
-    double t_inertial_only = std::chrono::duration_cast<std::chrono::duration<double> >(t1 - t0).count();
-    double t_update = std::chrono::duration_cast<std::chrono::duration<double> >(t3 - t2).count();
-    double t_viba = std::chrono::duration_cast<std::chrono::duration<double> >(t5 - t4).count();
-    cout << t_inertial_only << ", " << t_update << ", " << t_viba << endl;*/
-
     mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex();
 
     return;
@@ -1428,7 +1461,7 @@ void LocalMapping::ScaleRefinement()
     Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale);
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
-    if (mScale<1e-1) // 1e-1
+    if (mScale<1e-1)
     {
         cout << "scale too small" << endl;
         bInitializing=false;

src/MLPnPsolver.cpp

@@ -369,13 +369,10 @@ namespace ORB_SLAM3 {
 
         Eigen::Matrix3d planarTest = points3 * points3.transpose();
         Eigen::FullPivHouseholderQR<Eigen::Matrix3d> rankTest(planarTest);
-        //int r, c;
-        //double minEigenVal = abs(eigen_solver.eigenvalues().real().minCoeff(&r, &c));
         Eigen::Matrix3d eigenRot;
         eigenRot.setIdentity();
 
         // if yes -> transform points to new eigen frame
-        //if (minEigenVal < 1e-3 || minEigenVal == 0.0)
         //rankTest.setThreshold(1e-10);
         if (rankTest.rank() == 2) {
             planar = true;
@@ -529,7 +526,6 @@ namespace ORB_SLAM3 {
             tmp << 0.0, result1(0, 0), result1(1, 0),
                     0.0, result1(2, 0), result1(3, 0),
                     0.0, result1(4, 0), result1(5, 0);
-            //double scale = 1 / sqrt(tmp.col(1).norm() * tmp.col(2).norm());
             // row 3
             tmp.col(0) = tmp.col(1).cross(tmp.col(2));
             tmp.transposeInPlace();
@@ -732,7 +728,6 @@ namespace ORB_SLAM3 {
             // solve
             Eigen::LDLT<Eigen::MatrixXd> chol(A);
             dx = chol.solve(g);
-            //dx = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(g);
             // this is to prevent the solution from falling into a wrong minimum
             // if the linear estimate is spurious
             if (dx.array().abs().maxCoeff() > 5.0 || dx.array().abs().minCoeff() > 1.0)
@@ -757,7 +752,6 @@ namespace ORB_SLAM3 {
         rodrigues_t w(x[0], x[1], x[2]);
         translation_t T(x[3], x[4], x[5]);
 
-        //rotation_t R = math::cayley2rot(c);
         rotation_t R = rodrigues2rot(w);
         int ii = 0;
 

src/Map.cc

@@ -489,154 +489,5 @@ void Map::SetLastMapChange(int currentChangeId)
     mnMapChangeNotified = currentChangeId;
 }
 
-void Map::printReprojectionError(list<KeyFrame*> &lpLocalWindowKFs, KeyFrame* mpCurrentKF, string &name, string &name_folder)
-{
-    string path_imgs = "./" + name_folder + "/";
-    for(KeyFrame* pKFi : lpLocalWindowKFs)
-    {
-        //cout << "KF " << pKFi->mnId << endl;
-        cv::Mat img_i = cv::imread(pKFi->mNameFile, CV_LOAD_IMAGE_UNCHANGED);
-        //cout << "Image -> " << img_i.cols << ", " << img_i.rows << endl;
-        cv::cvtColor(img_i, img_i, CV_GRAY2BGR);
-        //cout << "Change of color in the image " << endl;
-
-        vector<MapPoint*> vpMPs = pKFi->GetMapPointMatches();
-        int num_points = 0;
-        for(int j=0; j<vpMPs.size(); ++j)
-        {
-            MapPoint* pMPij = vpMPs[j];
-            if(!pMPij || pMPij->isBad())
-            {
-                continue;
-            }
-
-            cv::KeyPoint point_img = pKFi->mvKeysUn[j];
-            cv::Point2f reproj_p;
-            float u, v;
-            bool bIsInImage = pKFi->ProjectPointUnDistort(pMPij, reproj_p, u, v);
-            if(bIsInImage){
-                //cout << "Reproj in the image" << endl;
-                cv::circle(img_i, point_img.pt, 1/*point_img.octave*/, cv::Scalar(0, 255, 0));
-                cv::line(img_i, point_img.pt, reproj_p, cv::Scalar(0, 0, 255));
-                num_points++;
-            }
-            else
-            {
-                //cout << "Reproj out of the image" << endl;
-                cv::circle(img_i, point_img.pt, point_img.octave, cv::Scalar(0, 0, 255));
-            }
-
-        }
-        //cout << "Image painted" << endl;
-        string filename_img = path_imgs +  "KF" + to_string(mpCurrentKF->mnId) + "_" + to_string(pKFi->mnId) +  name + "points" + to_string(num_points) + ".png";
-        cv::imwrite(filename_img, img_i);
-    }
-
-}
-
-void Map::PreSave(std::set<GeometricCamera*> &spCams)
-{
-    int nMPWithoutObs = 0;
-    for(MapPoint* pMPi : mspMapPoints)
-    {
-        if(pMPi->GetObservations().size() == 0)
-        {
-            nMPWithoutObs++;
-        }
-        map<KeyFrame*, std::tuple<int,int>> mpObs = pMPi->GetObservations();
-        for(map<KeyFrame*, std::tuple<int,int>>::iterator it= mpObs.begin(), end=mpObs.end(); it!=end; ++it)
-        {
-            if(it->first->GetMap() != this)
-            {
-                pMPi->EraseObservation(it->first); //We need to find where the KF is set as Bad but the observation is not removed
-            }
-
-        }
-    }
-    cout << "  Bad MapPoints removed" << endl;
-
-    // Saves the id of KF origins
-    mvBackupKeyFrameOriginsId.reserve(mvpKeyFrameOrigins.size());
-    for(int i = 0, numEl = mvpKeyFrameOrigins.size(); i < numEl; ++i)
-    {
-        mvBackupKeyFrameOriginsId.push_back(mvpKeyFrameOrigins[i]->mnId);
-    }
-
-    mvpBackupMapPoints.clear();
-    // Backup of set container to vector
-    //std::copy(mspMapPoints.begin(), mspMapPoints.end(), std::back_inserter(mvpBackupMapPoints));
-    for(MapPoint* pMPi : mspMapPoints)
-    {
-        //cout << "Pre-save of mappoint " << pMPi->mnId << endl;
-        mvpBackupMapPoints.push_back(pMPi);
-        pMPi->PreSave(mspKeyFrames,mspMapPoints);
-    }
-    cout << "  MapPoints back up done!!" << endl;
-
-    mvpBackupKeyFrames.clear();
-    //std::copy(mspKeyFrames.begin(), mspKeyFrames.end(), std::back_inserter(mvpBackupKeyFrames));
-    for(KeyFrame* pKFi : mspKeyFrames)
-    {
-        mvpBackupKeyFrames.push_back(pKFi);
-        pKFi->PreSave(mspKeyFrames,mspMapPoints, spCams);
-    }
-    cout << "  KeyFrames back up done!!" << endl;
-
-    mnBackupKFinitialID = -1;
-    if(mpKFinitial)
-    {
-        mnBackupKFinitialID = mpKFinitial->mnId;
-    }
-
-    mnBackupKFlowerID = -1;
-    if(mpKFlowerID)
-    {
-        mnBackupKFlowerID = mpKFlowerID->mnId;
-    }
-
-}
-
-void Map::PostLoad(KeyFrameDatabase* pKFDB, ORBVocabulary* pORBVoc, map<long unsigned int, KeyFrame*>& mpKeyFrameId, map<unsigned int, GeometricCamera*> &mpCams)
-{
-    std::copy(mvpBackupMapPoints.begin(), mvpBackupMapPoints.end(), std::inserter(mspMapPoints, mspMapPoints.begin()));
-    std::copy(mvpBackupKeyFrames.begin(), mvpBackupKeyFrames.end(), std::inserter(mspKeyFrames, mspKeyFrames.begin()));
-
-    map<long unsigned int,MapPoint*> mpMapPointId;
-    for(MapPoint* pMPi : mspMapPoints)
-    {
-        pMPi->UpdateMap(this);
-        mpMapPointId[pMPi->mnId] = pMPi;
-    }
-
-    //map<long unsigned int, KeyFrame*> mpKeyFrameId;
-    for(KeyFrame* pKFi : mspKeyFrames)
-    {
-        pKFi->UpdateMap(this);
-        pKFi->SetORBVocabulary(pORBVoc);
-        pKFi->SetKeyFrameDatabase(pKFDB);
-        mpKeyFrameId[pKFi->mnId] = pKFi;
-    }
-    cout << "Number of KF: " << mspKeyFrames.size() << endl;
-    cout << "Number of MP: " << mspMapPoints.size() << endl;
-
-    // References reconstruction between different instances
-    for(MapPoint* pMPi : mspMapPoints)
-    {
-        //cout << "Post-Load of mappoint " << pMPi->mnId << endl;
-        pMPi->PostLoad(mpKeyFrameId, mpMapPointId);
-    }
-    cout << "End to rebuild MapPoint references" << endl;
-
-    for(KeyFrame* pKFi : mspKeyFrames)
-    {
-        pKFi->PostLoad(mpKeyFrameId, mpMapPointId, mpCams);
-        pKFDB->add(pKFi);
-    }
-
-    cout << "End to rebuild KeyFrame references" << endl;
-
-    mvpBackupMapPoints.clear();
-}
-
 
 } //namespace ORB_SLAM3

src/MapDrawer.cc

@@ -184,14 +184,10 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const b
                 glLineWidth(mKeyFrameLineWidth*5);
                 glColor3f(1.0f,0.0f,0.0f);
                 glBegin(GL_LINES);
-
-                //cout << "Initial KF: " << mpAtlas->GetCurrentMap()->GetOriginKF()->mnId << endl;
-                //cout << "Parent KF: " << vpKFs[i]->mnId << endl;
             }
             else
             {
                 glLineWidth(mKeyFrameLineWidth);
-                //glColor3f(0.0f,0.0f,1.0f);
                 glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]);
                 glBegin(GL_LINES);
             }
@@ -220,32 +216,6 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const b
 
             glPopMatrix();
 
-            //Draw lines with Loop and Merge candidates
-            /*glLineWidth(mGraphLineWidth);
-            glColor4f(1.0f,0.6f,0.0f,1.0f);
-            glBegin(GL_LINES);
-            cv::Mat Ow = pKF->GetCameraCenter();
-            const vector<KeyFrame*> vpLoopCandKFs = pKF->mvpLoopCandKFs;
-            if(!vpLoopCandKFs.empty())
-            {
-                for(vector<KeyFrame*>::const_iterator vit=vpLoopCandKFs.begin(), vend=vpLoopCandKFs.end(); vit!=vend; vit++)
-                {
-                    cv::Mat Ow2 = (*vit)->GetCameraCenter();
-                    glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
-                    glVertex3f(Ow2.at<float>(0),Ow2.at<float>(1),Ow2.at<float>(2));
-                }
-            }
-            const vector<KeyFrame*> vpMergeCandKFs = pKF->mvpMergeCandKFs;
-            if(!vpMergeCandKFs.empty())
-            {
-                for(vector<KeyFrame*>::const_iterator vit=vpMergeCandKFs.begin(), vend=vpMergeCandKFs.end(); vit!=vend; vit++)
-                {
-                    cv::Mat Ow2 = (*vit)->GetCameraCenter();
-                    glVertex3f(Ow.at<float>(0),Ow.at<float>(1),Ow.at<float>(2));
-                    glVertex3f(Ow2.at<float>(0),Ow2.at<float>(1),Ow2.at<float>(2));
-                }
-            }*/
-
             glEnd();
         }
     }
@@ -256,7 +226,6 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const b
         glColor4f(0.0f,1.0f,0.0f,0.6f);
         glBegin(GL_LINES);
 
-        // cout << "-----------------Draw graph-----------------" << endl;
         for(size_t i=0; i<vpKFs.size(); i++)
         {
             // Covisibility Graph
@@ -351,7 +320,6 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph, const b
                 else
                 {
                     glLineWidth(mKeyFrameLineWidth);
-                    //glColor3f(0.0f,0.0f,1.0f);
                     glColor3f(mfFrameColors[index_color][0],mfFrameColors[index_color][1],mfFrameColors[index_color][2]);
                     glBegin(GL_LINES);
                 }

src/MapPoint.cc

@@ -44,7 +44,9 @@ MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap):
     mnOriginMapId(pMap->GetId())
 {
     Pos.copyTo(mWorldPos);
+    mWorldPosx = cv::Matx31f(Pos.at<float>(0), Pos.at<float>(1), Pos.at<float>(2));
     mNormalVector = cv::Mat::zeros(3,1,CV_32F);
+    mNormalVectorx = cv::Matx31f::zeros();
 
     mbTrackInViewR = false;
     mbTrackInView = false;
@@ -67,6 +69,7 @@ MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF,
     mpHostKF = pHostKF;
 
     mNormalVector = cv::Mat::zeros(3,1,CV_32F);
+    mNormalVectorx = cv::Matx31f::zeros();
 
     // Worldpos is not set
     // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
@@ -81,6 +84,8 @@ MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF
     mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap), mnOriginMapId(pMap->GetId())
 {
     Pos.copyTo(mWorldPos);
+    mWorldPosx = cv::Matx31f(Pos.at<float>(0), Pos.at<float>(1), Pos.at<float>(2));
+
     cv::Mat Ow;
     if(pFrame -> Nleft == -1 || idxF < pFrame -> Nleft){
         Ow = pFrame->GetCameraCenter();
@@ -94,6 +99,8 @@ MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF
     }
     mNormalVector = mWorldPos - Ow;
     mNormalVector = mNormalVector/cv::norm(mNormalVector);
+    mNormalVectorx = cv::Matx31f(mNormalVector.at<float>(0), mNormalVector.at<float>(1), mNormalVector.at<float>(2));
+
 
     cv::Mat PC = Pos - Ow;
     const float dist = cv::norm(PC);
@@ -118,6 +125,7 @@ void MapPoint::SetWorldPos(const cv::Mat &Pos)
     unique_lock<mutex> lock2(mGlobalMutex);
     unique_lock<mutex> lock(mMutexPos);
     Pos.copyTo(mWorldPos);
+    mWorldPosx = cv::Matx31f(Pos.at<float>(0), Pos.at<float>(1), Pos.at<float>(2));
 }
 
 cv::Mat MapPoint::GetWorldPos()
@@ -132,6 +140,18 @@ cv::Mat MapPoint::GetNormal()
     return mNormalVector.clone();
 }
 
+cv::Matx31f MapPoint::GetWorldPos2()
+{
+    unique_lock<mutex> lock(mMutexPos);
+    return mWorldPosx;
+}
+
+cv::Matx31f MapPoint::GetNormal2()
+{
+    unique_lock<mutex> lock(mMutexPos);
+    return mNormalVectorx;
+}
+
 KeyFrame* MapPoint::GetReferenceKeyFrame()
 {
     unique_lock<mutex> lock(mMutexFeatures);
@@ -172,7 +192,6 @@ void MapPoint::EraseObservation(KeyFrame* pKF)
         unique_lock<mutex> lock(mMutexFeatures);
         if(mObservations.count(pKF))
         {
-            //int idx = mObservations[pKF];
             tuple<int,int> indexes = mObservations[pKF];
             int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
 
@@ -481,7 +500,6 @@ void MapPoint::UpdateNormalAndDepth()
         level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave;
     }
 
-    //const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
     const float levelScaleFactor =  pRefKF->mvScaleFactors[level];
     const int nLevels = pRefKF->mnScaleLevels;
 
@@ -490,6 +508,7 @@ void MapPoint::UpdateNormalAndDepth()
         mfMaxDistance = dist*levelScaleFactor;
         mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];
         mNormalVector = normal/n;
+        mNormalVectorx = cv::Matx31f(mNormalVector.at<float>(0), mNormalVector.at<float>(1), mNormalVector.at<float>(2));
     }
 }
 
@@ -497,6 +516,7 @@ void MapPoint::SetNormalVector(cv::Mat& normal)
 {
     unique_lock<mutex> lock3(mMutexPos);
     mNormalVector = normal;
+    mNormalVectorx = cv::Matx31f(mNormalVector.at<float>(0), mNormalVector.at<float>(1), mNormalVector.at<float>(2));
 }
 
 float MapPoint::GetMinDistanceInvariance()
@@ -545,18 +565,6 @@ int MapPoint::PredictScale(const float &currentDist, Frame* pF)
     return nScale;
 }
 
-void MapPoint::PrintObservations()
-{
-    cout << "MP_OBS: MP " << mnId << endl;
-    for(map<KeyFrame*,tuple<int,int>>::iterator mit=mObservations.begin(), mend=mObservations.end(); mit!=mend; mit++)
-    {
-        KeyFrame* pKFi = mit->first;
-        tuple<int,int> indexes = mit->second;
-        int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
-        cout << "--OBS in KF " << pKFi->mnId << " in map " << pKFi->GetMap()->GetId() << endl;
-    }
-}
-
 Map* MapPoint::GetMap()
 {
     unique_lock<mutex> lock(mMutexMap);
@@ -569,66 +577,4 @@ void MapPoint::UpdateMap(Map* pMap)
     mpMap = pMap;
 }
 
-void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
-{
-    mBackupReplacedId = -1;
-    if(mpReplaced && spMP.find(mpReplaced) != spMP.end())
-        mBackupReplacedId = mpReplaced->mnId;
-
-    mBackupObservationsId1.clear();
-    mBackupObservationsId2.clear();
-    // Save the id and position in each KF who view it
-    for(std::map<KeyFrame*,std::tuple<int,int> >::const_iterator it = mObservations.begin(), end = mObservations.end(); it != end; ++it)
-    {
-        KeyFrame* pKFi = it->first;
-        if(spKF.find(pKFi) != spKF.end())
-        {
-            mBackupObservationsId1[it->first->mnId] = get<0>(it->second);
-            mBackupObservationsId2[it->first->mnId] = get<1>(it->second);
-        }
-        else
-        {
-            EraseObservation(pKFi);
-        }
-    }
-
-    // Save the id of the reference KF
-    if(spKF.find(mpRefKF) != spKF.end())
-    {
-        mBackupRefKFId = mpRefKF->mnId;
-    }
-}
-
-void MapPoint::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid, map<long unsigned int, MapPoint*>& mpMPid)
-{
-    mpRefKF = mpKFid[mBackupRefKFId];
-    if(!mpRefKF)
-    {
-        cout << "MP without KF reference " << mBackupRefKFId << "; Num obs: " << nObs << endl;
-    }
-    mpReplaced = static_cast<MapPoint*>(NULL);
-    if(mBackupReplacedId>=0)
-    {
-       map<long unsigned int, MapPoint*>::iterator it = mpMPid.find(mBackupReplacedId);
-       if (it != mpMPid.end())
-        mpReplaced = it->second;
-    }
-
-    mObservations.clear();
-
-    for(map<long unsigned int, int>::const_iterator it = mBackupObservationsId1.begin(), end = mBackupObservationsId1.end(); it != end; ++it)
-    {
-        KeyFrame* pKFi = mpKFid[it->first];
-        map<long unsigned int, int>::const_iterator it2 = mBackupObservationsId2.find(it->first);
-        std::tuple<int, int> indexes = tuple<int,int>(it->second,it2->second);
-        if(pKFi)
-        {
-           mObservations[pKFi] = indexes;
-        }
-    }
-
-    mBackupObservationsId1.clear();
-    mBackupObservationsId2.clear();
-}
-
 } //namespace ORB_SLAM

src/ORBextractor.cc

@@ -764,7 +764,7 @@ namespace ORB_SLAM3
     {
         allKeypoints.resize(nlevels);
 
-        const float W = 30;
+        const float W = 35;
 
         for (int level = 0; level < nlevels; ++level)
         {

src/ORBmatcher.cc

@@ -1205,6 +1205,250 @@ int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F
     return nmatches;
 }
 
+    int ORBmatcher::SearchForTriangulation_(KeyFrame *pKF1, KeyFrame *pKF2, cv::Matx33f F12,
+                                           vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo, const bool bCoarse)
+    {
+        const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
+        const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
+
+        //Compute epipole in second image
+        auto Cw = pKF1->GetCameraCenter_();
+        auto R2w = pKF2->GetRotation_();
+        auto t2w = pKF2->GetTranslation_();
+        auto C2 = R2w*Cw+t2w;
+
+        cv::Point2f ep = pKF2->mpCamera->project(C2);
+
+        auto R1w = pKF1->GetRotation_();
+        auto t1w = pKF1->GetTranslation_();
+
+        cv::Matx33f R12;
+        cv::Matx31f t12;
+
+        cv::Matx33f Rll,Rlr,Rrl,Rrr;
+        cv::Matx31f tll,tlr,trl,trr;
+
+        GeometricCamera* pCamera1 = pKF1->mpCamera, *pCamera2 = pKF2->mpCamera;
+
+        if(!pKF1->mpCamera2 && !pKF2->mpCamera2){
+            R12 = R1w*R2w.t();
+            t12 = -R1w*R2w.t()*t2w+t1w;
+        }
+        else{
+            Rll = pKF1->GetRotation_() * pKF2->GetRotation_().t();
+            Rlr = pKF1->GetRotation_() * pKF2->GetRightRotation_().t();
+            Rrl = pKF1->GetRightRotation_() * pKF2->GetRotation_().t();
+            Rrr = pKF1->GetRightRotation_() * pKF2->GetRightRotation_().t();
+
+            tll = pKF1->GetRotation_() * (-pKF2->GetRotation_().t() * pKF2->GetTranslation_()) + pKF1->GetTranslation_();
+            tlr = pKF1->GetRotation_() * (-pKF2->GetRightRotation_().t() * pKF2->GetRightTranslation_()) + pKF1->GetTranslation_();
+            trl = pKF1->GetRightRotation_() * (-pKF2->GetRotation_().t() * pKF2->GetTranslation_()) + pKF1->GetRightTranslation_();
+            trr = pKF1->GetRightRotation_() * (-pKF2->GetRightRotation_().t() * pKF2->GetRightTranslation_()) + pKF1->GetRightTranslation_();
+        }
+
+        // Find matches between not tracked keypoints
+        // Matching speed-up by ORB Vocabulary
+        // Compare only ORB that share the same node
+
+        int nmatches=0;
+        vector<bool> vbMatched2(pKF2->N,false);
+        vector<int> vMatches12(pKF1->N,-1);
+
+        vector<int> rotHist[HISTO_LENGTH];
+        for(int i=0;i<HISTO_LENGTH;i++)
+            rotHist[i].reserve(500);
+
+        const float factor = 1.0f/HISTO_LENGTH;
+
+        DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
+        DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
+        DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
+        DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();
+
+        while(f1it!=f1end && f2it!=f2end)
+        {
+            if(f1it->first == f2it->first)
+            {
+                for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
+                {
+                    const size_t idx1 = f1it->second[i1];
+
+                    MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
+
+                    // If there is already a MapPoint skip
+                    if(pMP1)
+                    {
+                        continue;
+                    }
+
+                    const bool bStereo1 = (!pKF1->mpCamera2 && pKF1->mvuRight[idx1]>=0);
+
+                    if(bOnlyStereo)
+                        if(!bStereo1)
+                            continue;
+
+
+                    const cv::KeyPoint &kp1 = (pKF1 -> NLeft == -1) ? pKF1->mvKeysUn[idx1]
+                                                                    : (idx1 < pKF1 -> NLeft) ? pKF1 -> mvKeys[idx1]
+                                                                                             : pKF1 -> mvKeysRight[idx1 - pKF1 -> NLeft];
+
+                    const bool bRight1 = (pKF1 -> NLeft == -1 || idx1 < pKF1 -> NLeft) ? false
+                                                                                       : true;
+                    //if(bRight1) continue;
+                    const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
+
+                    int bestDist = TH_LOW;
+                    int bestIdx2 = -1;
+
+                    for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
+                    {
+                        size_t idx2 = f2it->second[i2];
+
+                        MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
+
+                        // If we have already matched or there is a MapPoint skip
+                        if(vbMatched2[idx2] || pMP2)
+                            continue;
+
+                        const bool bStereo2 = (!pKF2->mpCamera2 &&  pKF2->mvuRight[idx2]>=0);
+
+                        if(bOnlyStereo)
+                            if(!bStereo2)
+                                continue;
+
+                        const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
+
+                        const int dist = DescriptorDistance(d1,d2);
+
+                        if(dist>TH_LOW || dist>bestDist)
+                            continue;
+
+                        const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2]
+                                                                        : (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2]
+                                                                                                 : pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft];
+                        const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false
+                                                                                           : true;
+
+                        if(!bStereo1 && !bStereo2 && !pKF1->mpCamera2)
+                        {
+                            const float distex = ep.x-kp2.pt.x;
+                            const float distey = ep.y-kp2.pt.y;
+                            if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
+                            {
+                                continue;
+                            }
+                        }
+
+                        if(pKF1->mpCamera2 && pKF2->mpCamera2){
+                            if(bRight1 && bRight2){
+                                R12 = Rrr;
+                                t12 = trr;
+
+                                pCamera1 = pKF1->mpCamera2;
+                                pCamera2 = pKF2->mpCamera2;
+                            }
+                            else if(bRight1 && !bRight2){
+                                R12 = Rrl;
+                                t12 = trl;
+
+                                pCamera1 = pKF1->mpCamera2;
+                                pCamera2 = pKF2->mpCamera;
+                            }
+                            else if(!bRight1 && bRight2){
+                                R12 = Rlr;
+                                t12 = tlr;
+
+                                pCamera1 = pKF1->mpCamera;
+                                pCamera2 = pKF2->mpCamera2;
+                            }
+                            else{
+                                R12 = Rll;
+                                t12 = tll;
+
+                                pCamera1 = pKF1->mpCamera;
+                                pCamera2 = pKF2->mpCamera;
+                            }
+
+                        }
+
+
+                        if(pCamera1->epipolarConstrain_(pCamera2,kp1,kp2,R12,t12,pKF1->mvLevelSigma2[kp1.octave],pKF2->mvLevelSigma2[kp2.octave])||bCoarse) // MODIFICATION_2
+                        {
+                            bestIdx2 = idx2;
+                            bestDist = dist;
+                        }
+                    }
+
+                    if(bestIdx2>=0)
+                    {
+                        const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[bestIdx2]
+                                                                        : (bestIdx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[bestIdx2]
+                                                                                                     : pKF2 -> mvKeysRight[bestIdx2 - pKF2 -> NLeft];
+                        vMatches12[idx1]=bestIdx2;
+                        nmatches++;
+
+                        if(mbCheckOrientation)
+                        {
+                            float rot = kp1.angle-kp2.angle;
+                            if(rot<0.0)
+                                rot+=360.0f;
+                            int bin = round(rot*factor);
+                            if(bin==HISTO_LENGTH)
+                                bin=0;
+                            assert(bin>=0 && bin<HISTO_LENGTH);
+                            rotHist[bin].push_back(idx1);
+                        }
+                    }
+                }
+
+                f1it++;
+                f2it++;
+            }
+            else if(f1it->first < f2it->first)
+            {
+                f1it = vFeatVec1.lower_bound(f2it->first);
+            }
+            else
+            {
+                f2it = vFeatVec2.lower_bound(f1it->first);
+            }
+        }
+
+        if(mbCheckOrientation)
+        {
+            int ind1=-1;
+            int ind2=-1;
+            int ind3=-1;
+
+            ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
+
+            for(int i=0; i<HISTO_LENGTH; i++)
+            {
+                if(i==ind1 || i==ind2 || i==ind3)
+                    continue;
+                for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
+                {
+                    vMatches12[rotHist[i][j]]=-1;
+                    nmatches--;
+                }
+            }
+
+        }
+
+        vMatchedPairs.clear();
+        vMatchedPairs.reserve(nmatches);
+
+        for(size_t i=0, iend=vMatches12.size(); i<iend; i++)
+        {
+            if(vMatches12[i]<0)
+                continue;
+            vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
+        }
+
+        return nmatches;
+    }
+
+
     int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                            vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo, vector<cv::Mat> &vMatchedPoints)
     {
@@ -1438,8 +1682,6 @@ int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const
             continue;
         }
 
-        /*if(pMP->isBad() || pMP->IsInKeyFrame(pKF))
-            continue;*/
         if(pMP->isBad())
         {
             count_bad++;
@@ -1595,16 +1837,6 @@ int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const
 
     }
 
-    /*cout << "count_notMP = " << count_notMP << endl;
-    cout << "count_bad = " << count_bad << endl;
-    cout << "count_isinKF = " << count_isinKF << endl;
-    cout << "count_negdepth = " << count_negdepth << endl;
-    cout << "count_notinim = " << count_notinim << endl;
-    cout << "count_dist = " << count_dist << endl;
-    cout << "count_normal = " << count_normal << endl;
-    cout << "count_notidx = " << count_notidx << endl;
-    cout << "count_thcheck = " << count_thcheck << endl;
-    cout << "tot fused points: " << nFused << endl;*/
     return nFused;
 }
 

src/Sim3Solver.cc

@@ -304,7 +304,7 @@ cv::Mat Sim3Solver::find(vector<bool> &vbInliers12, int &nInliers)
 
 void Sim3Solver::ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C)
 {
-    cv::reduce(P,C,1,CV_REDUCE_SUM);
+    cv::reduce(P,C,1,cv::REDUCE_SUM);
     C = C/P.cols;
 
     for(int i=0; i<P.cols; i++)

src/System.cc

@@ -92,76 +92,7 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
     mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
 
     //Create the Atlas
-    //mpMap = new Map();
     mpAtlas = new Atlas(0);
-    //----
-
-    /*if(strLoadingFile.empty())
-    {
-        //Load ORB Vocabulary
-        cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
-
-        mpVocabulary = new ORBVocabulary();
-        bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
-        if(!bVocLoad)
-        {
-            cerr << "Wrong path to vocabulary. " << endl;
-            cerr << "Falied to open at: " << strVocFile << endl;
-            exit(-1);
-        }
-        cout << "Vocabulary loaded!" << endl << endl;
-
-        //Create KeyFrame Database
-        mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
-
-        //Create the Atlas
-        //mpMap = new Map();
-        mpAtlas = new Atlas(0);
-    }
-    else
-    {
-        //Load ORB Vocabulary
-        cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
-
-        mpVocabulary = new ORBVocabulary();
-        bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
-        if(!bVocLoad)
-        {
-            cerr << "Wrong path to vocabulary. " << endl;
-            cerr << "Falied to open at: " << strVocFile << endl;
-            exit(-1);
-        }
-        cout << "Vocabulary loaded!" << endl << endl;
-
-        cout << "Load File" << endl;
-
-        // Load the file with an earlier session
-        //clock_t start = clock();
-        bool isRead = LoadAtlas(strLoadingFile,BINARY_FILE);
-
-        if(!isRead)
-        {
-            cout << "Error to load the file, please try with other session file or vocabulary file" << endl;
-            exit(-1);
-        }
-        mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
-
-        mpAtlas->SetKeyFrameDababase(mpKeyFrameDatabase);
-        mpAtlas->SetORBVocabulary(mpVocabulary);
-        mpAtlas->PostLoad();
-        //cout << "KF in DB: " << mpKeyFrameDatabase->mnNumKFs << "; words: " << mpKeyFrameDatabase->mnNumWords << endl;
-
-        loadedAtlas = true;
-
-        mpAtlas->CreateNewMap();
-
-        //clock_t timeElapsed = clock() - start;
-        //unsigned msElapsed = timeElapsed / (CLOCKS_PER_SEC / 1000);
-        //cout << "Binary file read in " << msElapsed << " ms" << endl;
-
-        //usleep(10*1000*1000);
-    }*/
-
 
     if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR)
         mpAtlas->SetInertialSensor();
@@ -179,7 +110,6 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
     //Initialize the Local Mapping thread and launch
     mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR, mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO, strSequence);
     mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);
-    mpLocalMapper->mInitFr = initFr;
     mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];
     if(mpLocalMapper->mThFarPoints!=0)
     {
@@ -190,7 +120,6 @@ System::System(const string &strVocFile, const string &strSettingsFile, const eS
         mpLocalMapper->mbFarPoints = false;
 
     //Initialize the Loop Closing thread and launch
-    // mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR
     mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR); // mSensor!=MONOCULAR);
     mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
 
@@ -272,11 +201,8 @@ cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const
         for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
             mpTracker->GrabImuData(vImuMeas[i_imu]);
 
-    // std::cout << "start GrabImageStereo" << std::endl;
     cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp,filename);
 
-    // std::cout << "out grabber" << std::endl;
-
     unique_lock<mutex> lock2(mMutexState);
     mTrackingState = mpTracker->mState;
     mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
@@ -473,6 +399,10 @@ void System::Shutdown()
 
     if(mpViewer)
         pangolin::BindToContext("ORB-SLAM2: Map Viewer");
+
+#ifdef REGISTER_TIMES
+    mpTracker->PrintTimeStats();
+#endif
 }
 
 
@@ -605,7 +535,6 @@ void System::SaveTrajectoryEuRoC(const string &filename)
 
     ofstream f;
     f.open(filename.c_str());
-    // cout << "file open" << endl;
     f << fixed;
 
     // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
@@ -618,56 +547,34 @@ void System::SaveTrajectoryEuRoC(const string &filename)
     list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
     list<bool>::iterator lbL = mpTracker->mlbLost.begin();
 
-    //cout << "size mlpReferences: " << mpTracker->mlpReferences.size() << endl;
-    //cout << "size mlRelativeFramePoses: " << mpTracker->mlRelativeFramePoses.size() << endl;
-    //cout << "size mpTracker->mlFrameTimes: " << mpTracker->mlFrameTimes.size() << endl;
-    //cout << "size mpTracker->mlbLost: " << mpTracker->mlbLost.size() << endl;
-
-
     for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
         lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
     {
-        //cout << "1" << endl;
         if(*lbL)
             continue;
 
 
         KeyFrame* pKF = *lRit;
-        //cout << "KF: " << pKF->mnId << endl;
 
         cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
 
-        /*cout << "2" << endl;
-        cout << "KF id: " << pKF->mnId << endl;*/
-
         // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
         if (!pKF)
             continue;
 
-        //cout << "2.5" << endl;
-
         while(pKF->isBad())
         {
-            //cout << " 2.bad" << endl;
             Trw = Trw*pKF->mTcp;
             pKF = pKF->GetParent();
-            //cout << "--Parent KF: " << pKF->mnId << endl;
         }
 
         if(!pKF || pKF->GetMap() != pBiggerMap)
         {
-            //cout << "--Parent KF is from another map" << endl;
-            /*if(pKF)
-                cout << "--Parent KF " << pKF->mnId << " is from another map " << pKF->GetMap()->GetId() << endl;*/
             continue;
         }
 
-        //cout << "3" << endl;
-
         Trw = Trw*pKF->GetPose()*Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference
 
-        // cout << "4" << endl;
-
         if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO)
         {
             cv::Mat Tbw = pKF->mImuCalib.Tbc*(*lit)*Trw;
@@ -685,7 +592,6 @@ void System::SaveTrajectoryEuRoC(const string &filename)
             f << setprecision(6) << 1e9*(*lT) << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
         }
 
-        // cout << "5" << endl;
     }
     //cout << "end saving trajectory" << endl;
     f.close();
@@ -722,8 +628,6 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename)
     {
         KeyFrame* pKF = vpKFs[i];
 
-       // pKF->SetPose(pKF->GetPose()*Two);
-
         if(pKF->isBad())
             continue;
         if (mSensor == IMU_MONOCULAR || mSensor == IMU_STEREO)
@@ -798,63 +702,6 @@ void System::SaveTrajectoryKITTI(const string &filename)
     f.close();
 }
 
-
-void System::SaveDebugData(const int &initIdx)
-{
-    // 0. Save initialization trajectory
-    SaveTrajectoryEuRoC("init_FrameTrajectoy_" +to_string(mpLocalMapper->mInitSect)+ "_" + to_string(initIdx)+".txt");
-
-    // 1. Save scale
-    ofstream f;
-    f.open("init_Scale_" + to_string(mpLocalMapper->mInitSect) + ".txt", ios_base::app);
-    f << fixed;
-    f << mpLocalMapper->mScale << endl;
-    f.close();
-
-    // 2. Save gravity direction
-    f.open("init_GDir_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app);
-    f << fixed;
-    f << mpLocalMapper->mRwg(0,0) << "," << mpLocalMapper->mRwg(0,1) << "," << mpLocalMapper->mRwg(0,2) << endl;
-    f << mpLocalMapper->mRwg(1,0) << "," << mpLocalMapper->mRwg(1,1) << "," << mpLocalMapper->mRwg(1,2) << endl;
-    f << mpLocalMapper->mRwg(2,0) << "," << mpLocalMapper->mRwg(2,1) << "," << mpLocalMapper->mRwg(2,2) << endl;
-    f.close();
-
-    // 3. Save computational cost
-    f.open("init_CompCost_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app);
-    f << fixed;
-    f << mpLocalMapper->mCostTime << endl;
-    f.close();
-
-    // 4. Save biases
-    f.open("init_Biases_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app);
-    f << fixed;
-    f << mpLocalMapper->mbg(0) << "," << mpLocalMapper->mbg(1) << "," << mpLocalMapper->mbg(2) << endl;
-    f << mpLocalMapper->mba(0) << "," << mpLocalMapper->mba(1) << "," << mpLocalMapper->mba(2) << endl;
-    f.close();
-
-    // 5. Save covariance matrix
-    f.open("init_CovMatrix_" +to_string(mpLocalMapper->mInitSect)+ "_" +to_string(initIdx)+".txt", ios_base::app);
-    f << fixed;
-    for(int i=0; i<mpLocalMapper->mcovInertial.rows(); i++)
-    {
-        for(int j=0; j<mpLocalMapper->mcovInertial.cols(); j++)
-        {
-            if(j!=0)
-                f << ",";
-            f << setprecision(15) << mpLocalMapper->mcovInertial(i,j);
-        }
-        f << endl;
-    }
-    f.close();
-
-    // 6. Save initialization time
-    f.open("init_Time_" +to_string(mpLocalMapper->mInitSect)+ ".txt", ios_base::app);
-    f << fixed;
-    f << mpLocalMapper->mInitTime << endl;
-    f.close();
-}
-
-
 int System::GetTrackingState()
 {
     unique_lock<mutex> lock(mMutexState);
@@ -888,7 +735,7 @@ bool System::isLost()
         return false;
     else
     {
-        if ((mpTracker->mState==Tracking::LOST)) //||(mpTracker->mState==Tracking::RECENTLY_LOST))
+        if ((mpTracker->mState==Tracking::LOST))
             return true;
         else
             return false;
@@ -915,154 +762,18 @@ void System::ChangeDataset()
     mpTracker->NewDataset();
 }
 
-/*void System::SaveAtlas(int type){
-    cout << endl << "Enter the name of the file if you want to save the current Atlas session. To exit press ENTER: ";
-    string saveFileName;
-    getline(cin,saveFileName);
-    if(!saveFileName.empty())
-    {
-        //clock_t start = clock();
-
-        // Save the current session
-        mpAtlas->PreSave();
-        mpKeyFrameDatabase->PreSave();
-
-        string pathSaveFileName = "./";
-        pathSaveFileName = pathSaveFileName.append(saveFileName);
-        pathSaveFileName = pathSaveFileName.append(".osa");
-
-        string strVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE);
-        std::size_t found = mStrVocabularyFilePath.find_last_of("/\\");
-        string strVocabularyName = mStrVocabularyFilePath.substr(found+1);
-
-        if(type == TEXT_FILE) // File text
-        {
-            cout << "Starting to write the save text file " << endl;
-            std::remove(pathSaveFileName.c_str());
-            std::ofstream ofs(pathSaveFileName, std::ios::binary);
-            boost::archive::text_oarchive oa(ofs);
-
-            oa << strVocabularyName;
-            oa << strVocabularyChecksum;
-            oa << mpAtlas;
-            oa << mpKeyFrameDatabase;
-            cout << "End to write the save text file" << endl;
-        }
-        else if(type == BINARY_FILE) // File binary
-        {
-            cout << "Starting to write the save binary file" << endl;
-            std::remove(pathSaveFileName.c_str());
-            std::ofstream ofs(pathSaveFileName, std::ios::binary);
-            boost::archive::binary_oarchive oa(ofs);
-            oa << strVocabularyName;
-            oa << strVocabularyChecksum;
-            oa << mpAtlas;
-            oa << mpKeyFrameDatabase;
-            cout << "End to write save binary file" << endl;
-        }
-
-        //clock_t timeElapsed = clock() - start;
-        //unsigned msElapsed = timeElapsed / (CLOCKS_PER_SEC / 1000);
-        //cout << "Binary file saved in " << msElapsed << " ms" << endl;
-    }
-}
-
-bool System::LoadAtlas(string filename, int type)
+#ifdef REGISTER_TIMES
+void System::InsertRectTime(double& time)
 {
-    string strFileVoc, strVocChecksum;
-    bool isRead = false;
-
-    if(type == TEXT_FILE) // File text
-    {
-        cout << "Starting to read the save text file " << endl;
-        std::ifstream ifs(filename, std::ios::binary);
-        if(!ifs.good())
-        {
-            cout << "Load file not found" << endl;
-            return false;
-        }
-        boost::archive::text_iarchive ia(ifs);
-        ia >> strFileVoc;
-        ia >> strVocChecksum;
-        ia >> mpAtlas;
-        //ia >> mpKeyFrameDatabase;
-        cout << "End to load the save text file " << endl;
-        isRead = true;
-    }
-    else if(type == BINARY_FILE) // File binary
-    {
-        cout << "Starting to read the save binary file"  << endl;
-        std::ifstream ifs(filename, std::ios::binary);
-        if(!ifs.good())
-        {
-            cout << "Load file not found" << endl;
-            return false;
-        }
-        boost::archive::binary_iarchive ia(ifs);
-        ia >> strFileVoc;
-        ia >> strVocChecksum;
-        ia >> mpAtlas;
-        //ia >> mpKeyFrameDatabase;
-        cout << "End to load the save binary file" << endl;
-        isRead = true;
-    }
-
-    if(isRead)
-    {
-        //Check if the vocabulary is the same
-        string strInputVocabularyChecksum = CalculateCheckSum(mStrVocabularyFilePath,TEXT_FILE);
-
-        if(strInputVocabularyChecksum.compare(strVocChecksum) != 0)
-        {
-            cout << "The vocabulary load isn't the same which the load session was created " << endl;
-            cout << "-Vocabulary name: " << strFileVoc << endl;
-            return false; // Both are differents
-        }
-
-        return true;
-    }
-    return false;
+    mpTracker->vdRectStereo_ms.push_back(time);
 }
 
-string System::CalculateCheckSum(string filename, int type)
+void System::InsertTrackTime(double& time)
 {
-    string checksum = "";
-
-    unsigned char c[MD5_DIGEST_LENGTH];
-
-    std::ios_base::openmode flags = std::ios::in;
-    if(type == BINARY_FILE) // Binary file
-        flags = std::ios::in | std::ios::binary;
-
-    ifstream f(filename.c_str(), flags);
-    if ( !f.is_open() )
-    {
-        cout << "[E] Unable to open the in file " << filename << " for Md5 hash." << endl;
-        return checksum;
-    }
-
-    MD5_CTX md5Context;
-    char buffer[1024];
-
-    MD5_Init (&md5Context);
-    while ( int count = f.readsome(buffer, sizeof(buffer)))
-    {
-        MD5_Update(&md5Context, buffer, count);
-    }
-
-    f.close();
-
-    MD5_Final(c, &md5Context );
-
-    for(int i = 0; i < MD5_DIGEST_LENGTH; i++)
-    {
-        char aux[10];
-        sprintf(aux,"%02x", c[i]);
-        checksum = checksum + aux;
-    }
+    mpTracker->vdTrackTotal_ms.push_back(time);
+}
+#endif
 
-    return checksum;
-}*/
 
 } //namespace ORB_SLAM
 

src/Viewer.cc

@@ -145,15 +145,12 @@ void Viewer::Run()
     pangolin::Var<bool> menuFollowCamera("menu.Follow Camera",true,true);
     pangolin::Var<bool> menuCamView("menu.Camera View",false,false);
     pangolin::Var<bool> menuTopView("menu.Top View",false,false);
-    // pangolin::Var<bool> menuSideView("menu.Side View",false,false);
     pangolin::Var<bool> menuShowPoints("menu.Show Points",true,true);
     pangolin::Var<bool> menuShowKeyFrames("menu.Show KeyFrames",true,true);
     pangolin::Var<bool> menuShowGraph("menu.Show Graph",false,true);
     pangolin::Var<bool> menuShowInertialGraph("menu.Show Inertial Graph",true,true);
     pangolin::Var<bool> menuLocalizationMode("menu.Localization Mode",false,true);
     pangolin::Var<bool> menuReset("menu.Reset",false,false);
-    pangolin::Var<bool> menuStepByStep("menu.Step By Step",false,true);  // false, true
-    pangolin::Var<bool> menuStep("menu.Step",false,false);
 
     // Define Camera Render Object (for view / scene browsing)
     pangolin::OpenGlRenderState s_cam(
@@ -192,7 +189,6 @@ void Viewer::Run()
 
         if(mbStopTrack)
         {
-            menuStepByStep = true;
             mbStopTrack = false;
         }
 
@@ -237,21 +233,11 @@ void Viewer::Run()
         {
             menuTopView = false;
             bCameraView = false;
-            /*s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,1000));
-            s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,10, 0,0,0,0.0,0.0, 1.0));*/
             s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000));
             s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0,0.01,50, 0,0,0,0.0,0.0, 1.0));
             s_cam.Follow(Ow);
         }
 
-        /*if(menuSideView && mpMapDrawer->mpAtlas->isImuInitialized())
-        {
-            s_cam.SetProjectionMatrix(pangolin::ProjectionMatrix(1024,768,3000,3000,512,389,0.1,10000));
-            s_cam.SetModelViewMatrix(pangolin::ModelViewLookAt(0.0,0.1,30.0,0,0,0,0.0,0.0,1.0));
-            s_cam.Follow(Twwp);
-        }*/
-
-
         if(menuLocalizationMode && !bLocalizationMode)
         {
             mpSystem->ActivateLocalizationMode();
@@ -263,24 +249,6 @@ void Viewer::Run()
             bLocalizationMode = false;
         }
 
-        if(menuStepByStep && !bStepByStep)
-        {
-            mpTracker->SetStepByStep(true);
-            bStepByStep = true;
-        }
-        else if(!menuStepByStep && bStepByStep)
-        {
-            mpTracker->SetStepByStep(false);
-            bStepByStep = false;
-        }
-
-        if(menuStep)
-        {
-            mpTracker->mbStep = true;
-            menuStep = false;
-        }
-
-
         d_cam.Activate(s_cam);
         glClearColor(1.0f,1.0f,1.0f,1.0f);
         mpMapDrawer->DrawCurrentCamera(Twc);
@@ -317,7 +285,6 @@ void Viewer::Run()
             bLocalizationMode = false;
             bFollow = true;
             menuFollowCamera = true;
-            //mpSystem->Reset();
             mpSystem->ResetActiveMap();
             menuReset = false;
         }

tum_vi_eval_examples.sh

@@ -1,11 +0,0 @@
-#!/bin/bash
-pathDatasetTUM_VI='/Datasets/TUM_VI' #Example, it is necesary to change it by the dataset path
-
-
-# Single Session Example
-
-echo "Launching Magistrale 1 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt dataset-magistrale1_512_stereoi
-echo "------------------------------------"
-echo "Evaluation of Magistrale 1 trajectory with Stereo-Inertial sensor"
-python evaluation/evaluate_ate_scale.py "$pathDatasetTUM_VI"/magistrale1_512_16/mav0/mocap0/data.csv f_dataset-magistrale1_512_stereoi.txt --plot magistrale1_512_stereoi.pdf

tum_vi_examples.sh

@@ -1,240 +0,0 @@
-#!/bin/bash
-pathDatasetTUM_VI='/Datasets/TUM_VI' #Example, it is necesary to change it by the dataset path
-
-#------------------------------------
-# Monocular Examples
-echo "Launching Room 1 with Monocular sensor"
-./Examples/Monocular/mono_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Examples/Monocular/TUM_TimeStamps/dataset-room1_512.txt dataset-room1_512_mono
-
-echo "Launching Room 2 with Monocular sensor"
-./Examples/Monocular/mono_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data Examples/Monocular/TUM_TimeStamps/dataset-room2_512.txt dataset-room2_512_mono
-
-echo "Launching Room 3 with Monocular sensor"
-./Examples/Monocular/mono_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data Examples/Monocular/TUM_TimeStamps/dataset-room3_512.txt dataset-room3_512_mono
-
-echo "Launching Room 4 with Monocular sensor"
-./Examples/Monocular/mono_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data Examples/Monocular/TUM_TimeStamps/dataset-room4_512.txt dataset-room4_512_mono
-
-echo "Launching Room 5 with Monocular sensor"
-./Examples/Monocular/mono_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data Examples/Monocular/TUM_TimeStamps/dataset-room5_512.txt dataset-room5_512_mono
-
-echo "Launching Room 6 with Monocular sensor"
-./Examples/Monocular/mono_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data Examples/Monocular/TUM_TimeStamps/dataset-room6_512.txt dataset-room6_512_mono
-
-
-#------------------------------------
-# Stereo Examples
-echo "Launching Room 1 with Stereo sensor"
-./Examples/Stereo/stereo_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Examples/Stereo/TUM_TimeStamps/dataset-room1_512.txt dataset-room1_512_stereo
-
-echo "Launching Room 2 with Stereo sensor"
-./Examples/Stereo/stereo_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam1/data Examples/Stereo/TUM_TimeStamps/dataset-room2_512.txt dataset-room2_512_stereo
-
-echo "Launching Room 3 with Stereo sensor"
-./Examples/Stereo/stereo_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam1/data Examples/Stereo/TUM_TimeStamps/dataset-room3_512.txt dataset-room3_512_stereo
-
-echo "Launching Room 4 with Stereo sensor"
-./Examples/Stereo/stereo_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam1/data Examples/Stereo/TUM_TimeStamps/dataset-room4_512.txt dataset-room4_512_stereo
-
-echo "Launching Room 5 with Stereo sensor"
-./Examples/Stereo/stereo_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam1/data Examples/Stereo/TUM_TimeStamps/dataset-room5_512.txt dataset-room5_512_stereo
-
-echo "Launching Room 6 with Stereo sensor"
-./Examples/Stereo/stereo_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam1/data Examples/Stereo/TUM_TimeStamps/dataset-room6_512.txt dataset-room6_512_stereo
-
-
-#------------------------------------
-# Monocular-Inertial Examples
-echo "Launching Corridor 1 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor1_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-corridor1_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-corridor1_512.txt dataset-corridor1_512_monoi
-
-echo "Launching Corridor 2 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor2_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-corridor2_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-corridor2_512.txt dataset-corridor2_512_monoi
-
-echo "Launching Corridor 3 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor3_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-corridor3_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-corridor3_512.txt dataset-corridor3_512_monoi
-
-echo "Launching Corridor 4 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor4_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-corridor4_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-corridor4_512.txt dataset-corridor4_512_monoi
-
-echo "Launching Corridor 5 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor5_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-corridor5_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-corridor5_512.txt dataset-corridor5_512_monoi
-
-
-echo "Launching Magistrale 1 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-magistrale1_512.txt dataset-magistrale1_512_monoi
-
-echo "Launching Magistrale 2 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale2_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-magistrale2_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-magistrale2_512.txt dataset-magistrale2_512
-
-echo "Launching Magistrale 3 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale3_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-magistrale3_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-magistrale3_512.txt dataset-magistrale3_512_monoi
-
-echo "Launching Magistrale 4 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale4_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-magistrale4_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-magistrale4_512.txt dataset-magistrale4_512_monoi
-
-echo "Launching Magistrale 5 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-magistrale5_512.txt dataset-magistrale5_512_monoi
-
-echo "Launching Magistrale 6 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale6_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-magistrale6_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-magistrale6_512.txt dataset-magistrale6_512_monoi
-
-
-echo "Launching Outdoor 1 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors1_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-outdoors1_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-outdoors1_512.txt dataset-outdoors1_512_monoi
-
-echo "Launching Outdoor 2 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors2_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-outdoors2_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-outdoors2_512.txt dataset-outdoors2_512_monoi
-
-echo "Launching Outdoor 3 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors3_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-outdoors3_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-outdoors3_512.txt dataset-outdoors3_512_monoi
-
-echo "Launching Outdoor 4 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-outdoors4_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-outdoors4_512.txt dataset-outdoors4_512_monoi
-
-echo "Launching Outdoor 5 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors5_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-outdoors5_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-outdoors5_512.txt dataset-outdoors5_512_monoi
-
-echo "Launching Outdoor 6 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors6_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-outdoors6_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-outdoors6_512.txt dataset-outdoors6_512_monoi
-
-echo "Launching Outdoor 7 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors7_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-outdoors7_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-outdoors7_512.txt dataset-outdoors7_512_monoi
-
-echo "Launching Outdoor 8 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512_far.yaml "$pathDatasetTUM_VI"/dataset-outdoors8_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-outdoors8_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-outdoors8_512.txt dataset-outdoors8_512_monoi
-
-
-echo "Launching Room 1 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-room1_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-room1_512.txt dataset-room1_512_monoi
-
-echo "Launching Room 2 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-room2_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-room2_512.txt dataset-room2_512_monoi
-
-echo "Launching Room 3 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-room3_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-room3_512.txt dataset-room3_512_monoi
-
-echo "Launching Room 4 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-room4_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-room4_512.txt dataset-room4_512_monoi
-
-echo "Launching Room 5 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-room5_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-room5_512.txt dataset-room5_512_monoi
-
-echo "Launching Room 6 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-room6_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-room6_512.txt dataset-room6_512_monoi
-
-
-echo "Launching Slides 1 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-slides1_512_monoi
-
-echo "Launching Slides 2 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides2_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-slides2_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-slides2_512.txt dataset-slides2_512_monoi
-
-echo "Launching Slides 3 with Monocular-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides3_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-slides3_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-slides3_512.txt dataset-slides3_512_monoi
-
-
-# MultiSession Monocular Examples
-echo "Launching Room 1, Magistrale 1, Magistrale 5 and Slides 1 in the same session with Stereo-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale5_512.txt "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-room1_mag1_mag5_slides1_monoi
-
-echo "Launching all Rooms (1-6) in the same session with Stereo-Inertial sensor"
-./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room2_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room2_512.txt "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room3_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room3_512.txt "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room4_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room4_512.txt "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room5_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room5_512.txt "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room6_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room6_512.txt dataset-rooms123456_monoi
-
-#------------------------------------
-# Stereo-Inertial Examples
-echo "Launching Corridor 1 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-corridor1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-corridor1_512.txt dataset-corridor1_512_stereoi
-
-echo "Launching Corridor 2 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor2_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-corridor2_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-corridor2_512.txt dataset-corridor2_512_stereoi
-
-echo "Launching Corridor 3 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor3_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-corridor3_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-corridor3_512.txt dataset-corridor3_512_stereoi
-
-echo "Launching Corridor 4 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor4_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-corridor4_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-corridor4_512.txt dataset-corridor4_512_stereoi
-
-echo "Launching Corridor 5 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-corridor5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-corridor5_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-corridor5_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-corridor5_512.txt dataset-corridor5_512_stereoi
-
-
-echo "Launching Magistrale 1 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt dataset-magistrale1_512_stereoi
-
-echo "Launching Magistrale 2 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale2_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale2_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale2_512.txt dataset-magistrale2_512_stereoi
-
-echo "Launching Magistrale 3 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale3_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale3_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale3_512.txt dataset-magistrale3_512_stereoi
-
-echo "Launching Magistrale 4 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale4_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale4_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale4_512.txt dataset-magistrale4_512_stereoi
-
-echo "Launching Magistrale 5 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale5_512.txt dataset-magistrale5_512_stereoi
-
-echo "Launching Magistrale 6 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-magistrale6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale6_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale6_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale6_512.txt dataset-magistrale6_512_stereoi
-
-
-echo "Launching Outdoor 1 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-outdoors1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-outdoors1_512.txt outdoors1_512_stereoi
-
-echo "Launching Outdoor 2 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors2_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-outdoors2_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-outdoors2_512.txt outdoors2_512_stereoi
-
-echo "Launching Outdoor 3 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors3_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-outdoors3_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-outdoors3_512.txt outdoors3_512
-
-echo "Launching Outdoor 4 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors4_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-outdoors4_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-outdoors4_512.txt outdoors4_512
-
-echo "Launching Outdoor 5 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors5_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-outdoors5_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-outdoors5_512.txt outdoors5_512_stereoi
-
-echo "Launching Outdoor 6 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors6_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-outdoors6_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-outdoors6_512.txt outdoors6_512_stereoi
-
-echo "Launching Outdoor 7 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors7_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors7_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-outdoors7_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-outdoors7_512.txt outdoors7_512_stereoi
-
-echo "Launching Outdoor 8 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512_outdoors.yaml "$pathDatasetTUM_VI"/dataset-outdoors8_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-outdoors8_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-outdoors8_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-outdoors8_512.txt outdoors8_512_stereoi
-
-
-echo "Launching Room 1 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room1_512.txt dataset-room1_512_stereoi
-
-echo "Launching Room 2 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room2_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room2_512.txt dataset-room2_512_stereoi
-
-echo "Launching Room 3 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room3_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room3_512.txt dataset-room3_512_stereoi
-
-echo "Launching Room 4 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room4_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room4_512.txt dataset-room4_512_stereoi
-
-echo "Launching Room 5 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room5_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room5_512.txt dataset-room5_512_stereoi
-
-echo "Launching Room 6 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room6_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room6_512.txt dataset-room6_512_stereoi
-
-
-echo "Launching Slides 1 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-slides1_512_stereoi
-
-echo "Launching Slides 2 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides2_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-slides2_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-slides2_512.txt dataset-slides2_512_stereoi
-
-echo "Launching Slides 3 with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-slides3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides3_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-slides3_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-slides3_512.txt dataset-slides3_512_stereoi
-
-
-# MultiSession Stereo-Inertial Examples
-echo "Launching Room 1, Magistrale 1, Magistrale 5 and Slides 1 in the same session with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale1_512.txt "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-magistrale5_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-magistrale5_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-magistrale5_512.txt "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-slides1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-slides1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-slides1_512.txt dataset-room1_mag1_mag5_slides1_stereoi
-
-echo "Launching all Rooms (1-6) in the same session with Stereo-Inertial sensor"
-./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM_512.yaml "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room1_512.txt "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room2_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room2_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room2_512.txt "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room3_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room3_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room3_512.txt "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room4_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room4_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room4_512.txt "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room5_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room5_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room5_512.txt "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam0/data "$pathDatasetTUM_VI"/dataset-room6_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-room6_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-room6_512.txt dataset-rooms123456_stereoi