V1

.vscode/c_cpp_properties.json

@@ -0,0 +1,20 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "${workspaceFolder}/include",
+                "/usr/include/opencv4",
+                "/usr/local/include/opencv4",
+                "/usr/include/eigen3"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++17",
+            "intelliSenseMode": "linux-gcc-x64"
+        }
+    ],
+    "version": 4
+}

CLAUDE.md

@@ -0,0 +1,113 @@
+# CLAUDE.md
+
+This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository.
+
+## Build
+
+**First-time build** (builds Thirdparty libs + main library):
+```bash
+./build.sh
+```
+
+**Incremental rebuild** (after modifying `src/` or `include/`):
+```bash
+cd build && make -j4
+```
+
+**ROS nodes** (optional):
+```bash
+./build_ros.sh
+```
+
+The build produces `lib/libORB_SLAM3.so` and executables under `Examples/`.
+
+## Running Examples
+
+All executables take `Vocabulary/ORBvoc.txt` as first argument and a YAML config as second.
+
+**Stereo-Inertial on EuRoC:**
+```bash
+./Examples/Stereo-Inertial/stereo_inertial_euroc \
+    Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/EuRoC.yaml \
+    /path/to/EuRoC/MH_01_easy ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo_inertial
+```
+
+**Stereo on EuRoC:**
+```bash
+./Examples/Stereo/stereo_euroc \
+    Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml \
+    /path/to/EuRoC/MH_01_easy ./Examples/Stereo/EuRoC_TimeStamps/MH01.txt dataset-MH01_stereo
+```
+
+Dataset paths on this machine:
+- EuRoC: `/media/lab405/Windows1/data/Euroc`
+- TUM-VI: `/media/lab405/Windows1/data/TUM`
+
+## Evaluation / Benchmarking
+
+The unified benchmark script runs all sensor configurations and computes APE RMSE via `evo_ape`:
+```bash
+cd evaluation
+
+# All algorithms and datasets
+python3 run_stereo_inertial_benchmark.py
+
+# Filter by algorithm or dataset
+python3 run_stereo_inertial_benchmark.py --algo stereo --dataset euroc
+python3 run_stereo_inertial_benchmark.py --algo stereo_inertial
+python3 run_stereo_inertial_benchmark.py --algo mono_inertial --dataset tum
+```
+
+Results are saved to `evaluation/benchmark_results_<timestamp>.csv`. The script:
+1. Runs the binary
+2. Converts timestamps ns→s via `fix_time.py`
+3. Computes APE RMSE with `evo_ape`
+
+`evaluation/ate_3d.py` is a 3D ATE evaluation script.
+
+## Enabling Timing Profiling
+
+Uncomment `#define REGISTER_TIMES` in `include/Config.h` (note: currently the file is a stub — the actual flag was used in the old code path), then rebuild. Stats are printed to terminal and saved to `ExecTimeMean.txt`.
+
+## Architecture
+
+ORB-SLAM3 is a **multi-threaded SLAM system** built around three parallel threads:
+
+### Core Threads
+- **Tracking** (`src/Tracking.cc`): Main thread. Receives each frame, extracts ORB features, estimates camera pose via motion model or relocalization, and decides when to insert KeyFrames.
+- **LocalMapping** (`src/LocalMapping.cc`): Processes new KeyFrames — creates new MapPoints via triangulation, culls redundant KeyFrames, and runs Local BA (via `Optimizer`).
+- **LoopClosing** (`src/LoopClosing.cc`): Detects loop closures using DBoW2 place recognition, computes Sim3 correction, runs Essential Graph optimization, and triggers full Bundle Adjustment.
+
+### Key Data Structures
+- **Atlas** (`src/Atlas.cc`): Container for multiple Maps. Enables multi-map SLAM — when tracking is lost, a new Map is created; loop closure can merge maps.
+- **Map** / **MapPoint** / **KeyFrame**: The 3D map representation. MapPoints store 3D position and observations from multiple KeyFrames.
+- **Frame**: A single image capture with extracted ORB features. Short-lived; converted to KeyFrame when selected.
+
+### Optimization (`src/Optimizer.cc`)
+All non-linear optimization uses **g2o** (included in `Thirdparty/g2o`). Key functions:
+- `PoseOptimization` — single-frame pose-only BA
+- `LocalBundleAdjustment` — local map BA triggered by LocalMapping; the `num_iters` parameter has been added to this fork for monitoring
+- `OptimizeEssentialGraph` — loop closure correction
+- `GlobalBundleAdjustemnt` — full map BA after loop closure
+
+### IMU Integration
+`src/ImuTypes.cc` and `include/ImuTypes.h` define IMU preintegration. `src/G2oTypes.cc` defines the g2o edge/vertex types for visual-inertial optimization. IMU initialization happens in Tracking and is refined in LocalMapping.
+
+### Camera Models
+`include/CameraModels/`: `Pinhole` (standard) and `KannalaBrandt8` (fisheye). Both inherit from `GeometricCamera`. The active model is selected based on the YAML config `Camera.type` field.
+
+### Settings / Config
+`src/Settings.cc` parses YAML config files (OpenCV `FileStorage`). Each YAML in `Examples/*/` corresponds to a specific sensor+dataset combination. IMU noise parameters and camera intrinsics/extrinsics live there.
+
+### Vocabulary
+`Vocabulary/ORBvoc.txt` is the prebuilt DBoW2 vocabulary (decompressed from `ORBvoc.txt.tar.gz` by `build.sh`). Required at runtime — do not delete.
+
+## Modified Files in This Fork
+
+This fork adds monitoring/logging capabilities:
+- `src/Tracking.cc` — instrumentation additions
+- `src/Optimizer.cc` — `num_iters` parameter for `LocalBundleAdjustment`, monitoring output
+- `Examples/Stereo-Inertial/TUM-VI_far.yaml` — tuned config for far-range TUM-VI
+- `Examples/Stereo/EuRoC.yaml` — tuned stereo config
+
+CSV logs (`localmapping_log.csv`, `tracking_log.csv`) are written during runs for analysis. `map_points_total` = current map point count; `new_map_points` = points attempted in a given frame.

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.10)
 project(ORB_SLAM3)
 
 IF(NOT CMAKE_BUILD_TYPE)
@@ -14,18 +14,19 @@ set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
 
 # Check C++11 or C++0x support
 include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
 CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
-   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+if(COMPILER_SUPPORTS_CXX14)
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -Wno-maybe-uninitialized -w")
+   add_definitions(-DCOMPILEDWITHC14)
+   message(STATUS "Using flag -std=c++14.")
+elseif(COMPILER_SUPPORTS_CXX11)
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wno-maybe-uninitialized -w")
    add_definitions(-DCOMPILEDWITHC11)
    message(STATUS "Using flag -std=c++11.")
-elseif(COMPILER_SUPPORTS_CXX0X)
-   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
-   add_definitions(-DCOMPILEDWITHC0X)
-   message(STATUS "Using flag -std=c++0x.")
 else()
-   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11/C++14 support. Please use a different C++ compiler.")
 endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)

Examples/Monocular-Inertial/TUM-VI.yaml

@@ -1,5 +1,5 @@
 %YAML:1.0
-
+loopClosing: 0
 #--------------------------------------------------------------------------------------------
 # Camera Parameters. Adjust them!
 #--------------------------------------------------------------------------------------------

Examples/Monocular-Inertial/TUM-VI_far.yaml

@@ -46,7 +46,7 @@ IMU.NoiseGyro: 0.00016 # 0.004 (VINS) # 0.00016 (TUM) # 0.00016    # rad/s^0.5
 IMU.NoiseAcc: 0.0028 # 0.04 (VINS) # 0.0028 (TUM) # 0.0028     # m/s^1.5
 IMU.GyroWalk: 0.000022 # 0.000022 (VINS and TUM) rad/s^1.5
 IMU.AccWalk: 0.00086 # 0.0004 (VINS) # 0.00086 # 0.00086    # m/s^2.5
-IMU.Frequency: 200
+IMU.Frequency: 200.0
 System.thFarPoints: 20.0
 
 #--------------------------------------------------------------------------------------------

Examples/Monocular-Inertial/mono_inertial_euroc.cc

@@ -117,7 +117,7 @@ int main(int argc, char *argv[])
     cout.precision(17);
 
     // 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);
+    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, false);
     float imageScale = SLAM.GetImageScale();
 
     double t_resize = 0.f;
@@ -151,7 +151,7 @@ int main(int argc, char *argv[])
     #ifdef COMPILEDWITHC11
                 std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
     #else
-                std::chrono::monotonic_clock::time_point t_Start_Resize = std::chrono::monotonic_clock::now();
+                std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
     #endif
 #endif
                 int width = im.cols * imageScale;
@@ -161,7 +161,7 @@ int main(int argc, char *argv[])
     #ifdef COMPILEDWITHC11
                 std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
     #else
-                std::chrono::monotonic_clock::time_point t_End_Resize = std::chrono::monotonic_clock::now();
+                std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
     #endif
                 t_resize = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Resize - t_Start_Resize).count();
                 SLAM.InsertResizeTime(t_resize);
@@ -187,7 +187,7 @@ int main(int argc, char *argv[])
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     #else
-            std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
+            std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     #endif
 
             // Pass the image to the SLAM system
@@ -197,7 +197,7 @@ int main(int argc, char *argv[])
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
     #else
-            std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
+            std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
     #endif
 
 #ifdef REGISTER_TIMES
@@ -239,11 +239,13 @@ int main(int argc, char *argv[])
         const string f_file =  "f_" + string(argv[argc-1]) + ".txt";
         SLAM.SaveTrajectoryEuRoC(f_file);
         SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+        SLAM.SaveMapPoints(string(argv[argc-1]) + "_mappoints.txt");
     }
     else
     {
         SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
         SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+        SLAM.SaveMapPoints("mappoints.txt");
     }
 
     return 0;

Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc

@@ -317,7 +317,7 @@ int main(int argc, char **argv) {
 #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point time_Start_Process = std::chrono::steady_clock::now();
 #else
-            std::chrono::monotonic_clock::time_point time_Start_Process = std::chrono::monotonic_clock::now();
+            std::chrono::steady_clock::time_point time_Start_Process = std::chrono::steady_clock::now();
 #endif
 
             if(count_im_buffer>1)
@@ -368,7 +368,7 @@ int main(int argc, char **argv) {
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
     #else
-            std::chrono::monotonic_clock::time_point t_Start_Resize = std::chrono::monotonic_clock::now();
+            std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
     #endif
 #endif
             int width = im.cols * imageScale;
@@ -378,7 +378,7 @@ int main(int argc, char **argv) {
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
     #else
-            std::chrono::monotonic_clock::time_point t_End_Resize = std::chrono::monotonic_clock::now();
+            std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
     #endif
             t_resize = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Resize - t_Start_Resize).count();
             SLAM.InsertResizeTime(t_resize);
@@ -389,7 +389,7 @@ int main(int argc, char **argv) {
     #ifdef COMPILEDWITHC11
         std::chrono::steady_clock::time_point t_Start_Track = std::chrono::steady_clock::now();
     #else
-        std::chrono::monotonic_clock::time_point t_Start_Track = std::chrono::monotonic_clock::now();
+        std::chrono::steady_clock::time_point t_Start_Track = std::chrono::steady_clock::now();
     #endif
 #endif
         // Pass the image to the SLAM system
@@ -398,7 +398,7 @@ int main(int argc, char **argv) {
     #ifdef COMPILEDWITHC11
         std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now();
     #else
-        std::chrono::monotonic_clock::time_point t_End_Track = std::chrono::monotonic_clock::now();
+        std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now();
     #endif
         t_track = t_resize + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Track - t_Start_Track).count();
         SLAM.InsertTrackTime(t_track);

Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc

@@ -233,7 +233,7 @@ int main(int argc, char **argv)
 #ifdef COMPILEDWITHC11
         std::chrono::steady_clock::time_point time_Start_Process = std::chrono::steady_clock::now();
 #else
-        std::chrono::monotonic_clock::time_point time_Start_Process = std::chrono::monotonic_clock::now();
+        std::chrono::steady_clock::time_point time_Start_Process = std::chrono::steady_clock::now();
 #endif
 
             if(count_im_buffer>1)
@@ -260,7 +260,7 @@ int main(int argc, char **argv)
         #ifdef COMPILEDWITHC11
                 std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
         #else
-                std::chrono::monotonic_clock::time_point t_Start_Resize = std::chrono::monotonic_clock::now();
+                std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
         #endif
     #endif
                 int width = imCV.cols * imageScale;
@@ -270,7 +270,7 @@ int main(int argc, char **argv)
         #ifdef COMPILEDWITHC11
                 std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
         #else
-                std::chrono::monotonic_clock::time_point t_End_Resize = std::chrono::monotonic_clock::now();
+                std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
         #endif
                 t_resize = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Resize - t_Start_Resize).count();
                 SLAM.InsertResizeTime(t_resize);
@@ -311,15 +311,15 @@ int main(int argc, char **argv)
 #ifdef COMPILEDWITHC11
         std::chrono::steady_clock::time_point t_Start_Track = std::chrono::steady_clock::now();
 #else
-        std::chrono::monotonic_clock::time_point t_Start_Track = std::chrono::monotonic_clock::now();
+        std::chrono::steady_clock::time_point t_Start_Track = std::chrono::steady_clock::now();
 #endif
         // Pass the image to the SLAM system
         SLAM.TrackMonocular(im, timestamp, vImuMeas);
 
 #ifdef COMPILEDWITHC11
         std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now();
 #else
-        std::chrono::monotonic_clock::time_point t_End_Track = std::chrono::monotonic_clock::now();
+        std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now();
 #endif
 
 #ifdef REGISTER_TIMES

Examples/Monocular-Inertial/mono_inertial_tum_vi.cc

@@ -116,7 +116,7 @@ int main(int argc, char **argv)
     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);
+    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, false, 0, file_name);
     float imageScale = SLAM.GetImageScale();
 
     double t_resize = 0.f;
@@ -175,7 +175,7 @@ int main(int argc, char **argv)
     #ifdef COMPILEDWITHC11
                 std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
     #else
-                std::chrono::monotonic_clock::time_point t_Start_Resize = std::chrono::monotonic_clock::now();
+                std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
     #endif
 #endif
                 int width = im.cols * imageScale;
@@ -185,7 +185,7 @@ int main(int argc, char **argv)
     #ifdef COMPILEDWITHC11
                 std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
     #else
-                std::chrono::monotonic_clock::time_point t_End_Resize = std::chrono::monotonic_clock::now();
+                std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
     #endif
                 t_resize = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Resize - t_Start_Resize).count();
                 SLAM.InsertResizeTime(t_resize);
@@ -198,7 +198,7 @@ int main(int argc, char **argv)
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     #else
-            std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
+            std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     #endif
 
             // Pass the image to the SLAM system
@@ -208,7 +208,7 @@ int main(int argc, char **argv)
     #ifdef COMPILEDWITHC11
             std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
     #else
-            std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
+            std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
     #endif
 
 #ifdef REGISTER_TIMES
@@ -257,11 +257,13 @@ int main(int argc, char **argv)
         const string f_file =  "f_" + string(argv[argc-1]) + ".txt";
         SLAM.SaveTrajectoryEuRoC(f_file);
         SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+        SLAM.SaveMapPoints(string(argv[argc-1]) + "_mappoints.txt");
     }
     else
     {
         SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
         SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+        SLAM.SaveMapPoints("mappoints.txt");
     }
 
     sort(vTimesTrack.begin(),vTimesTrack.end());