Update 'README.md'

allow merge

.gitignore

@@ -66,8 +66,6 @@ Thirdparty/Sophus/build/
 Thirdparty/g2o/build/
 Thirdparty/g2o/config.h
 Thirdparty/g2o/lib/
-Vocabulary/ORBvoc.txt
-Vocabulary/ORBvoc.txt.tar.gz
 build/
 
 lib/
@@ -85,8 +83,6 @@ LBA_Stats.txt
 LocalMapTimeStats.txt
 TrackLocalMapStats.txt
 TrackingTimeStats.txt
-time*
-Time*
 Table_Euroc
 Table_TUMVI_rooms/
 *.pyc
@@ -99,3 +95,6 @@ my_settings.txt
 borrar/*
 
 */ExecMean.txt
+
+# Unignore selected files:
+!timeutil.*

Examples/Stereo/ComplexUrban.yaml

@@ -3,73 +3,54 @@
 #--------------------------------------------------------------------------------------------
 # Camera Parameters. Adjust them!
 #--------------------------------------------------------------------------------------------
-#File.version: "1.0"
+File.version: "1.0"
 
 Camera.type: "PinHole"
 
-# Camera calibration and distortion parameters (OpenCV) 
 Camera1.fx: 816.9037
 Camera1.fy: 811.5680
 Camera1.cx: 608.5072
 Camera1.cy: 263.4759
 
-Camera.fx: 816.9037
-Camera.fy: 811.5680
-Camera.cx: 608.5072
-Camera.cy: 263.4759
+Camera1.k1: -0.056143027782782
+Camera1.k2: 0.139525632030074
+Camera1.k3: -0.080878168794740
+Camera1.p1: -0.001215590686538
+Camera1.p2: -9.728139470848294e-04
 
-# Equidistant distortion 0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182
-#Camera.bFishEye: 1
-Camera.k1: -0.056143027782782
-Camera.k2: 0.139525632030074
-#Camera.k3: -0.080878168794740
-Camera.p1: -0.001215590686538
-Camera.p2: -9.728139470848294e-04
-
-# Second camera
-#Camera2.type: "PinHole"
 Camera2.fx: 8.137820553958778e+02
 Camera2.fy: 8.085216557426838e+02
 Camera2.cx: 6.138641953932292e+02
 Camera2.cy: 2.494104934865211e+02
 
-# Equidistant distortion 0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202, 0.00020293673591811182
-#Camera.bFishEye: 1
 Camera2.k1: -0.054921981757479
 Camera2.k2: 0.142436574286058
-#Camera2.k3: -0.085665408330633
+Camera2.k3: -0.085665408330633
 Camera2.p1: 7.541230501084140e-05
-Camera2.p1: -6.756052742429136e-04
+Camera2.p2: -6.756052742429136e-04
 
-Tlr: !!opencv-matrix
-  rows: 3
-  cols: 4
-  dt: f
-  data: [ 0.999991433056271,   0.002082719758761,   0.003577162628705,   -4.751436000507748e-1,
-            -0.002066281899364,   0.999987318309473,   -0.004592787752592,   -1.144594387276745e-3,
-            -0.003586682754036,  0.004585356980047,   0.999983054960527,   2.012928530231533e-3]
-    
-# Camera resolution
 Camera.width: 1280
 Camera.height: 560
 
-# Lapping area between images
-Camera.lappingBegin: 0
-Camera.lappingEnd: 1279
-
-Camera2.lappingBegin: 0
-Camera2.lappingEnd: 1279
-
 # Camera frames per second 
-Camera.fps: 20
+Camera.fps: 10
 
 # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
 Camera.RGB: 1
 
 # Close/Far threshold. Baseline times.
-ThDepth: 40.0
+# Is taken from the stereoParam calibration file. 
+Stereo.ThDepth: 60.0
+Stereo.T_c1_c2: !!opencv-matrix
+  rows: 4
+  cols: 4
+  dt: f
+  data: [ 0.999991433056271,   0.002082719758761,   0.003577162628705,   -4.751436000507748e-1,
+            -0.002066281899364,   0.999987318309473,   -0.004592787752592,   -1.144594387276745e-3,
+            -0.003586682754036,  0.004585356980047,   0.999983054960527,   2.012928530231533e-3,
+            0, 0, 0, 1.000000000000000]
 
-Camera.bf: 388.151174248813
+#Camera.bf: 388.151174248813
 
 #--------------------------------------------------------------------------------------------
 # ORB Parameters

Examples/Stereo/run_stereo_complex_urban.sh

@@ -1 +1,2 @@
+# arguments: path_to_vocabulary path_to_config path_to_stereo_left path_to_stereo_right path_to_timestamps
 ./stereo_tum_vi /home/pi/work_drivecast/slams/ORB_SLAM3-1.0-release/Vocabulary/ORBvoc.txt /home/pi/work_drivecast/slams/ORB_SLAM3-1.0-release/Examples/Stereo/ComplexUrban.yaml /home/pi/work_drivecast/datasets/complex_urban_dataset/image/stereo_left /home/pi/work_drivecast/datasets/complex_urban_dataset/image/stereo_right /home/pi/work_drivecast/datasets/complex_urban_dataset/stereo_stamp.csv

Examples/Stereo/run_stereo_uzh-fpv.sh

@@ -1,4 +1,5 @@
 # Usage: ./stereo_uzh-fpv path_to_vocabulary path_to_settings path_to_image_folder_1 path_to_times_file_left_1 path_to_times_file_right_1
+
 #./stereo_uzh-fpv /home/pi/work_drivecast/slams/ORB_SLAM3-1.0-release/Vocabulary/ORBvoc.txt /home/pi/work_drivecast/slams/ORB_SLAM3-1.0-release/Examples/Stereo/uzh-fpv.yaml ~/work_drivecast/datasets/uzh-fpv/indoor_forward_6_snapdragon_with_gt /home/pi/work_drivecast/datasets/uzh-fpv/indoor_forward_6_snapdragon_with_gt/left_images.txt /home/pi/work_drivecast/datasets/uzh-fpv/indoor_forward_6_snapdragon_with_gt/right_images.txt
 
 ./stereo_uzh-fpv /home/pi/work_drivecast/slams/ORB_SLAM3-1.0-release/Vocabulary/ORBvoc.txt /home/pi/work_drivecast/slams/ORB_SLAM3-1.0-release/Examples/Stereo/uzh-fpv.yaml ~/work_drivecast/datasets/uzh-fpv/indoor_forward_3_snapdragon_with_gt /home/pi/work_drivecast/datasets/uzh-fpv/indoor_forward_3_snapdragon_with_gt/left_images.txt /home/pi/work_drivecast/datasets/uzh-fpv/indoor_forward_3_snapdragon_with_gt/right_images.txt

README.md

@@ -1,221 +1,50 @@
-<<<<<<< HEAD
-# ORB-SLAM3
-The ORB-SLAM3 system with changes made for specific tasks at DriveCast.
+# ORB-SLAM3 for Linux
+Redmine issue: https://redmine.drivecast.tech/issues/335
 
-### V1.0, December 22th, 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/).
+## Installation
+Please refer to the original repository on the details of the installation procedure: https://github.com/UZ-SLAMLab/ORB_SLAM3
 
-The [Changelog](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Changelog.md) describes the features of each version.
+You can use the script to install the Thirdparty libraries and ORB-SLAM3 core and the examples. But make sure you have installed all the required packages (You may need to change the paths to the packages in your CMakeLists.txt):  
 
-ORB-SLAM3 is the first real-time SLAM library able to perform **Visual, Visual-Inertial and Multi-Map SLAM** with **monocular, stereo and RGB-D** cameras, using **pin-hole and fisheye** lens models. In all sensor configurations, ORB-SLAM3 is as robust as the best systems available in the literature, and significantly more accurate.
-
-We provide examples to run ORB-SLAM3 in the [EuRoC dataset](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) using stereo or monocular, with or without IMU, and in the [TUM-VI dataset](https://vision.in.tum.de/data/datasets/visual-inertial-dataset) using fisheye stereo or monocular, with or without IMU. Videos of some example executions can be found at [ORB-SLAM3 channel](https://www.youtube.com/channel/UCXVt-kXG6T95Z4tVaYlU80Q).
-
-This software is based on [ORB-SLAM2](https://github.com/raulmur/ORB_SLAM2) developed by [Raul Mur-Artal](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2)).
-
-<a href="https://youtu.be/HyLNq-98LRo" target="_blank"><img src="https://img.youtube.com/vi/HyLNq-98LRo/0.jpg"
-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**, *IEEE Transactions on Robotics 37(6):1874-1890, Dec. 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)**
-
-[ORBSLAM-Atlas] Richard Elvira, J. M. M. Montiel and Juan D. Tardós, **ORBSLAM-Atlas: a robust and accurate multi-map system**, *IROS 2019*. **[PDF](https://arxiv.org/pdf/1908.11585.pdf)**.
-
-[ORBSLAM-VI] Raúl Mur-Artal, and Juan D. Tardós, **Visual-inertial monocular SLAM with map reuse**, IEEE Robotics and Automation Letters, vol. 2 no. 2, pp. 796-803, 2017. **[PDF](https://arxiv.org/pdf/1610.05949.pdf)**.
-
-[Stereo and RGB-D] Raúl Mur-Artal and Juan D. Tardós. **ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras**. *IEEE Transactions on Robotics,* vol. 33, no. 5, pp. 1255-1262, 2017. **[PDF](https://arxiv.org/pdf/1610.06475.pdf)**.
-
-[Monocular] Raúl Mur-Artal, José M. M. Montiel and Juan D. Tardós. **ORB-SLAM: A Versatile and Accurate Monocular SLAM System**. *IEEE Transactions on Robotics,* vol. 31, no. 5, pp. 1147-1163, 2015. (**2015 IEEE Transactions on Robotics Best Paper Award**). **[PDF](https://arxiv.org/pdf/1502.00956.pdf)**.
-
-[DBoW2 Place Recognition] Dorian Gálvez-López and Juan D. Tardós. **Bags of Binary Words for Fast Place Recognition in Image Sequences**. *IEEE Transactions on Robotics,* vol. 28, no. 5, pp. 1188-1197, 2012. **[PDF](http://doriangalvez.com/php/dl.php?dlp=GalvezTRO12.pdf)**
-
-# 1. License
-
-ORB-SLAM3 is released under [GPLv3 license](https://github.com/UZ-SLAMLab/ORB_SLAM3/LICENSE). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/UZ-SLAMLab/ORB_SLAM3/blob/master/Dependencies.md).
-
-For a closed-source version of ORB-SLAM3 for commercial purposes, please contact the authors: orbslam (at) unizar (dot) es.
-
-If you use ORB-SLAM3 in an academic work, please cite:
-
-    @article{ORBSLAM3_2020,
-      title={{ORB-SLAM3}: An Accurate Open-Source Library for Visual, Visual-Inertial
-               and Multi-Map {SLAM}},
-      author={Campos, Carlos AND Elvira, Richard AND G\´omez, Juan J. AND Montiel,
-              Jos\'e M. M. AND Tard\'os, Juan D.},
-      journal={IEEE Transactions on Robotics},
-      volume={37},
-      number={6},
-      pages={1874-1890},
-      year={2021}
-     }
-
-# 2. Prerequisites
-We have tested the library in **Ubuntu 16.04** and **18.04**, but it should be easy to compile in other platforms. A powerful computer (e.g. i7) will ensure real-time performance and provide more stable and accurate results.
-
-## C++11 or C++0x Compiler
-We use the new thread and chrono functionalities of C++11.
-
-## Pangolin
-We use [Pangolin](https://github.com/stevenlovegrove/Pangolin) for visualization and user interface. Dowload and install instructions can be found at: https://github.com/stevenlovegrove/Pangolin.
-
-## OpenCV
-We use [OpenCV](http://opencv.org) to manipulate images and features. Dowload and install instructions can be found at: http://opencv.org. **Required at leat 3.0. Tested with OpenCV 3.2.0 and 4.4.0**.
-
-## Eigen3
-Required by g2o (see below). Download and install instructions can be found at: http://eigen.tuxfamily.org. **Required at least 3.1.0**.
-
-## DBoW2 and g2o (Included in Thirdparty folder)
-We use modified versions of the [DBoW2](https://github.com/dorian3d/DBoW2) library to perform place recognition and [g2o](https://github.com/RainerKuemmerle/g2o) library to perform non-linear optimizations. Both modified libraries (which are BSD) are included in the *Thirdparty* folder.
-
-## Python
-Required to calculate the alignment of the trajectory with the ground truth. **Required Numpy module**.
-
-* (win) http://www.python.org/downloads/windows
-* (deb) `sudo apt install libpython2.7-dev`
-* (mac) preinstalled with osx
-
-## ROS (optional)
-
-We provide some examples to process input of a monocular, monocular-inertial, stereo, stereo-inertial or RGB-D camera using ROS. Building these examples is optional. These have been tested with ROS Melodic under Ubuntu 18.04.
-
-# 3. Building ORB-SLAM3 library and examples
-
-Clone the repository:
 ```
-git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3
-```
-
-We provide a script `build.sh` to build the *Thirdparty* libraries and *ORB-SLAM3*. Please make sure you have installed all required dependencies (see section 2). Execute:
-```
-cd ORB_SLAM3
+cd ORB-SLA3_Linux  
 chmod +x  build.sh  
 ./build.sh
 ```
 
-This will create **libORB_SLAM3.so**  at *lib* folder and the executables in *Examples* folder.
+## Running
 
-# 4. EuRoC Examples
-[EuRoC dataset](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) was recorded with two pinhole cameras and an inertial sensor. We provide an example script to launch EuRoC sequences in all the sensor configurations.
-
-1. Download a sequence (ASL format) from http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
-
-2. Open the script "euroc_examples.sh" in the root of the project. Change **pathDatasetEuroc** variable to point to the directory where the dataset has been uncompressed.
-
-3. Execute the following script to process all the sequences with all sensor configurations:
+### Mono-Kitti Examples 
+1. Скомпилируйте файл папке **Examples/Stereo/stereo-kitti.cc** и запустите бинарник с параметрами: путь к словарю, путь к файлу с настройками, путь к папке с изображениями. Пример: 
 ```
-./euroc_examples
+stereo-kitti /home/ivan/ivan/git/work_drivecast2/SLAM/ORB_SLAM3-1.0-release/Vocabulary/ORBvoc.txt /home/ivan/ivan/git/work_drivecast2/SLAM/ORB_SLAM3-1.0-release/Examples/Stereo/KITTI04-12.yaml /home/ivan/ivan/git/work_drivecast2/SLAM/datasets/mono-kitti/sequences/06
+```
+2. Бинарник создаст файлы `CameraTrajectory.txt` и `RealtimeCameraTrajectory.txt`, они практически идентичные. Первый файл выдаёт чуть более точную траекторию, но она недоступна в режиме "онлайн", когда позиция камеры в моменте. 
+3. Используйте данные файлы далее в репозитории [ue_demo](https://git.drivecast.tech/pi/ue_demo)
+
+
+### Other Examples
+Besides all the examples listed in ORB-SLAM3 original repository you can also run uzh-fpv and kaist urban examples:  
+
+- Examples/Stereo/run_stereo_uzh-fpv.sh
+- Examples/Stereo/run_stereo_complex_urban.sh
+- Examples/Stereo/run_stereo_uzh-fpv_multiple.sh
+- Examples/Stereo-Inertial/run_stereo_inertial_uzh-fpv.sh
+- Examples/Stereo-Inertial/run_stereo_inertial_zurich_urban_mav.sh
+
+For stereo examples change the paths to your dataset files in the scripts (Examples/Stereo/run_stereo_uzh_fpv.sh). 
+
+### Datasets 
+- Download uzh-fpv dataset from here: https://fpv.ifi.uzh.ch/  
+- Download complex-urban dataset from here: https://sites.google.com/view/complex-urban-dataset/system  
+
+NOTE: The download of the complex-urban dataset may take a while because you need to send an email to them to ask the permission. You can use the downloaded sequence from `/mnt/disk-small/podmivan/datasets/complex-urban` folder in the DriveCast server. 
+
+After all the datasets are downloaded and the paths to them are changed in the scrips you can run them. From the ORB-SLAM3_Linux directory run the example:
+
+```
+cd Examples/Stereo/
+./run_stereo_uzh_fpv.sh
 ```
 
-## Evaluation
-EuRoC provides ground truth for each sequence in the IMU body reference. As pure visual executions report trajectories centered in the left camera, we provide in the "evaluation" folder the transformation of the ground truth to the left camera reference. Visual-inertial trajectories use the ground truth from the dataset.
-
-Execute the following script to process sequences and compute the RMS ATE:
-```
-./euroc_eval_examples
-```
-
-# 5. TUM-VI Examples
-[TUM-VI dataset](https://vision.in.tum.de/data/datasets/visual-inertial-dataset) was recorded with two fisheye cameras and an inertial sensor.
-
-1. Download a sequence from https://vision.in.tum.de/data/datasets/visual-inertial-dataset and uncompress it.
-
-2. Open the script "tum_vi_examples.sh" in the root of the project. Change **pathDatasetTUM_VI** variable to point to the directory where the dataset has been uncompressed.
-
-3. Execute the following script to process all the sequences with all sensor configurations:
-```
-./tum_vi_examples
-```
-
-## Evaluation
-In TUM-VI ground truth is only available in the room where all sequences start and end. As a result the error measures the drift at the end of the sequence.
-
-Execute the following script to process sequences and compute the RMS ATE:
-```
-./tum_vi_eval_examples
-```
-
-# 6. ROS Examples
-
-### Building the nodes for mono, mono-inertial, stereo, stereo-inertial and RGB-D
-Tested with ROS Melodic and ubuntu 18.04.
-
-1. Add the path including *Examples/ROS/ORB_SLAM3* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file:
-  ```
-  gedit ~/.bashrc
-  ```
-and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM3:
-
-  ```
-  export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS
-  ```
-
-2. Execute `build_ros.sh` script:
-
-  ```
-  chmod +x build_ros.sh
-  ./build_ros.sh
-  ```
-
-### Running Monocular Node
-For a monocular input from topic `/camera/image_raw` run node ORB_SLAM3/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above.
-
-  ```
-  rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
-  ```
-
-### Running Monocular-Inertial Node
-For a monocular input from topic `/camera/image_raw` and an inertial input from topic `/imu`, run node ORB_SLAM3/Mono_Inertial. Setting the optional third argument to true will apply CLAHE equalization to images (Mainly for TUM-VI dataset).
-
-  ```
-  rosrun ORB_SLAM3 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE [EQUALIZATION]
-  ```
-
-### Running Stereo Node
-For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node ORB_SLAM3/Stereo. You will need to provide the vocabulary file and a settings file. For Pinhole camera model, if you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**. For FishEye camera model, rectification is not required since system works with original images:
-
-  ```
-  rosrun ORB_SLAM3 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION
-  ```
-
-### Running Stereo-Inertial Node
-For a stereo input from topics `/camera/left/image_raw` and `/camera/right/image_raw`, and an inertial input from topic `/imu`, run node ORB_SLAM3/Stereo_Inertial. You will need to provide the vocabulary file and a settings file, including rectification matrices if required in a similar way to Stereo case:
-
-  ```
-  rosrun ORB_SLAM3 Stereo_Inertial PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION [EQUALIZATION]
-  ```
-
-### Running RGB_D Node
-For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node ORB_SLAM3/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above.
-
-  ```
-  rosrun ORB_SLAM3 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
-  ```
-
-**Running ROS example:** Download a rosbag (e.g. V1_02_medium.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab for a Stereo-Inertial configuration:
-  ```
-  roscore
-  ```
-
-  ```
-  rosrun ORB_SLAM3 Stereo_Inertial Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml true
-  ```
-
-  ```
-  rosbag play --pause V1_02_medium.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw /imu0:=/imu
-  ```
-
-Once ORB-SLAM3 has loaded the vocabulary, press space in the rosbag tab.
-
-**Remark:** For rosbags from TUM-VI dataset, some play issue may appear due to chunk size. One possible solution is to rebag them with the default chunk size, for example:
-  ```
-  rosrun rosbag fastrebag.py dataset-room1_512_16.bag dataset-room1_512_16_small_chunks.bag
-  ```
-
-# 7. Running 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`).
-
-# 8. Calibration
-You can find a tutorial for visual-inertial calibration and a detailed description of the contents of valid configuration files at  `Calibration_Tutorial.pdf`
+The other examples you can run accordingly to the details provided in the [ORB-SLAM3](https://github.com/UZ-SLAMLab/ORB_SLAM3) repository.

Thirdparty/DBoW2/DUtils/Timestamp.cpp

@@ -0,0 +1,246 @@
+/*
+ * File: Timestamp.cpp
+ * Author: Dorian Galvez-Lopez
+ * Date: March 2009
+ * Description: timestamping functions
+ * 
+ * Note: in windows, this class has a 1ms resolution
+ *
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <cstdio>
+#include <cstdlib>
+#include <ctime>
+#include <cmath>
+#include <sstream>
+#include <iomanip>
+
+#ifdef _WIN32
+#ifndef WIN32
+#define WIN32
+#endif
+#endif
+
+#ifdef WIN32
+#include <sys/timeb.h>
+#define sprintf sprintf_s
+#else
+#include <sys/time.h>
+#endif
+
+#include "Timestamp.h"
+
+using namespace std;
+
+using namespace DUtils;
+
+Timestamp::Timestamp(Timestamp::tOptions option)
+{
+  if(option & CURRENT_TIME)
+    setToCurrentTime();
+  else if(option & ZERO)
+    setTime(0.);
+}
+
+Timestamp::~Timestamp(void)
+{
+}
+
+bool Timestamp::empty() const
+{
+  return m_secs == 0 && m_usecs == 0;
+}
+
+void Timestamp::setToCurrentTime(){
+	
+#ifdef WIN32
+	struct __timeb32 timebuffer;
+	_ftime32_s( &timebuffer ); // C4996
+	// Note: _ftime is deprecated; consider using _ftime_s instead
+	m_secs = timebuffer.time;
+	m_usecs = timebuffer.millitm * 1000;
+#else
+	struct timeval now;
+	gettimeofday(&now, NULL);
+	m_secs = now.tv_sec;
+	m_usecs = now.tv_usec;
+#endif
+
+}
+
+void Timestamp::setTime(const string &stime){
+	string::size_type p = stime.find('.');
+	if(p == string::npos){
+		m_secs = atol(stime.c_str());
+		m_usecs = 0;
+	}else{
+		m_secs = atol(stime.substr(0, p).c_str());
+		
+		string s_usecs = stime.substr(p+1, 6);
+		m_usecs = atol(stime.substr(p+1).c_str());
+		m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length()));
+	}
+}
+
+void Timestamp::setTime(double s)
+{
+  m_secs = (unsigned long)s;
+  m_usecs = (s - (double)m_secs) * 1e6;
+}
+
+double Timestamp::getFloatTime() const {
+	return double(m_secs) + double(m_usecs)/1000000.0;
+}
+
+string Timestamp::getStringTime() const {
+	char buf[32];
+	sprintf(buf, "%.6lf", this->getFloatTime());
+	return string(buf);
+}
+
+double Timestamp::operator- (const Timestamp &t) const {
+	return this->getFloatTime() - t.getFloatTime();
+}
+
+Timestamp& Timestamp::operator+= (double s)
+{
+  *this = *this + s;
+  return *this;
+}
+
+Timestamp& Timestamp::operator-= (double s)
+{
+  *this = *this - s;
+  return *this;
+}
+
+Timestamp Timestamp::operator+ (double s) const
+{
+	unsigned long secs = (long)floor(s);
+	unsigned long usecs = (long)((s - (double)secs) * 1e6);
+
+  return this->plus(secs, usecs);
+}
+
+Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const
+{
+  Timestamp t;
+
+	const unsigned long max = 1000000ul;
+
+	if(m_usecs + usecs >= max)
+		t.setTime(m_secs + secs + 1, m_usecs + usecs - max);
+	else
+		t.setTime(m_secs + secs, m_usecs + usecs);
+	
+	return t;
+}
+
+Timestamp Timestamp::operator- (double s) const
+{
+	unsigned long secs = (long)floor(s);
+	unsigned long usecs = (long)((s - (double)secs) * 1e6);
+
+	return this->minus(secs, usecs);
+}
+
+Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const
+{
+  Timestamp t;
+
+	const unsigned long max = 1000000ul;
+
+	if(m_usecs < usecs)
+		t.setTime(m_secs - secs - 1, max - (usecs - m_usecs));
+	else
+		t.setTime(m_secs - secs, m_usecs - usecs);
+	
+	return t;
+}
+
+bool Timestamp::operator> (const Timestamp &t) const
+{
+	if(m_secs > t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs > t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator>= (const Timestamp &t) const
+{
+	if(m_secs > t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator< (const Timestamp &t) const
+{
+	if(m_secs < t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs < t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator<= (const Timestamp &t) const
+{
+	if(m_secs < t.m_secs) return true;
+	else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs;
+	else return false;
+}
+
+bool Timestamp::operator== (const Timestamp &t) const
+{
+	return(m_secs == t.m_secs && m_usecs == t.m_usecs);
+}
+
+
+string Timestamp::Format(bool machine_friendly) const 
+{
+  struct tm tm_time;
+
+  time_t t = (time_t)getFloatTime();
+
+#ifdef WIN32
+  localtime_s(&tm_time, &t);
+#else
+  localtime_r(&t, &tm_time);
+#endif
+  
+  char buffer[128];
+  
+  if(machine_friendly)
+  {
+    strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time);
+  }
+  else
+  {
+    strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001
+  }
+  
+  return string(buffer);
+}
+
+string Timestamp::Format(double s) {
+	int days = int(s / (24. * 3600.0));
+	s -= days * (24. * 3600.0);
+	int hours = int(s / 3600.0);
+	s -= hours * 3600;
+	int minutes = int(s / 60.0);
+	s -= minutes * 60;
+	int seconds = int(s);
+	int ms = int((s - seconds)*1e6);
+
+	stringstream ss;
+	ss.fill('0');
+	bool b;
+	if((b = (days > 0))) ss << days << "d ";
+	if((b = (b || hours > 0))) ss << setw(2) << hours << ":";
+	if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":";
+	if(b) ss << setw(2);
+	ss << seconds;
+	if(!b) ss << "." << setw(6) << ms;
+
+	return ss.str();
+}
+
+

Thirdparty/DBoW2/DUtils/Timestamp.h

@@ -0,0 +1,204 @@
+/*
+ * File: Timestamp.h
+ * Author: Dorian Galvez-Lopez
+ * Date: March 2009
+ * Description: timestamping functions
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_TIMESTAMP__
+#define __D_TIMESTAMP__
+
+#include <iostream>
+using namespace std;
+
+namespace DUtils {
+
+/// Timestamp
+class Timestamp
+{
+public:
+
+  /// Options to initiate a timestamp
+  enum tOptions
+  {
+    NONE = 0,
+    CURRENT_TIME = 0x1,
+    ZERO = 0x2
+  };
+  
+public:
+  
+  /**
+   * Creates a timestamp 
+   * @param option option to set the initial time stamp
+   */
+	Timestamp(Timestamp::tOptions option = NONE);
+	
+	/**
+	 * Destructor
+	 */
+	virtual ~Timestamp(void);
+
+  /**
+   * Says if the timestamp is "empty": seconds and usecs are both 0, as 
+   * when initiated with the ZERO flag
+   * @return true iif secs == usecs == 0
+   */
+  bool empty() const;
+
+	/**
+	 * Sets this instance to the current time
+	 */
+	void setToCurrentTime();
+
+	/**
+	 * Sets the timestamp from seconds and microseconds
+	 * @param secs: seconds
+	 * @param usecs: microseconds
+	 */
+	inline void setTime(unsigned long secs, unsigned long usecs){
+		m_secs = secs;
+		m_usecs = usecs;
+	}
+	
+	/**
+	 * Returns the timestamp in seconds and microseconds
+	 * @param secs seconds
+	 * @param usecs microseconds
+	 */
+	inline void getTime(unsigned long &secs, unsigned long &usecs) const
+	{
+	  secs = m_secs;
+	  usecs = m_usecs;
+	}
+
+	/**
+	 * Sets the timestamp from a string with the time in seconds
+	 * @param stime: string such as "1235603336.036609"
+	 */
+	void setTime(const string &stime);
+	
+	/**
+	 * Sets the timestamp from a number of seconds from the epoch
+	 * @param s seconds from the epoch
+	 */
+	void setTime(double s);
+	
+	/**
+	 * Returns this timestamp as the number of seconds in (long) float format
+	 */
+	double getFloatTime() const;
+
+	/**
+	 * Returns this timestamp as the number of seconds in fixed length string format
+	 */
+	string getStringTime() const;
+
+	/**
+	 * Returns the difference in seconds between this timestamp (greater) and t (smaller)
+	 * If the order is swapped, a negative number is returned
+	 * @param t: timestamp to subtract from this timestamp
+	 * @return difference in seconds
+	 */
+	double operator- (const Timestamp &t) const;
+
+	/** 
+	 * Returns a copy of this timestamp + s seconds + us microseconds
+	 * @param s seconds
+	 * @param us microseconds
+	 */
+	Timestamp plus(unsigned long s, unsigned long us) const;
+
+  /**
+   * Returns a copy of this timestamp - s seconds - us microseconds
+   * @param s seconds
+   * @param us microseconds
+   */
+  Timestamp minus(unsigned long s, unsigned long us) const;
+
+  /**
+   * Adds s seconds to this timestamp and returns a reference to itself
+   * @param s seconds
+   * @return reference to this timestamp
+   */
+  Timestamp& operator+= (double s);
+  
+  /**
+   * Substracts s seconds to this timestamp and returns a reference to itself
+   * @param s seconds
+   * @return reference to this timestamp
+   */
+  Timestamp& operator-= (double s);
+
+	/**
+	 * Returns a copy of this timestamp + s seconds
+	 * @param s: seconds
+	 */
+	Timestamp operator+ (double s) const;
+
+	/**
+	 * Returns a copy of this timestamp - s seconds
+	 * @param s: seconds
+	 */
+	Timestamp operator- (double s) const;
+
+	/**
+	 * Returns whether this timestamp is at the future of t
+	 * @param t
+	 */
+	bool operator> (const Timestamp &t) const;
+
+	/**
+	 * Returns whether this timestamp is at the future of (or is the same as) t
+	 * @param t
+	 */
+	bool operator>= (const Timestamp &t) const;
+
+	/** 
+	 * Returns whether this timestamp and t represent the same instant
+	 * @param t
+	 */
+	bool operator== (const Timestamp &t) const;
+
+	/**
+	 * Returns whether this timestamp is at the past of t
+	 * @param t
+	 */
+	bool operator< (const Timestamp &t) const;
+
+	/**
+	 * Returns whether this timestamp is at the past of (or is the same as) t
+	 * @param t
+	 */
+	bool operator<= (const Timestamp &t) const;
+
+  /**
+   * Returns the timestamp in a human-readable string
+   * @param machine_friendly if true, the returned string is formatted
+   *   to yyyymmdd_hhmmss, without weekday or spaces
+   * @note This has not been tested under Windows
+   * @note The timestamp is truncated to seconds
+   */
+  string Format(bool machine_friendly = false) const;
+
+	/**
+	 * Returns a string version of the elapsed time in seconds, with the format
+	 * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us
+	 * @param s: elapsed seconds (given by getFloatTime) to format
+	 */
+	static string Format(double s);
+	
+
+protected:
+  /// Seconds
+	unsigned long m_secs;	// seconds
+	/// Microseconds
+	unsigned long m_usecs;	// microseconds
+};
+
+}
+
+#endif
+

Thirdparty/Sophus/CMakeLists.txt

@@ -97,17 +97,17 @@ target_compile_features (sophus INTERFACE
 add_custom_target(other SOURCES ${SOPHUS_OTHER_FILES})
 
 # Create 'test' make target using ctest
-option(BUILD_TESTS "Build tests." ON)
-if(BUILD_TESTS)
-    enable_testing()
-    add_subdirectory(test)
-endif()
+#option(BUILD_TESTS "Build tests." ON)
+#if(BUILD_TESTS)
+#    enable_testing()
+#    add_subdirectory(test)
+#endif()
 
 # Create examples make targets using ctest
-option(BUILD_EXAMPLES "Build examples." ON)
-if(BUILD_EXAMPLES)
-    add_subdirectory(examples)
-endif()
+#option(BUILD_EXAMPLES "Build examples." ON)
+#if(BUILD_EXAMPLES)
+#    add_subdirectory(examples)
+#endif()
 
 # Export package for use from the build tree
 set(SOPHUS_CMAKE_EXPORT_DIR ${CMAKE_INSTALL_DATADIR}/sophus/cmake)

Thirdparty/g2o/g2o/stuff/timeutil.cpp

@@ -0,0 +1,124 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "timeutil.h"
+#include <iostream>
+
+#ifdef _WINDOWS
+#include <time.h>
+#include <windows.h>
+#endif
+
+#ifdef UNIX
+#include <unistd.h>
+#endif
+
+namespace g2o {
+
+#ifdef _WINDOWS
+#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
+  #define DELTA_EPOCH_IN_MICROSECS  11644473600000000Ui64
+#else
+  #define DELTA_EPOCH_IN_MICROSECS  11644473600000000ULL
+#endif
+
+struct timezone
+{
+  int  tz_minuteswest; /* minutes W of Greenwich */
+  int  tz_dsttime;     /* type of dst correction */
+};
+
+int gettimeofday(struct timeval *tv, struct timezone *tz)
+{
+// Define a structure to receive the current Windows filetime
+  FILETIME ft;
+ 
+// Initialize the present time to 0 and the timezone to UTC
+  unsigned __int64 tmpres = 0;
+  static int tzflag = 0;
+ 
+  if (NULL != tv)
+  {
+    GetSystemTimeAsFileTime(&ft);
+ 
+// The GetSystemTimeAsFileTime returns the number of 100 nanosecond 
+// intervals since Jan 1, 1601 in a structure. Copy the high bits to 
+// the 64 bit tmpres, shift it left by 32 then or in the low 32 bits.
+    tmpres |= ft.dwHighDateTime;
+    tmpres <<= 32;
+    tmpres |= ft.dwLowDateTime;
+ 
+// Convert to microseconds by dividing by 10
+    tmpres /= 10;
+ 
+// The Unix epoch starts on Jan 1 1970.  Need to subtract the difference 
+// in seconds from Jan 1 1601.
+    tmpres -= DELTA_EPOCH_IN_MICROSECS;
+ 
+// Finally change microseconds to seconds and place in the seconds value. 
+// The modulus picks up the microseconds.
+    tv->tv_sec = (long)(tmpres / 1000000UL);
+    tv->tv_usec = (long)(tmpres % 1000000UL);
+  }
+ 
+  if (NULL != tz) {
+    if (!tzflag) {
+      _tzset();
+      tzflag++;
+    }
+
+    long sec;
+    int hours;
+    _get_timezone(&sec);
+    _get_daylight(&hours);
+  
+// Adjust for the timezone west of Greenwich
+    tz->tz_minuteswest = sec / 60;
+    tz->tz_dsttime = hours;
+  }
+ 
+  return 0;
+}
+#endif
+
+ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {}
+
+ScopeTime::~ScopeTime() {
+  std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n";
+}
+
+double get_monotonic_time()
+{
+#if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK))
+  struct timespec ts;
+  clock_gettime(CLOCK_MONOTONIC, &ts);
+  return ts.tv_sec + ts.tv_nsec*1e-9;
+#else
+  return get_time();
+#endif
+}
+
+} // end namespace

Thirdparty/g2o/g2o/stuff/timeutil.h

@@ -0,0 +1,132 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_TIMEUTIL_H
+#define G2O_TIMEUTIL_H
+
+#ifdef _WINDOWS
+#include <time.h>
+#else
+#include <sys/time.h>
+#endif
+
+#include <string>
+
+
+/** @addtogroup utils **/
+// @{
+
+/** \file timeutil.h
+ * \brief utility functions for handling time related stuff
+ */
+
+/// Executes code, only if secs are gone since last exec.
+/// extended version, in which the current time is given, e.g., timestamp of IPC message
+#ifndef DO_EVERY_TS
+#define DO_EVERY_TS(secs, currentTime, code) \
+if (1) {\
+  static double s_lastDone_ = (currentTime); \
+  double s_now_ = (currentTime); \
+  if (s_lastDone_ > s_now_) \
+    s_lastDone_ = s_now_; \
+  if (s_now_ - s_lastDone_ > (secs)) { \
+    code; \
+    s_lastDone_ = s_now_; \
+  }\
+} else \
+  (void)0
+#endif
+
+/// Executes code, only if secs are gone since last exec.
+#ifndef DO_EVERY
+#define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code)
+#endif
+
+#ifndef MEASURE_TIME
+#define MEASURE_TIME(text, code) \
+  if(1) { \
+    double _start_time_ = g2o::get_time(); \
+    code; \
+    fprintf(stderr, "%s took %f sec\n", text, g2o::get_time() - _start_time_); \
+  } else \
+    (void) 0
+#endif
+
+namespace g2o {
+
+#ifdef _WINDOWS
+typedef struct timeval {
+  long tv_sec;
+  long tv_usec;
+} timeval;
+ int gettimeofday(struct timeval *tv, struct timezone *tz);
+#endif
+
+/**
+ * return the current time in seconds since 1. Jan 1970
+ */
+inline double get_time() 
+{
+  struct timeval ts;
+  gettimeofday(&ts,0);
+  return ts.tv_sec + ts.tv_usec*1e-6;
+}
+
+/**
+ * return a monotonic increasing time which basically does not need to
+ * have a reference point. Consider this for measuring how long some
+ * code fragments required to execute.
+ *
+ * On Linux we call clock_gettime() on other systems we currently
+ * call get_time().
+ */
+ double get_monotonic_time();
+
+/**
+ * \brief Class to measure the time spent in a scope
+ *
+ * To use this class, e.g. to measure the time spent in a function,
+ * just create and instance at the beginning of the function.
+ */
+class  ScopeTime {
+  public: 
+    ScopeTime(const char* title);
+    ~ScopeTime();
+  private:
+    std::string _title;
+    double _startTime;
+};
+
+} // end namespace
+
+#ifndef MEASURE_FUNCTION_TIME
+#define MEASURE_FUNCTION_TIME \
+  g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__)
+#endif
+
+
+// @}
+#endif

build.sh

@@ -0,0 +1,40 @@
+echo "Configuring and building Thirdparty/DBoW2 ..."
+
+cd Thirdparty/DBoW2
+mkdir build
+cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release
+make -j
+
+cd ../../g2o
+
+echo "Configuring and building Thirdparty/g2o ..."
+
+mkdir build
+cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release
+make -j
+
+cd ../../Sophus
+
+echo "Configuring and building Thirdparty/Sophus ..."
+
+mkdir build
+cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release
+make -j
+
+cd ../../../
+
+echo "Uncompress vocabulary ..."
+
+cd Vocabulary
+tar -xf ORBvoc.txt.tar.gz
+cd ..
+
+echo "Configuring and building ORB_SLAM3 ..."
+
+mkdir build
+cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release
+make -j4