diff --git a/CMakeLists.txt b/CMakeLists.txt
index 016e74354d..3cd1a80d41 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,31 +12,31 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall   -O3")
 set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native")
 set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
 
-# Check C++11 or C++0x support
+# Check C++14 or C++0x support
 include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++14" 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")
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
    add_definitions(-DCOMPILEDWITHC11)
-   message(STATUS "Using flag -std=c++11.")
+   message(STATUS "Using flag -std=c++14.")
 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++14 support. Please use a different C++ compiler.")
 endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
-find_package(OpenCV 4.4)
-   if(NOT OpenCV_FOUND)
-      message(FATAL_ERROR "OpenCV > 4.4 not found.")
-   endif()
+#find_package(OpenCV 4.4)
+#   if(NOT OpenCV_FOUND)
+#      message(FATAL_ERROR "OpenCV > 4.4 not found.")
+#   endif()
 
-MESSAGE("OPENCV VERSION:")
-MESSAGE(${OpenCV_VERSION})
+#MESSAGE("OPENCV VERSION:")
+#MESSAGE(${OpenCV_VERSION})
 
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
@@ -121,6 +121,54 @@ ${EIGEN3_LIBS}
 ${Pangolin_LIBRARIES}
 ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
 ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
+#/usr/local/lib/aarch64-linux-gnu/libopencv_core.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_aruco.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_bgsegm.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_bioinspired.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_calib3d.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_ccalib.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_core.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_datasets.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_dnn_objdetect.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_dnn.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_dpm.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_face.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_features2d.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_flann.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_freetype.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_fuzzy.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_hdf.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_hfs.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_highgui.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_imgcodecs.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_img_hash.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_imgproc.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_line_descriptor.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_ml.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_objdetect.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_optflow.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_phase_unwrapping.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_photo.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_plot.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_reg.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_rgbd.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_saliency.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_sfm.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_shape.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_stereo.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_stitching.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_structured_light.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_superres.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_surface_matching.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_text.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_tracking.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_videoio.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_video.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_videostab.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_xfeatures2d.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_ximgproc.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_xobjdetect.so
+/usr/local/lib/aarch64-linux-gnu/libopencv_xphoto.so
 -lboost_serialization
 -lcrypto
 )
diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc
index 3a233129be..1ea8f95784 100644
--- a/Examples/Monocular/mono_euroc.cc
+++ b/Examples/Monocular/mono_euroc.cc
@@ -80,7 +80,7 @@ int main(int argc, char **argv)
     int fps = 20;
     float dT = 1.f/fps;
     // 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::MONOCULAR, false);
+    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR, true);
     float imageScale = SLAM.GetImageScale();
 
     double t_resize = 0.f;
diff --git a/Examples/euroc_eval_examples.sh b/Examples/euroc_eval_examples.sh
new file mode 100755
index 0000000000..908b2f2833
--- /dev/null
+++ b/Examples/euroc_eval_examples.sh
@@ -0,0 +1,19 @@
+#!/bin/bash
+pathDatasetEuroc='~/Downloads/euroc_test'
+
+# 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
+
diff --git a/Examples/euroc_examples.sh b/Examples/euroc_examples.sh
new file mode 100755
index 0000000000..8dc6d63ff5
--- /dev/null
+++ b/Examples/euroc_examples.sh
@@ -0,0 +1,182 @@
+#!/bin/bash
+pathDatasetEuroc='~/Downloads'
+
+#------------------------------------
+# 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
diff --git a/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt b/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt
index 12792fe44b..12e4ae8964 100644
--- a/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt
+++ b/Examples_old/ROS/ORB_SLAM3/CMakeLists.txt
@@ -12,20 +12,20 @@ MESSAGE("Build type: " ${ROS_BUILD_TYPE})
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall  -O3 -march=native ")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall  -O3 -march=native")
 
-# Check C++11 or C++0x support
+# Check C++14 or C++0x support
 include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++14" 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")
+   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
    add_definitions(-DCOMPILEDWITHC11)
-   message(STATUS "Using flag -std=c++11.")
+   message(STATUS "Using flag -std=c++14.")
 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++14 support. Please use a different C++ compiler.")
 endif()
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules)
diff --git a/build.sh b/build.sh
index 96d1c0941c..12e735f22f 100755
--- a/build.sh
+++ b/build.sh
@@ -4,7 +4,7 @@ cd Thirdparty/DBoW2
 mkdir build
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j
+make
 
 cd ../../g2o
 
@@ -13,7 +13,7 @@ echo "Configuring and building Thirdparty/g2o ..."
 mkdir build
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j
+make
 
 cd ../../Sophus
 
@@ -22,7 +22,7 @@ echo "Configuring and building Thirdparty/Sophus ..."
 mkdir build
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j
+make
 
 cd ../../../
 
@@ -37,4 +37,4 @@ echo "Configuring and building ORB_SLAM3 ..."
 mkdir build
 cd build
 cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j4
+make
diff --git a/euroc_examples.sh b/euroc_examples.sh
new file mode 100755
index 0000000000..8dc6d63ff5
--- /dev/null
+++ b/euroc_examples.sh
@@ -0,0 +1,182 @@
+#!/bin/bash
+pathDatasetEuroc='~/Downloads'
+
+#------------------------------------
+# 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
diff --git a/include/LoopClosing.h b/include/LoopClosing.h
index c9ccb269ae..91c5e2b11e 100644
--- a/include/LoopClosing.h
+++ b/include/LoopClosing.h
@@ -143,7 +143,9 @@ class LoopClosing
     void MergeLocal2();
 
     void CheckObservations(set<KeyFrame*> &spKFsMap1, set<KeyFrame*> &spKFsMap2);
-
+    
+    static bool compareByValue(const std::pair<int, int>& a, const std::pair<int, int>& b);
+    
     void ResetIfRequested();
     bool mbResetRequested;
     bool mbResetActiveMapRequested;
diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h
index 92bedd7f9a..c907de7069 100644
--- a/include/ORBmatcher.h
+++ b/include/ORBmatcher.h
@@ -66,7 +66,7 @@ namespace ORB_SLAM3
         // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level)
         // Used in Relocalisation and Loop Detection
         int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector<MapPoint*> &vpMapPointMatches);
-        int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector<MapPoint*> &vpMatches12);
+        int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector<MapPoint*> &vpMatches12, std::map<int, int> &mapPointsIndexToDistance, std::vector<std::pair<MapPoint*,MapPoint*>> &mapPoint1ToMapPoint2Pairs, std::map<MapPoint*, KeyFrame*> &mapPointToKeyFrame, std::set<MapPoint*> sMatchedMapPoints1, std::set<MapPoint*> sMatchedMapPoints2);
 
         // Matching for the Map Initialization (only used in the monocular case)
         int SearchForInitialization(Frame &F1, Frame &F2, std::vector<cv::Point2f> &vbPrevMatched, std::vector<int> &vnMatches12, int windowSize=10);
@@ -104,4 +104,4 @@ namespace ORB_SLAM3
 
 }// namespace ORB_SLAM
 
-#endif // ORBMATCHER_H
\ No newline at end of file
+#endif // ORBMATCHER_H
diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h
index 5539987823..abf1ea0bdb 100644
--- a/include/Sim3Solver.h
+++ b/include/Sim3Solver.h
@@ -34,9 +34,10 @@ class Sim3Solver
 {
 public:
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector<MapPoint*> &vpMatched12, const bool bFixScale = true,
-               const vector<KeyFrame*> vpKeyFrameMatchedMP = vector<KeyFrame*>());
-
+    Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint*> &mapPoints1,
+                        const vector<MapPoint *> &vpMatched12,
+                       std::map<MapPoint*, KeyFrame*> mapPointToKeyFrame, const bool bFixScale = true);
+    bool equalEigenVector(Eigen::Vector3f lastEigen, Eigen::Vector3f newEigen);
     void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300);
 
     Eigen::Matrix4f find(std::vector<bool> &vbInliers12, int &nInliers);
@@ -48,7 +49,7 @@ class Sim3Solver
     Eigen::Matrix3f GetEstimatedRotation();
     Eigen::Vector3f GetEstimatedTranslation();
     float GetEstimatedScale();
-
+    int N;
 protected:
 
     void ComputeCentroid(Eigen::Matrix3f &P, Eigen::Matrix3f &Pr, Eigen::Vector3f &C);
@@ -78,7 +79,7 @@ class Sim3Solver
     std::vector<size_t> mvnMaxError1;
     std::vector<size_t> mvnMaxError2;
 
-    int N;
+    
     int mN1;
 
     // Current Estimation
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index 293ab48182..ef91129752 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -376,6 +376,12 @@ MapPoint* KeyFrame::GetMapPoint(const size_t &idx)
     return mvpMapPoints[idx];
 }
 
+//this function loop on its map points, for each, it gets all the 
+//key frames that connected to it (called weight of each key frame). 
+//after counting all connected map points of key frames, 
+//we select the key frames that have more map points 
+//connected than the required threshold. if there are no kfs, 
+// choose the one with max weight.
 void KeyFrame::UpdateConnections(bool upParent)
 {
     map<KeyFrame*,int> KFcounter;
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index 319afb551b..8f2e5fb0b6 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -86,10 +86,19 @@ void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper)
     mpLocalMapper=pLocalMapper;
 }
 
+// When a new frame is captured, its ORB descriptors are matched with the descriptors 
+// of all the keyframes in the map.  If enough matches are found, the new 
+// frame is considered a loop closure candidate.
+
+// The next step is to verify whether the 
+// candidate frame is a true loop closure or a false positive. 
+
+// This is done by comparing the geometric consistency between the candidate 
+// frame and the keyframes in the map.
 
 void LoopClosing::Run()
 {
-    mbFinished =false;
+    mbFinished =false; 
 
     while(1)
     {
@@ -578,16 +587,20 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame*
 bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand, KeyFrame* &pMatchedKF2, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,
                                              int &nNumCoincidences, std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
 {
+    std::chrono::steady_clock::time_point time_StartQuery = std::chrono::steady_clock::now();
+
     int nBoWMatches = 20;
-    int nBoWInliers = 15;
+    int nBoWInliers = 10;
     int nSim3Inliers = 20;
     int nProjMatches = 50;
     int nProjOptMatches = 80;
 
+    // gets the set of all kfs that have enough shared map points (above threshold=15)
     set<KeyFrame*> spConnectedKeyFrames = mpCurrentKF->GetConnectedKeyFrames();
 
     int nNumCovisibles = 10;
 
+    //dont touch, these are good matcher values
     ORBmatcher matcherBoW(0.9, true);
     ORBmatcher matcher(0.75, true);
 
@@ -604,6 +617,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
     vector<int> vnMatchesStage(numCandidates, 0);
 
     int index = 0;
+    
     //Verbose::PrintMess("BoW candidates: There are " + to_string(vpBowCand.size()) + " possible candidates ", Verbose::VERBOSITY_DEBUG);
     for(KeyFrame* pKFi : vpBowCand)
     {
@@ -612,7 +626,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
 
         // std::cout << "KF candidate: " << pKFi->mnId << std::endl;
         // Current KF against KF with covisibles version
+        //find all covisible key frame of candidate, add the candidate to the list as well
         std::vector<KeyFrame*> vpCovKFi = pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles);
+        
         if(vpCovKFi.empty())
         {
             std::cout << "Covisible list empty" << std::endl;
@@ -621,7 +637,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
         else
         {
             vpCovKFi.push_back(vpCovKFi[0]);
-            vpCovKFi[0] = pKFi;
+            vpCovKFi[0] = pKFi; 
         }
 
 
@@ -641,73 +657,92 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
         }
         //std::cout << "Check BoW continue because is far to the matched one " << std::endl;
 
-
-        std::vector<std::vector<MapPoint*> > vvpMatchedMPs;
+    
+        std::vector<std::vector<MapPoint*> > vvpMatchedMPs; //for every cov kf, all matched map points
         vvpMatchedMPs.resize(vpCovKFi.size());
-        std::set<MapPoint*> spMatchedMPi;
         int numBoWMatches = 0;
 
-        KeyFrame* pMostBoWMatchesKF = pKFi;
+        KeyFrame* pMostBoWMatchesKF = pKFi; 
         int nMostBoWNumMatches = 0;
 
         std::vector<MapPoint*> vpMatchedPoints = std::vector<MapPoint*>(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
         std::vector<KeyFrame*> vpKeyFrameMatchedMP = std::vector<KeyFrame*>(mpCurrentKF->GetMapPointMatches().size(), static_cast<KeyFrame*>(NULL));
 
         int nIndexMostBoWMatchesKF=0;
-        for(int j=0; j<vpCovKFi.size(); ++j)
+        std::map<int, int> mapPointsIndexToDistance;
+        std::vector<std::pair<MapPoint*,MapPoint*>> mapPoint1ToMapPoint2Pairs;
+        std::map<MapPoint*, KeyFrame*> mapPointToKeyFrame;
+        
+        std::set<MapPoint*> sMatchedMapPoints1;
+        std::set<MapPoint*> sMatchedMapPoints2;
+        
+        for(int j=0; j<vpCovKFi.size(); ++j) //for cov kf
         {
             if(!vpCovKFi[j] || vpCovKFi[j]->isBad())
                 continue;
 
-            int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j]);
-            if (num > nMostBoWNumMatches)
+            //get matches by bow for our current frame with one of the cov kf of the i'th Bow Candidates
+            int num = matcherBoW.SearchByBoW(mpCurrentKF, vpCovKFi[j], vvpMatchedMPs[j], mapPointsIndexToDistance, mapPoint1ToMapPoint2Pairs, mapPointToKeyFrame, sMatchedMapPoints1, sMatchedMapPoints2); //fill the vvpMatchedMPs[j] with all matched map points of vpCovKFi[j] with mpCurrentKF
+            if (num > nMostBoWNumMatches) // pick the one with max matches
             {
                 nMostBoWNumMatches = num;
                 nIndexMostBoWMatchesKF = j;
             }
         }
-
-        for(int j=0; j<vpCovKFi.size(); ++j)
-        {
-            for(int k=0; k < vvpMatchedMPs[j].size(); ++k)
-            {
-                MapPoint* pMPi_j = vvpMatchedMPs[j][k];
-                if(!pMPi_j || pMPi_j->isBad())
-                    continue;
-
-                if(spMatchedMPi.find(pMPi_j) == spMatchedMPi.end())
-                {
-                    spMatchedMPi.insert(pMPi_j);
-                    numBoWMatches++;
-
-                    vpMatchedPoints[k]= pMPi_j;
-                    vpKeyFrameMatchedMP[k] = vpCovKFi[j];
+        
+        // Create a vector of pairs from the map
+        std::vector<std::pair<int, int>> pairsPointsIndexToDistance(mapPointsIndexToDistance.begin(), mapPointsIndexToDistance.end());
+
+        // Sort the vector based on the values using the custom comparator function
+        std::sort(pairsPointsIndexToDistance.begin(), pairsPointsIndexToDistance.end(), LoopClosing::compareByValue);
+
+        std::vector<MapPoint*> sortedMapPoints1ByScore;
+        std::vector<MapPoint*> sortedMapPoints2ByScore;
+        int lastScore = -1;
+        int repetitions = 1;
+        std::cout << "pairsPointsIndexToDistance.size = " << pairsPointsIndexToDistance.size() << std::endl;
+        std::cout << "the scores: " << std::endl;
+        for (const auto& [index,score] : pairsPointsIndexToDistance) {
+            MapPoint* mapPoint1 = mapPoint1ToMapPoint2Pairs[index].first;
+            MapPoint* mapPoint2 = mapPoint1ToMapPoint2Pairs[index].second;
+            if(!mapPoint1 || mapPoint1->isBad() || !mapPoint2 || mapPoint2->isBad()){
+                continue;
+            }
+            
+            if(score == lastScore){
+                repetitions++;
+                if(repetitions == 5){
+                    break;
                 }
+            } else {
+                repetitions = 1;
+                lastScore = score;
             }
+            
+            std::cout << score << ", ";
+            numBoWMatches++;
+            sortedMapPoints1ByScore.push_back(mapPoint1);
+            sortedMapPoints2ByScore.push_back(mapPoint2);
         }
+        std::cout << std::endl;
 
-        //pMostBoWMatchesKF = vpCovKFi[pMostBoWMatchesKF];
-
-        if(numBoWMatches >= nBoWMatches) // TODO pick a good threshold
+        if(numBoWMatches >= nBoWMatches) // TODO pick a good threshold, now nBoWMatches=20
         {
             // Geometric validation
             bool bFixedScale = mbFixScale;
             if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
                 bFixedScale=false;
 
-            Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, vpMatchedPoints, bFixedScale, vpKeyFrameMatchedMP);
-            solver.SetRansacParameters(0.99, nBoWInliers, 300); // at least 15 inliers
+            Sim3Solver solver = Sim3Solver(mpCurrentKF, pMostBoWMatchesKF, sortedMapPoints1ByScore, sortedMapPoints2ByScore, mapPointToKeyFrame, bFixedScale);
+            nBoWInliers = std::max(nBoWInliers, solver.N / 4);
+            solver.SetRansacParameters(0.99, nBoWInliers, 300); 
 
             bool bNoMore = false;
             vector<bool> vbInliers;
             int nInliers;
             bool bConverge = false;
             Eigen::Matrix4f mTcm;
-            while(!bConverge && !bNoMore)
-            {
-                mTcm = solver.iterate(20,bNoMore, vbInliers, nInliers, bConverge);
-                //Verbose::PrintMess("BoW guess: Solver achieve " + to_string(nInliers) + " geometrical inliers among " + to_string(nBoWInliers) + " BoW matches", Verbose::VERBOSITY_DEBUG);
-            }
+            mTcm = solver.iterate(300, bNoMore, vbInliers, nInliers, bConverge);
 
             if(bConverge)
             {
@@ -778,40 +813,6 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
 
                         if(numProjOptMatches >= nProjOptMatches)
                         {
-                            int max_x = -1, min_x = 1000000;
-                            int max_y = -1, min_y = 1000000;
-                            for(MapPoint* pMPi : vpMatchedMP)
-                            {
-                                if(!pMPi || pMPi->isBad())
-                                {
-                                    continue;
-                                }
-
-                                tuple<size_t,size_t> indexes = pMPi->GetIndexInKeyFrame(pKFi);
-                                int index = get<0>(indexes);
-                                if(index >= 0)
-                                {
-                                    int coord_x = pKFi->mvKeysUn[index].pt.x;
-                                    if(coord_x < min_x)
-                                    {
-                                        min_x = coord_x;
-                                    }
-                                    if(coord_x > max_x)
-                                    {
-                                        max_x = coord_x;
-                                    }
-                                    int coord_y = pKFi->mvKeysUn[index].pt.y;
-                                    if(coord_y < min_y)
-                                    {
-                                        min_y = coord_y;
-                                    }
-                                    if(coord_y > max_y)
-                                    {
-                                        max_y = coord_y;
-                                    }
-                                }
-                            }
-
                             int nNumKFs = 0;
                             //vpMatchedMPs = vpMatchedMP;
                             //vpMPs = vpMapPoints;
@@ -859,13 +860,14 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
                     }
                 }
             }
-            /*else
-            {
-                Verbose::PrintMess("BoW candidate: it don't match with the current one", Verbose::VERBOSITY_DEBUG);
-            }*/
         }
         index++;
     }
+    
+    std::chrono::steady_clock::time_point time_EndQuery = std::chrono::steady_clock::now();
+
+    double timeDataQuery = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndQuery - time_StartQuery).count();
+    std::cout << "time: " << timeDataQuery << std::endl;
 
     if(nBestMatchesReproj > 0)
     {
@@ -879,7 +881,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
 
         return nNumCoincidences >= 3;
     }
-    else
+    /* else
     {
         int maxStage = -1;
         int maxMatched;
@@ -891,10 +893,14 @@ bool LoopClosing::DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand,
                 maxMatched = vnMatchesStage[i];
             }
         }
-    }
+    } */
     return false;
 }
 
+bool LoopClosing::compareByValue(const std::pair<int, int>& a, const std::pair<int, int>& b) {
+    return a.second < b.second;
+}
+
 bool LoopClosing::DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
                                                 std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
 {
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index 9129683e4e..72793a60dc 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -762,7 +762,7 @@ namespace ORB_SLAM3
         return nmatches;
     }
 
-    int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)
+    int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12, std::map<int, int> &mapPointsIndexToDistance, std::vector<std::pair<MapPoint*,MapPoint*>> &mapPoint1ToMapPoint2Pairs, std::map<MapPoint*, KeyFrame*> &mapPointToKeyFrame, std::set<MapPoint*> sMatchedMapPoints1, std::set<MapPoint*> sMatchedMapPoints2)
     {
         const vector<cv::KeyPoint> &vKeysUn1 = pKF1->mvKeysUn;
         const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
@@ -823,10 +823,7 @@ namespace ORB_SLAM3
 
                         MapPoint* pMP2 = vpMapPoints2[idx2];
 
-                        if(vbMatched2[idx2] || !pMP2)
-                            continue;
-
-                        if(pMP2->isBad())
+                        if(vbMatched2[idx2] || !pMP2 || pMP2->isBad())
                             continue;
 
                         const cv::Mat &d2 = Descriptors2.row(idx2);
@@ -845,26 +842,27 @@ namespace ORB_SLAM3
                         }
                     }
 
-                    if(bestDist1<TH_LOW)
+                    if(bestDist1 < TH_LOW && 
+                        (static_cast<float>(bestDist1) < mfNNratio * static_cast<float>(bestDist2)))
                     {
-                        if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
-                        {
-                            vpMatches12[idx1]=vpMapPoints2[bestIdx2];
-                            vbMatched2[bestIdx2]=true;
+                        mapPoint1ToMapPoint2Pairs.push_back(std::make_pair(vpMapPoints1[idx1], vpMapPoints2[bestIdx2]));
+                        mapPointsIndexToDistance[mapPoint1ToMapPoint2Pairs.size() - 1] = bestDist1;
+                        mapPointToKeyFrame[vpMapPoints2[bestIdx2]] = pKF2;
+                        vpMatches12[idx1]=vpMapPoints2[bestIdx2]; //todo why dont we need to save from vpMapPoints1 too?
+                        vbMatched2[bestIdx2]=true;
 
-                            if(mbCheckOrientation)
-                            {
-                                float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].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);
-                            }
-                            nmatches++;
+                        if(mbCheckOrientation)
+                        {
+                            float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].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);
                         }
+                        nmatches++;
                     }
                 }
 
@@ -900,7 +898,6 @@ namespace ORB_SLAM3
                 }
             }
         }
-
         return nmatches;
     }
 
diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc
index b8d3abfb14..88b0990f39 100644
--- a/src/Sim3Solver.cc
+++ b/src/Sim3Solver.cc
@@ -32,28 +32,27 @@ namespace ORB_SLAM3
 {
 
 
-Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *> &vpMatched12, const bool bFixScale,
-                       vector<KeyFrame*> vpKeyFrameMatchedMP):
+Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint*> &vpKeyFrameMP1,
+                        const vector<MapPoint *> &vpKeyFrameMP2,
+                       std::map<MapPoint*, KeyFrame*> mapPointToKeyFrame, const bool bFixScale):
     mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale),
     pCamera1(pKF1->mpCamera), pCamera2(pKF2->mpCamera)
 {
-    bool bDifferentKFs = false;
-    if(vpKeyFrameMatchedMP.empty())
+    bool bDifferentKFs = true; //todo
+    if(mapPointToKeyFrame.empty())
     {
-        bDifferentKFs = true;
-        vpKeyFrameMatchedMP = vector<KeyFrame*>(vpMatched12.size(), pKF2);
+        bDifferentKFs = false;
+        //vpKeyFrameMatchedMP = vector<KeyFrame*>(vpMatched12.size(), pKF2);
     }
 
     mpKF1 = pKF1;
     mpKF2 = pKF2;
 
-    vector<MapPoint*> vpKeyFrameMP1 = pKF1->GetMapPointMatches();
-
-    mN1 = vpMatched12.size();
+    mN1 = vpKeyFrameMP2.size();
 
     mvpMapPoints1.reserve(mN1);
     mvpMapPoints2.reserve(mN1);
-    mvpMatches12 = vpMatched12;
+    mvpMatches12 = vpKeyFrameMP2;
     mvnIndices1.reserve(mN1);
     mvX3Dc1.reserve(mN1);
     mvX3Dc2.reserve(mN1);
@@ -70,19 +69,19 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *>
     KeyFrame* pKFm = pKF2; //Default variable
     for(int i1=0; i1<mN1; i1++)
     {
-        if(vpMatched12[i1])
+        if(vpKeyFrameMP1[i1] && vpKeyFrameMP2[i1])
         {
             MapPoint* pMP1 = vpKeyFrameMP1[i1];
-            MapPoint* pMP2 = vpMatched12[i1];
+            MapPoint* pMP2 = vpKeyFrameMP2[i1];
 
             if(!pMP1)
                 continue;
-
+            
             if(pMP1->isBad() || pMP2->isBad())
                 continue;
-
-            if(bDifferentKFs)
-                pKFm = vpKeyFrameMatchedMP[i1];
+            
+            if(bDifferentKFs) 
+                pKFm = mapPointToKeyFrame[pMP2];
 
             int indexKF1 = get<0>(pMP1->GetIndexInKeyFrame(pKF1));
             int indexKF2 = get<0>(pMP2->GetIndexInKeyFrame(pKFm));
@@ -96,6 +95,13 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *>
             const float sigmaSquare1 = pKF1->mvLevelSigma2[kp1.octave];
             const float sigmaSquare2 = pKFm->mvLevelSigma2[kp2.octave];
 
+            Eigen::Vector3f X3D1w = pMP1->GetWorldPos();
+            Eigen::Vector3f X3D2w = pMP2->GetWorldPos();
+            if(equalEigenVector(mvX3Dc1[mvX3Dc1.size() - 1], Rcw1*X3D1w+tcw1) || 
+                equalEigenVector(mvX3Dc2[mvX3Dc2.size() - 1], Rcw2*X3D2w+tcw2)){
+                continue;
+            }
+            
             mvnMaxError1.push_back(9.210*sigmaSquare1);
             mvnMaxError2.push_back(9.210*sigmaSquare2);
 
@@ -103,12 +109,9 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *>
             mvpMapPoints2.push_back(pMP2);
             mvnIndices1.push_back(i1);
 
-            Eigen::Vector3f X3D1w = pMP1->GetWorldPos();
             mvX3Dc1.push_back(Rcw1*X3D1w+tcw1);
-
-            Eigen::Vector3f X3D2w = pMP2->GetWorldPos();
             mvX3Dc2.push_back(Rcw2*X3D2w+tcw2);
-
+            
             mvAllIndices.push_back(idx);
             idx++;
         }
@@ -120,6 +123,10 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *>
     SetRansacParameters();
 }
 
+bool Sim3Solver::equalEigenVector(Eigen::Vector3f lastEigen, Eigen::Vector3f newEigen){
+    return lastEigen.x() == newEigen.x() && lastEigen.y() == newEigen.y() && lastEigen.z() == newEigen.z();
+}
+
 void Sim3Solver::SetRansacParameters(double probability, int minInliers, int maxIterations)
 {
     mRansacProb = probability;
@@ -228,38 +235,25 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool>
         return Eigen::Matrix4f::Identity();
     }
 
-    vector<size_t> vAvailableIndices;
-
     Eigen::Matrix3f P3Dc1i;
     Eigen::Matrix3f P3Dc2i;
 
     int nCurrentIterations = 0;
+    
 
     Eigen::Matrix4f bestSim3;
-
-    while(mnIterations<mRansacMaxIts && nCurrentIterations<nIterations)
-    {
-        nCurrentIterations++;
-        mnIterations++;
-
-        vAvailableIndices = mvAllIndices;
-
-        // Get min set of points
-        for(short i = 0; i < 3; ++i)
+    while(mnIterations<mRansacMaxIts && nCurrentIterations<nIterations && 
+          (mnIterations + 1) * 3 < mvX3Dc1.size()  && (mnIterations + 1) * 3 < mvX3Dc2.size())
+    {        
+        int startingIndex = mnIterations * 3;
+        for(short i = startingIndex; i < (mnIterations + 1) * 3 && i < mvX3Dc1.size() && i < mvX3Dc1.size(); ++i)
         {
-            int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
-
-            int idx = vAvailableIndices[randi];
-
-            P3Dc1i.col(i) = mvX3Dc1[idx];
-            P3Dc2i.col(i) = mvX3Dc2[idx];
-
-            vAvailableIndices[randi] = vAvailableIndices.back();
-            vAvailableIndices.pop_back();
+            equalEigenVector(mvX3Dc1[i], mvX3Dc1[i+1]);
+            P3Dc1i.col(i - startingIndex) = mvX3Dc1[i]; 
+            P3Dc2i.col(i - startingIndex) = mvX3Dc2[i];//in the contructor- we filled the mvX3Dc2 with the sorted order
         }
-
+        
         ComputeSim3(P3Dc1i,P3Dc2i);
-
         CheckInliers();
 
         if(mnInliersi>=mnBestInliers)
@@ -270,13 +264,14 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool>
             mBestRotation = mR12i;
             mBestTranslation = mt12i;
             mBestScale = ms12i;
-
+        
             if(mnInliersi>mRansacMinInliers)
             {
                 nInliers = mnInliersi;
-                for(int i=0; i<N; i++)
+                for(int i=0; i<N; i++){
                     if(mvbInliersi[i])
                         vbInliers[mvnIndices1[i]] = true;
+                    }
                 bConverge = true;
                 return mBestT12;
             }
@@ -285,9 +280,11 @@ Eigen::Matrix4f Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool>
                 bestSim3 = mBestT12;
             }
         }
+        nCurrentIterations++;
+        mnIterations++;
     }
-
-    if(mnIterations>=mRansacMaxIts)
+    
+    if(mnIterations>=mRansacMaxIts || (mnIterations + 1) * 3 >= mvX3Dc1.size() || (mnIterations + 1) * 3  >= mvX3Dc2.size())
         bNoMore=true;
 
     return bestSim3;
@@ -366,7 +363,6 @@ void Sim3Solver::ComputeSim3(Eigen::Matrix3f &P1, Eigen::Matrix3f &P2)
 
     vec = 2*ang*vec/vec.norm(); //Angle-axis representation. quaternion angle is the half
     mR12i = Sophus::SO3f::exp(vec).matrix();
-
     // Step 5: Rotate set 2
     Eigen::Matrix3f P3 = mR12i*Pr2;
 
@@ -436,6 +432,7 @@ void Sim3Solver::CheckInliers()
         else
             mvbInliersi[i]=false;
     }
+    std::cout << "mnInliersi: " << mnInliersi << std::endl;
 }
 
 Eigen::Matrix4f Sim3Solver::GetEstimatedTransformation()