From ab8b51d90c2aa1f904de1f49259c742df566fa6f Mon Sep 17 00:00:00 2001
From: Aaron Marburg <amarburg@uw.edu>
Date: Sun, 28 Dec 2025 17:10:14 +0000
Subject: [PATCH 1/9] Interim commit

---
 README.md                  |    6 +-
 Thirdparty/tl/expected.hpp | 2475 ++++++++++++++++++++++++++++++++++++
 include/Settings.h         |   18 +
 include/System.h           |    8 +
 src/Settings.cc            |  288 +++--
 src/System.cc              |   35 +-
 src/Tracking.cc            |    8 +-
 7 files changed, 2731 insertions(+), 107 deletions(-)
 create mode 100644 Thirdparty/tl/expected.hpp

diff --git a/README.md b/README.md
index 1b42a01b524..bb0cd830c64 100644
--- a/README.md
+++ b/README.md
@@ -1,14 +1,16 @@
 > [!NOTE]
-> This is my personal "working" fork of ORBSLAM3, which focuses on integrating ORBSLAM3 into ROS2.   The actual ROS2 integration is implemented in [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2).   Relative to the original code, this repo contains multiple updates:
+> This is my personal "working" fork of ORBSLAM3, which focuses on integrating ORBSLAM3 into ROS2.   The actual ROS2 integration is implemented in [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2).
+
+Relative to the original code, this repo contains multiple updates:
 
 * I removed the "ThirdParty" copies of "Sophus" and "g2o" in lieu of packages which can be installed "rosdep".  Due to API changes, this necessitated some syntactically invasive (but functionally equivalent) changes.
 * This branch contains preliminary migration to [spdlog](https://github.com/gabime/spdlog) as a more controllable logging backend.  This is a slow-motion migration to better manage text output from ORBSLAM3.
 * As I dug further into the code, I got more opinionated.  I also added [pre-commit](.pre-commit-config.yaml), which introduced significant textual changes.   No going back!
+* [`Thirdparty/tl/`](Thirdparty/tl/) includes a copy of [TartanLlama's expected](https://github.com/TartanLlama/expected) which is released under the [CC0-1.0 (Public doamin) license](http://creativecommons.org/publicdomain/zero/1.0/)
 
 > [!WARNING]
 > I _am not_ testing this repo outside of ROS2.   I am *only* checking [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2) in a ROS2 / colcon environment.
 
-
 -----
 -----
 
diff --git a/Thirdparty/tl/expected.hpp b/Thirdparty/tl/expected.hpp
new file mode 100644
index 00000000000..59e59aa1bd1
--- /dev/null
+++ b/Thirdparty/tl/expected.hpp
@@ -0,0 +1,2475 @@
+///
+// expected - An implementation of std::expected with extensions
+// Written in 2017 by Sy Brand (tartanllama@gmail.com, @TartanLlama)
+//
+// Documentation available at http://tl.tartanllama.xyz/
+//
+// To the extent possible under law, the author(s) have dedicated all
+// copyright and related and neighboring rights to this software to the
+// public domain worldwide. This software is distributed without any warranty.
+//
+// You should have received a copy of the CC0 Public Domain Dedication
+// along with this software. If not, see
+// <http://creativecommons.org/publicdomain/zero/1.0/>.
+///
+
+#ifndef TL_EXPECTED_HPP
+#define TL_EXPECTED_HPP
+
+#define TL_EXPECTED_VERSION_MAJOR 1
+#define TL_EXPECTED_VERSION_MINOR 3
+#define TL_EXPECTED_VERSION_PATCH 1
+
+#include <exception>
+#include <functional>
+#include <type_traits>
+#include <utility>
+
+#if defined(__EXCEPTIONS) || defined(_CPPUNWIND)
+#define TL_EXPECTED_EXCEPTIONS_ENABLED
+#endif
+
+#if (defined(_MSC_VER) && _MSC_VER == 1900)
+#define TL_EXPECTED_MSVC2015
+#define TL_EXPECTED_MSVC2015_CONSTEXPR
+#else
+#define TL_EXPECTED_MSVC2015_CONSTEXPR constexpr
+#endif
+
+#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 &&              \
+     !defined(__clang__))
+#define TL_EXPECTED_GCC49
+#endif
+
+#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 4 &&              \
+     !defined(__clang__))
+#define TL_EXPECTED_GCC54
+#endif
+
+#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 5 &&              \
+     !defined(__clang__))
+#define TL_EXPECTED_GCC55
+#endif
+
+#ifdef _MSVC_LANG
+#define TL_CPLUSPLUS _MSVC_LANG
+#else
+#define TL_CPLUSPLUS __cplusplus
+#endif
+
+#if !defined(TL_ASSERT)
+//can't have assert in constexpr in C++11 and GCC 4.9 has a compiler bug
+#if (TL_CPLUSPLUS > 201103L) && !defined(TL_EXPECTED_GCC49)
+#include <cassert>
+#define TL_ASSERT(x) assert(x)
+#else 
+#define TL_ASSERT(x)
+#endif
+#endif
+
+#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 &&              \
+     !defined(__clang__))
+// GCC < 5 doesn't support overloading on const&& for member functions
+
+#define TL_EXPECTED_NO_CONSTRR
+// GCC < 5 doesn't support some standard C++11 type traits
+#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T)                         \
+  std::has_trivial_copy_constructor<T>
+#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T)                            \
+  std::has_trivial_copy_assign<T>
+
+// This one will be different for GCC 5.7 if it's ever supported
+#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T)                               \
+  std::is_trivially_destructible<T>
+
+// GCC 5 < v < 8 has a bug in is_trivially_copy_constructible which breaks
+// std::vector for non-copyable types
+#elif (defined(__GNUC__) && __GNUC__ < 8 && !defined(__clang__))
+#ifndef TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX
+#define TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX
+namespace tl {
+namespace detail {
+template <class T>
+struct is_trivially_copy_constructible
+    : std::is_trivially_copy_constructible<T> {};
+#ifdef _GLIBCXX_VECTOR
+template <class T, class A>
+struct is_trivially_copy_constructible<std::vector<T, A>> : std::false_type {};
+#endif
+} // namespace detail
+} // namespace tl
+#endif
+
+#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T)                         \
+  tl::detail::is_trivially_copy_constructible<T>
+#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T)                            \
+  std::is_trivially_copy_assignable<T>
+#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T)                               \
+  std::is_trivially_destructible<T>
+#else
+#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T)                         \
+  std::is_trivially_copy_constructible<T>
+#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T)                            \
+  std::is_trivially_copy_assignable<T>
+#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T)                               \
+  std::is_trivially_destructible<T>
+#endif
+
+#if TL_CPLUSPLUS > 201103L
+#define TL_EXPECTED_CXX14
+#endif
+
+#ifdef TL_EXPECTED_GCC49
+#define TL_EXPECTED_GCC49_CONSTEXPR
+#else
+#define TL_EXPECTED_GCC49_CONSTEXPR constexpr
+#endif
+
+#if (TL_CPLUSPLUS == 201103L || defined(TL_EXPECTED_MSVC2015) ||                \
+     defined(TL_EXPECTED_GCC49))
+#define TL_EXPECTED_11_CONSTEXPR
+#else
+#define TL_EXPECTED_11_CONSTEXPR constexpr
+#endif
+
+#if TL_CPLUSPLUS >= 201703L 
+#define TL_EXPECTED_NODISCARD [[nodiscard]]
+#else
+#define TL_EXPECTED_NODISCARD
+#endif
+
+namespace tl {
+template <class T, class E> class TL_EXPECTED_NODISCARD expected;
+
+#ifndef TL_MONOSTATE_INPLACE_MUTEX
+#define TL_MONOSTATE_INPLACE_MUTEX
+class monostate {};
+
+struct in_place_t {
+  explicit in_place_t() = default;
+};
+static constexpr in_place_t in_place{};
+#endif
+
+template <class E> class unexpected {
+public:
+  static_assert(!std::is_same<E, void>::value, "E must not be void");
+
+  unexpected() = delete;
+  constexpr explicit unexpected(const E &e) : m_val(e) {}
+
+  constexpr explicit unexpected(E &&e) : m_val(std::move(e)) {}
+
+  template <class... Args, typename std::enable_if<std::is_constructible<
+                               E, Args &&...>::value>::type * = nullptr>
+  constexpr explicit unexpected(Args &&...args)
+      : m_val(std::forward<Args>(args)...) {}
+  template <
+      class U, class... Args,
+      typename std::enable_if<std::is_constructible<
+          E, std::initializer_list<U> &, Args &&...>::value>::type * = nullptr>
+  constexpr explicit unexpected(std::initializer_list<U> l, Args &&...args)
+      : m_val(l, std::forward<Args>(args)...) {}
+
+  constexpr const E &value() const & { return m_val; }
+  TL_EXPECTED_11_CONSTEXPR E &value() & { return m_val; }
+  TL_EXPECTED_11_CONSTEXPR E &&value() && { return std::move(m_val); }
+  constexpr const E &&value() const && { return std::move(m_val); }
+
+private:
+  E m_val;
+};
+
+#ifdef __cpp_deduction_guides
+template <class E> unexpected(E) -> unexpected<E>;
+#endif
+
+template <class E>
+constexpr bool operator==(const unexpected<E> &lhs, const unexpected<E> &rhs) {
+  return lhs.value() == rhs.value();
+}
+template <class E>
+constexpr bool operator!=(const unexpected<E> &lhs, const unexpected<E> &rhs) {
+  return lhs.value() != rhs.value();
+}
+template <class E>
+constexpr bool operator<(const unexpected<E> &lhs, const unexpected<E> &rhs) {
+  return lhs.value() < rhs.value();
+}
+template <class E>
+constexpr bool operator<=(const unexpected<E> &lhs, const unexpected<E> &rhs) {
+  return lhs.value() <= rhs.value();
+}
+template <class E>
+constexpr bool operator>(const unexpected<E> &lhs, const unexpected<E> &rhs) {
+  return lhs.value() > rhs.value();
+}
+template <class E>
+constexpr bool operator>=(const unexpected<E> &lhs, const unexpected<E> &rhs) {
+  return lhs.value() >= rhs.value();
+}
+
+template <class E>
+unexpected<typename std::decay<E>::type> make_unexpected(E &&e) {
+  return unexpected<typename std::decay<E>::type>(std::forward<E>(e));
+}
+
+struct unexpect_t {
+  unexpect_t() = default;
+};
+static constexpr unexpect_t unexpect{};
+
+#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED
+#define TL_EXPECTED_THROW_EXCEPTION(e) throw((e));
+#else
+#define TL_EXPECTED_THROW_EXCEPTION(e) std::terminate();
+#endif
+
+namespace detail {
+#ifndef TL_TRAITS_MUTEX
+#define TL_TRAITS_MUTEX
+// C++14-style aliases for brevity
+template <class T> using remove_const_t = typename std::remove_const<T>::type;
+template <class T>
+using remove_reference_t = typename std::remove_reference<T>::type;
+template <class T> using decay_t = typename std::decay<T>::type;
+template <bool E, class T = void>
+using enable_if_t = typename std::enable_if<E, T>::type;
+template <bool B, class T, class F>
+using conditional_t = typename std::conditional<B, T, F>::type;
+
+// std::conjunction from C++17
+template <class...> struct conjunction : std::true_type {};
+template <class B> struct conjunction<B> : B {};
+template <class B, class... Bs>
+struct conjunction<B, Bs...>
+    : std::conditional<bool(B::value), conjunction<Bs...>, B>::type {};
+
+#if defined(_LIBCPP_VERSION) && __cplusplus == 201103L
+#define TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND
+#endif
+
+// In C++11 mode, there's an issue in libc++'s std::mem_fn
+// which results in a hard-error when using it in a noexcept expression
+// in some cases. This is a check to workaround the common failing case.
+#ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND
+template <class T>
+struct is_pointer_to_non_const_member_func : std::false_type {};
+template <class T, class Ret, class... Args>
+struct is_pointer_to_non_const_member_func<Ret (T::*)(Args...)>
+    : std::true_type {};
+template <class T, class Ret, class... Args>
+struct is_pointer_to_non_const_member_func<Ret (T::*)(Args...) &>
+    : std::true_type {};
+template <class T, class Ret, class... Args>
+struct is_pointer_to_non_const_member_func<Ret (T::*)(Args...) &&>
+    : std::true_type {};
+template <class T, class Ret, class... Args>
+struct is_pointer_to_non_const_member_func<Ret (T::*)(Args...) volatile>
+    : std::true_type {};
+template <class T, class Ret, class... Args>
+struct is_pointer_to_non_const_member_func<Ret (T::*)(Args...) volatile &>
+    : std::true_type {};
+template <class T, class Ret, class... Args>
+struct is_pointer_to_non_const_member_func<Ret (T::*)(Args...) volatile &&>
+    : std::true_type {};
+
+template <class T> struct is_const_or_const_ref : std::false_type {};
+template <class T> struct is_const_or_const_ref<T const &> : std::true_type {};
+template <class T> struct is_const_or_const_ref<T const> : std::true_type {};
+#endif
+
+// std::invoke from C++17
+// https://stackoverflow.com/questions/38288042/c11-14-invoke-workaround
+template <
+    typename Fn, typename... Args,
+#ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND
+    typename = enable_if_t<!(is_pointer_to_non_const_member_func<Fn>::value &&
+                             is_const_or_const_ref<Args...>::value)>,
+#endif
+    typename = enable_if_t<std::is_member_pointer<decay_t<Fn>>::value>, int = 0>
+constexpr auto invoke(Fn &&f, Args &&...args) noexcept(
+    noexcept(std::mem_fn(f)(std::forward<Args>(args)...)))
+    -> decltype(std::mem_fn(f)(std::forward<Args>(args)...)) {
+  return std::mem_fn(f)(std::forward<Args>(args)...);
+}
+
+template <typename Fn, typename... Args,
+          typename = enable_if_t<!std::is_member_pointer<decay_t<Fn>>::value>>
+constexpr auto invoke(Fn &&f, Args &&...args) noexcept(
+    noexcept(std::forward<Fn>(f)(std::forward<Args>(args)...)))
+    -> decltype(std::forward<Fn>(f)(std::forward<Args>(args)...)) {
+  return std::forward<Fn>(f)(std::forward<Args>(args)...);
+}
+
+// std::invoke_result from C++17
+template <class F, class, class... Us> struct invoke_result_impl;
+
+template <class F, class... Us>
+struct invoke_result_impl<
+    F,
+    decltype(detail::invoke(std::declval<F>(), std::declval<Us>()...), void()),
+    Us...> {
+  using type =
+      decltype(detail::invoke(std::declval<F>(), std::declval<Us>()...));
+};
+
+template <class F, class... Us>
+using invoke_result = invoke_result_impl<F, void, Us...>;
+
+template <class F, class... Us>
+using invoke_result_t = typename invoke_result<F, Us...>::type;
+
+#if defined(_MSC_VER) && _MSC_VER <= 1900
+// TODO make a version which works with MSVC 2015
+template <class T, class U = T> struct is_swappable : std::true_type {};
+
+template <class T, class U = T> struct is_nothrow_swappable : std::true_type {};
+#else
+// https://stackoverflow.com/questions/26744589/what-is-a-proper-way-to-implement-is-swappable-to-test-for-the-swappable-concept
+namespace swap_adl_tests {
+// if swap ADL finds this then it would call std::swap otherwise (same
+// signature)
+struct tag {};
+
+template <class T> tag swap(T &, T &);
+template <class T, std::size_t N> tag swap(T (&a)[N], T (&b)[N]);
+
+// helper functions to test if an unqualified swap is possible, and if it
+// becomes std::swap
+template <class, class> std::false_type can_swap(...) noexcept(false);
+template <class T, class U,
+          class = decltype(swap(std::declval<T &>(), std::declval<U &>()))>
+std::true_type can_swap(int) noexcept(noexcept(swap(std::declval<T &>(),
+                                                    std::declval<U &>())));
+
+template <class, class> std::false_type uses_std(...);
+template <class T, class U>
+std::is_same<decltype(swap(std::declval<T &>(), std::declval<U &>())), tag>
+uses_std(int);
+
+template <class T>
+struct is_std_swap_noexcept
+    : std::integral_constant<bool,
+                             std::is_nothrow_move_constructible<T>::value &&
+                                 std::is_nothrow_move_assignable<T>::value> {};
+
+template <class T, std::size_t N>
+struct is_std_swap_noexcept<T[N]> : is_std_swap_noexcept<T> {};
+
+template <class T, class U>
+struct is_adl_swap_noexcept
+    : std::integral_constant<bool, noexcept(can_swap<T, U>(0))> {};
+} // namespace swap_adl_tests
+
+template <class T, class U = T>
+struct is_swappable
+    : std::integral_constant<
+          bool,
+          decltype(detail::swap_adl_tests::can_swap<T, U>(0))::value &&
+              (!decltype(detail::swap_adl_tests::uses_std<T, U>(0))::value ||
+               (std::is_move_assignable<T>::value &&
+                std::is_move_constructible<T>::value))> {};
+
+template <class T, std::size_t N>
+struct is_swappable<T[N], T[N]>
+    : std::integral_constant<
+          bool,
+          decltype(detail::swap_adl_tests::can_swap<T[N], T[N]>(0))::value &&
+              (!decltype(detail::swap_adl_tests::uses_std<T[N], T[N]>(
+                   0))::value ||
+               is_swappable<T, T>::value)> {};
+
+template <class T, class U = T>
+struct is_nothrow_swappable
+    : std::integral_constant<
+          bool,
+          is_swappable<T, U>::value &&
+              ((decltype(detail::swap_adl_tests::uses_std<T, U>(0))::value &&
+                detail::swap_adl_tests::is_std_swap_noexcept<T>::value) ||
+               (!decltype(detail::swap_adl_tests::uses_std<T, U>(0))::value &&
+                detail::swap_adl_tests::is_adl_swap_noexcept<T, U>::value))> {};
+#endif
+#endif
+
+// Trait for checking if a type is a tl::expected
+template <class T> struct is_expected_impl : std::false_type {};
+template <class T, class E>
+struct is_expected_impl<expected<T, E>> : std::true_type {};
+template <class T> using is_expected = is_expected_impl<decay_t<T>>;
+
+template <class T, class E, class U>
+using expected_enable_forward_value = detail::enable_if_t<
+    std::is_constructible<T, U &&>::value &&
+    !std::is_same<detail::decay_t<U>, in_place_t>::value &&
+    !std::is_same<expected<T, E>, detail::decay_t<U>>::value &&
+    !std::is_same<unexpected<E>, detail::decay_t<U>>::value>;
+
+template <class T, class E, class U, class G, class UR, class GR>
+using expected_enable_from_other = detail::enable_if_t<
+    std::is_constructible<T, UR>::value &&
+    std::is_constructible<E, GR>::value &&
+    !std::is_constructible<T, expected<U, G> &>::value &&
+    !std::is_constructible<T, expected<U, G> &&>::value &&
+    !std::is_constructible<T, const expected<U, G> &>::value &&
+    !std::is_constructible<T, const expected<U, G> &&>::value &&
+    !std::is_convertible<expected<U, G> &, T>::value &&
+    !std::is_convertible<expected<U, G> &&, T>::value &&
+    !std::is_convertible<const expected<U, G> &, T>::value &&
+    !std::is_convertible<const expected<U, G> &&, T>::value>;
+
+template <class T, class U>
+using is_void_or = conditional_t<std::is_void<T>::value, std::true_type, U>;
+
+template <class T>
+using is_copy_constructible_or_void =
+    is_void_or<T, std::is_copy_constructible<T>>;
+
+template <class T>
+using is_move_constructible_or_void =
+    is_void_or<T, std::is_move_constructible<T>>;
+
+template <class T>
+using is_copy_assignable_or_void = is_void_or<T, std::is_copy_assignable<T>>;
+
+template <class T>
+using is_move_assignable_or_void = is_void_or<T, std::is_move_assignable<T>>;
+
+} // namespace detail
+
+namespace detail {
+struct no_init_t {};
+static constexpr no_init_t no_init{};
+
+// Implements the storage of the values, and ensures that the destructor is
+// trivial if it can be.
+//
+// This specialization is for where neither `T` or `E` is trivially
+// destructible, so the destructors must be called on destruction of the
+// `expected`
+template <class T, class E, bool = std::is_trivially_destructible<T>::value,
+          bool = std::is_trivially_destructible<E>::value>
+struct expected_storage_base {
+  constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {}
+  constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {}
+
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<T, Args &&...>::value> * =
+                nullptr>
+  constexpr expected_storage_base(in_place_t, Args &&...args)
+      : m_val(std::forward<Args>(args)...), m_has_val(true) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                T, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr expected_storage_base(in_place_t, std::initializer_list<U> il,
+                                  Args &&...args)
+      : m_val(il, std::forward<Args>(args)...), m_has_val(true) {}
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<E, Args &&...>::value> * =
+                nullptr>
+  constexpr explicit expected_storage_base(unexpect_t, Args &&...args)
+      : m_unexpect(std::forward<Args>(args)...), m_has_val(false) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                E, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr explicit expected_storage_base(unexpect_t,
+                                           std::initializer_list<U> il,
+                                           Args &&...args)
+      : m_unexpect(il, std::forward<Args>(args)...), m_has_val(false) {}
+
+  ~expected_storage_base() {
+    if (m_has_val) {
+      m_val.~T();
+    } else {
+      m_unexpect.~unexpected<E>();
+    }
+  }
+  union {
+    T m_val;
+    unexpected<E> m_unexpect;
+    char m_no_init;
+  };
+  bool m_has_val;
+};
+
+// This specialization is for when both `T` and `E` are trivially-destructible,
+// so the destructor of the `expected` can be trivial.
+template <class T, class E> struct expected_storage_base<T, E, true, true> {
+  constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {}
+  constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {}
+
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<T, Args &&...>::value> * =
+                nullptr>
+  constexpr expected_storage_base(in_place_t, Args &&...args)
+      : m_val(std::forward<Args>(args)...), m_has_val(true) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                T, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr expected_storage_base(in_place_t, std::initializer_list<U> il,
+                                  Args &&...args)
+      : m_val(il, std::forward<Args>(args)...), m_has_val(true) {}
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<E, Args &&...>::value> * =
+                nullptr>
+  constexpr explicit expected_storage_base(unexpect_t, Args &&...args)
+      : m_unexpect(std::forward<Args>(args)...), m_has_val(false) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                E, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr explicit expected_storage_base(unexpect_t,
+                                           std::initializer_list<U> il,
+                                           Args &&...args)
+      : m_unexpect(il, std::forward<Args>(args)...), m_has_val(false) {}
+
+  expected_storage_base(const expected_storage_base &) = default;     
+  expected_storage_base(expected_storage_base &&) = default;
+  expected_storage_base &operator=(const expected_storage_base &) = default;
+  expected_storage_base &operator=(expected_storage_base &&) = default;
+  ~expected_storage_base() = default;
+  union {
+    T m_val;
+    unexpected<E> m_unexpect;
+    char m_no_init;
+  };
+  bool m_has_val;
+};
+
+// T is trivial, E is not.
+template <class T, class E> struct expected_storage_base<T, E, true, false> {
+  constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {}
+  TL_EXPECTED_MSVC2015_CONSTEXPR expected_storage_base(no_init_t)
+      : m_no_init(), m_has_val(false) {}
+
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<T, Args &&...>::value> * =
+                nullptr>
+  constexpr expected_storage_base(in_place_t, Args &&...args)
+      : m_val(std::forward<Args>(args)...), m_has_val(true) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                T, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr expected_storage_base(in_place_t, std::initializer_list<U> il,
+                                  Args &&...args)
+      : m_val(il, std::forward<Args>(args)...), m_has_val(true) {}
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<E, Args &&...>::value> * =
+                nullptr>
+  constexpr explicit expected_storage_base(unexpect_t, Args &&...args)
+      : m_unexpect(std::forward<Args>(args)...), m_has_val(false) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                E, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr explicit expected_storage_base(unexpect_t,
+                                           std::initializer_list<U> il,
+                                           Args &&...args)
+      : m_unexpect(il, std::forward<Args>(args)...), m_has_val(false) {}
+
+  expected_storage_base(const expected_storage_base &) = default;
+  expected_storage_base(expected_storage_base &&) = default;
+  expected_storage_base &operator=(const expected_storage_base &) = default;
+  expected_storage_base &operator=(expected_storage_base &&) = default;
+  ~expected_storage_base() {
+    if (!m_has_val) {
+      m_unexpect.~unexpected<E>();
+    }
+  }
+
+  union {
+    T m_val;
+    unexpected<E> m_unexpect;
+    char m_no_init;
+  };
+  bool m_has_val;
+};
+
+// E is trivial, T is not.
+template <class T, class E> struct expected_storage_base<T, E, false, true> {
+  constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {}
+  constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {}
+
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<T, Args &&...>::value> * =
+                nullptr>
+  constexpr expected_storage_base(in_place_t, Args &&...args)
+      : m_val(std::forward<Args>(args)...), m_has_val(true) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                T, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr expected_storage_base(in_place_t, std::initializer_list<U> il,
+                                  Args &&...args)
+      : m_val(il, std::forward<Args>(args)...), m_has_val(true) {}
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<E, Args &&...>::value> * =
+                nullptr>
+  constexpr explicit expected_storage_base(unexpect_t, Args &&...args)
+      : m_unexpect(std::forward<Args>(args)...), m_has_val(false) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                E, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr explicit expected_storage_base(unexpect_t,
+                                           std::initializer_list<U> il,
+                                           Args &&...args)
+      : m_unexpect(il, std::forward<Args>(args)...), m_has_val(false) {}
+
+  expected_storage_base(const expected_storage_base &) = default;
+  expected_storage_base(expected_storage_base &&) = default;
+  expected_storage_base &operator=(const expected_storage_base &) = default;
+  expected_storage_base &operator=(expected_storage_base &&) = default;
+  ~expected_storage_base() {
+    if (m_has_val) {
+      m_val.~T();
+    }
+  }
+  union {
+    T m_val;
+    unexpected<E> m_unexpect;
+    char m_no_init;
+  };
+  bool m_has_val;
+};
+
+// `T` is `void`, `E` is trivially-destructible
+template <class E> struct expected_storage_base<void, E, false, true> {
+  #if __GNUC__ <= 5
+  //no constexpr for GCC 4/5 bug
+  #else
+  TL_EXPECTED_MSVC2015_CONSTEXPR
+  #endif 
+  expected_storage_base() : m_has_val(true) {}
+     
+  constexpr expected_storage_base(no_init_t) : m_val(), m_has_val(false) {}
+
+  constexpr expected_storage_base(in_place_t) : m_has_val(true) {}
+
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<E, Args &&...>::value> * =
+                nullptr>
+  constexpr explicit expected_storage_base(unexpect_t, Args &&...args)
+      : m_unexpect(std::forward<Args>(args)...), m_has_val(false) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                E, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr explicit expected_storage_base(unexpect_t,
+                                           std::initializer_list<U> il,
+                                           Args &&...args)
+      : m_unexpect(il, std::forward<Args>(args)...), m_has_val(false) {}
+
+  expected_storage_base(const expected_storage_base &) = default;
+  expected_storage_base(expected_storage_base &&) = default;
+  expected_storage_base &operator=(const expected_storage_base &) = default;
+  expected_storage_base &operator=(expected_storage_base &&) = default;
+  ~expected_storage_base() = default;
+  struct dummy {};
+  union {
+    unexpected<E> m_unexpect;
+    dummy m_val;
+  };
+  bool m_has_val;
+};
+
+// `T` is `void`, `E` is not trivially-destructible
+template <class E> struct expected_storage_base<void, E, false, false> {
+  constexpr expected_storage_base() : m_dummy(), m_has_val(true) {}
+  constexpr expected_storage_base(no_init_t) : m_dummy(), m_has_val(false) {}
+
+  constexpr expected_storage_base(in_place_t) : m_dummy(), m_has_val(true) {}
+
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<E, Args &&...>::value> * =
+                nullptr>
+  constexpr explicit expected_storage_base(unexpect_t, Args &&...args)
+      : m_unexpect(std::forward<Args>(args)...), m_has_val(false) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                E, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr explicit expected_storage_base(unexpect_t,
+                                           std::initializer_list<U> il,
+                                           Args &&...args)
+      : m_unexpect(il, std::forward<Args>(args)...), m_has_val(false) {}
+
+  expected_storage_base(const expected_storage_base &) = default;
+  expected_storage_base(expected_storage_base &&) = default;
+  expected_storage_base &operator=(const expected_storage_base &) = default;
+  expected_storage_base &operator=(expected_storage_base &&) = default;
+  ~expected_storage_base() {
+    if (!m_has_val) {
+      m_unexpect.~unexpected<E>();
+    }
+  }
+
+  union {
+    unexpected<E> m_unexpect;
+    char m_dummy;
+  };
+  bool m_has_val;
+};
+
+// This base class provides some handy member functions which can be used in
+// further derived classes
+template <class T, class E>
+struct expected_operations_base : expected_storage_base<T, E> {
+  using expected_storage_base<T, E>::expected_storage_base;
+
+  template <class... Args> void construct(Args &&...args) noexcept {
+    new (std::addressof(this->m_val)) T(std::forward<Args>(args)...);
+    this->m_has_val = true;
+  }
+
+  template <class Rhs> void construct_with(Rhs &&rhs) noexcept {
+    new (std::addressof(this->m_val)) T(std::forward<Rhs>(rhs).get());
+    this->m_has_val = true;
+  }
+
+  template <class... Args> void construct_error(Args &&...args) noexcept {
+    new (std::addressof(this->m_unexpect))
+        unexpected<E>(std::forward<Args>(args)...);
+    this->m_has_val = false;
+  }
+
+#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED
+
+  // These assign overloads ensure that the most efficient assignment
+  // implementation is used while maintaining the strong exception guarantee.
+  // The problematic case is where rhs has a value, but *this does not.
+  //
+  // This overload handles the case where we can just copy-construct `T`
+  // directly into place without throwing.
+  template <class U = T,
+            detail::enable_if_t<std::is_nothrow_copy_constructible<U>::value>
+                * = nullptr>
+  void assign(const expected_operations_base &rhs) noexcept {
+    if (!this->m_has_val && rhs.m_has_val) {
+      geterr().~unexpected<E>();
+      construct(rhs.get());
+    } else {
+      assign_common(rhs);
+    }
+  }
+
+  // This overload handles the case where we can attempt to create a copy of
+  // `T`, then no-throw move it into place if the copy was successful.
+  template <class U = T,
+            detail::enable_if_t<!std::is_nothrow_copy_constructible<U>::value &&
+                                std::is_nothrow_move_constructible<U>::value>
+                * = nullptr>
+  void assign(const expected_operations_base &rhs) noexcept {
+    if (!this->m_has_val && rhs.m_has_val) {
+      T tmp = rhs.get();
+      geterr().~unexpected<E>();
+      construct(std::move(tmp));
+    } else {
+      assign_common(rhs);
+    }
+  }
+
+  // This overload is the worst-case, where we have to move-construct the
+  // unexpected value into temporary storage, then try to copy the T into place.
+  // If the construction succeeds, then everything is fine, but if it throws,
+  // then we move the old unexpected value back into place before rethrowing the
+  // exception.
+  template <class U = T,
+            detail::enable_if_t<!std::is_nothrow_copy_constructible<U>::value &&
+                                !std::is_nothrow_move_constructible<U>::value>
+                * = nullptr>
+  void assign(const expected_operations_base &rhs) {
+    if (!this->m_has_val && rhs.m_has_val) {
+      auto tmp = std::move(geterr());
+      geterr().~unexpected<E>();
+
+#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED
+      try {
+        construct(rhs.get());
+      } catch (...) {
+        geterr() = std::move(tmp);
+        throw;
+      }
+#else
+      construct(rhs.get());
+#endif
+    } else {
+      assign_common(rhs);
+    }
+  }
+
+  // These overloads do the same as above, but for rvalues
+  template <class U = T,
+            detail::enable_if_t<std::is_nothrow_move_constructible<U>::value>
+                * = nullptr>
+  void assign(expected_operations_base &&rhs) noexcept {
+    if (!this->m_has_val && rhs.m_has_val) {
+      geterr().~unexpected<E>();
+      construct(std::move(rhs).get());
+    } else {
+      assign_common(std::move(rhs));
+    }
+  }
+
+  template <class U = T,
+            detail::enable_if_t<!std::is_nothrow_move_constructible<U>::value>
+                * = nullptr>
+  void assign(expected_operations_base &&rhs) {
+    if (!this->m_has_val && rhs.m_has_val) {
+      auto tmp = std::move(geterr());
+      geterr().~unexpected<E>();
+#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED
+      try {
+        construct(std::move(rhs).get());
+      } catch (...) {
+        geterr() = std::move(tmp);
+        throw;
+      }
+#else
+      construct(std::move(rhs).get());
+#endif
+    } else {
+      assign_common(std::move(rhs));
+    }
+  }
+
+#else
+
+  // If exceptions are disabled then we can just copy-construct
+  void assign(const expected_operations_base &rhs) noexcept {
+    if (!this->m_has_val && rhs.m_has_val) {
+      geterr().~unexpected<E>();
+      construct(rhs.get());
+    } else {
+      assign_common(rhs);
+    }
+  }
+
+  void assign(expected_operations_base &&rhs) noexcept {
+    if (!this->m_has_val && rhs.m_has_val) {
+      geterr().~unexpected<E>();
+      construct(std::move(rhs).get());
+    } else {
+      assign_common(std::move(rhs));
+    }
+  }
+
+#endif
+
+  // The common part of move/copy assigning
+  template <class Rhs> void assign_common(Rhs &&rhs) {
+    if (this->m_has_val) {
+      if (rhs.m_has_val) {
+        get() = std::forward<Rhs>(rhs).get();
+      } else {
+        destroy_val();
+        construct_error(std::forward<Rhs>(rhs).geterr());
+      }
+    } else {
+      if (!rhs.m_has_val) {
+        geterr() = std::forward<Rhs>(rhs).geterr();
+      }
+    }
+  }
+
+  bool has_value() const { return this->m_has_val; }
+
+  TL_EXPECTED_11_CONSTEXPR T &get() & { return this->m_val; }
+  constexpr const T &get() const & { return this->m_val; }
+  TL_EXPECTED_11_CONSTEXPR T &&get() && { return std::move(this->m_val); }
+#ifndef TL_EXPECTED_NO_CONSTRR
+  constexpr const T &&get() const && { return std::move(this->m_val); }
+#endif
+
+  TL_EXPECTED_11_CONSTEXPR unexpected<E> &geterr() & {
+    return this->m_unexpect;
+  }
+  constexpr const unexpected<E> &geterr() const & { return this->m_unexpect; }
+  TL_EXPECTED_11_CONSTEXPR unexpected<E> &&geterr() && {
+    return std::move(this->m_unexpect);
+  }
+#ifndef TL_EXPECTED_NO_CONSTRR
+  constexpr const unexpected<E> &&geterr() const && {
+    return std::move(this->m_unexpect);
+  }
+#endif
+
+  TL_EXPECTED_11_CONSTEXPR void destroy_val() { get().~T(); }
+};
+
+// This base class provides some handy member functions which can be used in
+// further derived classes
+template <class E>
+struct expected_operations_base<void, E> : expected_storage_base<void, E> {
+  using expected_storage_base<void, E>::expected_storage_base;
+
+  template <class... Args> void construct() noexcept { this->m_has_val = true; }
+
+  // This function doesn't use its argument, but needs it so that code in
+  // levels above this can work independently of whether T is void
+  template <class Rhs> void construct_with(Rhs &&) noexcept {
+    this->m_has_val = true;
+  }
+
+  template <class... Args> void construct_error(Args &&...args) noexcept {
+    new (std::addressof(this->m_unexpect))
+        unexpected<E>(std::forward<Args>(args)...);
+    this->m_has_val = false;
+  }
+
+  template <class Rhs> void assign(Rhs &&rhs) noexcept {
+    if (!this->m_has_val) {
+      if (rhs.m_has_val) {
+        geterr().~unexpected<E>();
+        construct();
+      } else {
+        geterr() = std::forward<Rhs>(rhs).geterr();
+      }
+    } else {
+      if (!rhs.m_has_val) {
+        construct_error(std::forward<Rhs>(rhs).geterr());
+      }
+    }
+  }
+
+  bool has_value() const { return this->m_has_val; }
+
+  TL_EXPECTED_11_CONSTEXPR unexpected<E> &geterr() & {
+    return this->m_unexpect;
+  }
+  constexpr const unexpected<E> &geterr() const & { return this->m_unexpect; }
+  TL_EXPECTED_11_CONSTEXPR unexpected<E> &&geterr() && {
+    return std::move(this->m_unexpect);
+  }
+#ifndef TL_EXPECTED_NO_CONSTRR
+  constexpr const unexpected<E> &&geterr() const && {
+    return std::move(this->m_unexpect);
+  }
+#endif
+
+  TL_EXPECTED_11_CONSTEXPR void destroy_val() {
+    // no-op
+  }
+};
+
+// This class manages conditionally having a trivial copy constructor
+// This specialization is for when T and E are trivially copy constructible
+template <class T, class E,
+          bool = is_void_or<T, TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T)>::
+              value &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value,
+          bool = (is_copy_constructible_or_void<T>::value &&
+                  std::is_copy_constructible<E>::value)>
+struct expected_copy_base : expected_operations_base<T, E> {
+  using expected_operations_base<T, E>::expected_operations_base;
+};
+
+// This specialization is for when T or E are non-trivially copy constructible
+template <class T, class E>
+struct expected_copy_base<T, E, false, true> : expected_operations_base<T, E> {
+  using expected_operations_base<T, E>::expected_operations_base;
+
+  expected_copy_base() = default;
+  expected_copy_base(const expected_copy_base &rhs)
+      : expected_operations_base<T, E>(no_init) {
+    if (rhs.has_value()) {
+      this->construct_with(rhs);
+    } else {
+      this->construct_error(rhs.geterr());
+    }
+  }
+
+  expected_copy_base(expected_copy_base &&rhs) = default;
+  expected_copy_base &operator=(const expected_copy_base &rhs) = default;
+  expected_copy_base &operator=(expected_copy_base &&rhs) = default;
+};
+
+// This class manages conditionally having a trivial move constructor
+// Unfortunately there's no way to achieve this in GCC < 5 AFAIK, since it
+// doesn't implement an analogue to std::is_trivially_move_constructible. We
+// have to make do with a non-trivial move constructor even if T is trivially
+// move constructible
+#ifndef TL_EXPECTED_GCC49
+template <class T, class E,
+          bool = is_void_or<T, std::is_trivially_move_constructible<T>>::value
+              &&std::is_trivially_move_constructible<E>::value>
+struct expected_move_base : expected_copy_base<T, E> {
+  using expected_copy_base<T, E>::expected_copy_base;
+};
+#else
+template <class T, class E, bool = false> struct expected_move_base;
+#endif
+template <class T, class E>
+struct expected_move_base<T, E, false> : expected_copy_base<T, E> {
+  using expected_copy_base<T, E>::expected_copy_base;
+
+  expected_move_base() = default;
+  expected_move_base(const expected_move_base &rhs) = default;
+
+  expected_move_base(expected_move_base &&rhs) noexcept(
+      std::is_nothrow_move_constructible<T>::value)
+      : expected_copy_base<T, E>(no_init) {
+    if (rhs.has_value()) {
+      this->construct_with(std::move(rhs));
+    } else {
+      this->construct_error(std::move(rhs.geterr()));
+    }
+  }
+  expected_move_base &operator=(const expected_move_base &rhs) = default;
+  expected_move_base &operator=(expected_move_base &&rhs) = default;
+};
+
+// This class manages conditionally having a trivial copy assignment operator
+template <class T, class E,
+          bool = is_void_or<
+              T, conjunction<TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T),
+                             TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T),
+                             TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T)>>::value
+              &&TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(E)::value
+                  &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value
+                      &&TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(E)::value,
+          bool = (is_copy_constructible_or_void<T>::value &&
+             std::is_copy_constructible<E>::value &&
+             is_copy_assignable_or_void<T>::value &&
+             std::is_copy_assignable<E>::value)>
+struct expected_copy_assign_base : expected_move_base<T, E> {
+  using expected_move_base<T, E>::expected_move_base;
+};
+
+template <class T, class E>
+struct expected_copy_assign_base<T, E, false, true> : expected_move_base<T, E> {
+  using expected_move_base<T, E>::expected_move_base;
+
+  expected_copy_assign_base() = default;
+  expected_copy_assign_base(const expected_copy_assign_base &rhs) = default;
+
+  expected_copy_assign_base(expected_copy_assign_base &&rhs) = default;
+  expected_copy_assign_base &operator=(const expected_copy_assign_base &rhs) {
+    this->assign(rhs);
+    return *this;
+  }
+  expected_copy_assign_base &
+  operator=(expected_copy_assign_base &&rhs) = default;
+};
+
+// This class manages conditionally having a trivial move assignment operator
+// Unfortunately there's no way to achieve this in GCC < 5 AFAIK, since it
+// doesn't implement an analogue to std::is_trivially_move_assignable. We have
+// to make do with a non-trivial move assignment operator even if T is trivially
+// move assignable
+#ifndef TL_EXPECTED_GCC49
+template <class T, class E,
+          bool =
+              is_void_or<T, conjunction<std::is_trivially_destructible<T>,
+                                        std::is_trivially_move_constructible<T>,
+                                        std::is_trivially_move_assignable<T>>>::
+                  value &&std::is_trivially_destructible<E>::value
+                      &&std::is_trivially_move_constructible<E>::value
+                          &&std::is_trivially_move_assignable<E>::value>
+struct expected_move_assign_base : expected_copy_assign_base<T, E> {
+  using expected_copy_assign_base<T, E>::expected_copy_assign_base;
+};
+#else
+template <class T, class E, bool = false> struct expected_move_assign_base;
+#endif
+
+template <class T, class E>
+struct expected_move_assign_base<T, E, false>
+    : expected_copy_assign_base<T, E> {
+  using expected_copy_assign_base<T, E>::expected_copy_assign_base;
+
+  expected_move_assign_base() = default;
+  expected_move_assign_base(const expected_move_assign_base &rhs) = default;
+
+  expected_move_assign_base(expected_move_assign_base &&rhs) = default;
+
+  expected_move_assign_base &
+  operator=(const expected_move_assign_base &rhs) = default;
+
+  expected_move_assign_base &
+  operator=(expected_move_assign_base &&rhs) noexcept(
+      std::is_nothrow_move_constructible<T>::value
+          &&std::is_nothrow_move_assignable<T>::value) {
+    this->assign(std::move(rhs));
+    return *this;
+  }
+};
+
+// expected_delete_ctor_base will conditionally delete copy and move
+// constructors depending on whether T is copy/move constructible
+template <class T, class E,
+          bool EnableCopy = (is_copy_constructible_or_void<T>::value &&
+                             std::is_copy_constructible<E>::value),
+          bool EnableMove = (is_move_constructible_or_void<T>::value &&
+                             std::is_move_constructible<E>::value)>
+struct expected_delete_ctor_base {
+  expected_delete_ctor_base() = default;
+  expected_delete_ctor_base(const expected_delete_ctor_base &) = default;
+  expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default;
+  expected_delete_ctor_base &
+  operator=(const expected_delete_ctor_base &) = default;
+  expected_delete_ctor_base &
+  operator=(expected_delete_ctor_base &&) noexcept = default;
+};
+
+template <class T, class E>
+struct expected_delete_ctor_base<T, E, true, false> {
+  expected_delete_ctor_base() = default;
+  expected_delete_ctor_base(const expected_delete_ctor_base &) = default;
+  expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete;
+  expected_delete_ctor_base &
+  operator=(const expected_delete_ctor_base &) = default;
+  expected_delete_ctor_base &
+  operator=(expected_delete_ctor_base &&) noexcept = default;
+};
+
+template <class T, class E>
+struct expected_delete_ctor_base<T, E, false, true> {
+  expected_delete_ctor_base() = default;
+  expected_delete_ctor_base(const expected_delete_ctor_base &) = delete;
+  expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default;
+  expected_delete_ctor_base &
+  operator=(const expected_delete_ctor_base &) = default;
+  expected_delete_ctor_base &
+  operator=(expected_delete_ctor_base &&) noexcept = default;
+};
+
+template <class T, class E>
+struct expected_delete_ctor_base<T, E, false, false> {
+  expected_delete_ctor_base() = default;
+  expected_delete_ctor_base(const expected_delete_ctor_base &) = delete;
+  expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete;
+  expected_delete_ctor_base &
+  operator=(const expected_delete_ctor_base &) = default;
+  expected_delete_ctor_base &
+  operator=(expected_delete_ctor_base &&) noexcept = default;
+};
+
+// expected_delete_assign_base will conditionally delete copy and move
+// constructors depending on whether T and E are copy/move constructible +
+// assignable
+template <class T, class E,
+          bool EnableCopy = (is_copy_constructible_or_void<T>::value &&
+                             std::is_copy_constructible<E>::value &&
+                             is_copy_assignable_or_void<T>::value &&
+                             std::is_copy_assignable<E>::value),
+          bool EnableMove = (is_move_constructible_or_void<T>::value &&
+                             std::is_move_constructible<E>::value &&
+                             is_move_assignable_or_void<T>::value &&
+                             std::is_move_assignable<E>::value)>
+struct expected_delete_assign_base {
+  expected_delete_assign_base() = default;
+  expected_delete_assign_base(const expected_delete_assign_base &) = default;
+  expected_delete_assign_base(expected_delete_assign_base &&) noexcept =
+      default;
+  expected_delete_assign_base &
+  operator=(const expected_delete_assign_base &) = default;
+  expected_delete_assign_base &
+  operator=(expected_delete_assign_base &&) noexcept = default;
+};
+
+template <class T, class E>
+struct expected_delete_assign_base<T, E, true, false> {
+  expected_delete_assign_base() = default;
+  expected_delete_assign_base(const expected_delete_assign_base &) = default;
+  expected_delete_assign_base(expected_delete_assign_base &&) noexcept =
+      default;
+  expected_delete_assign_base &
+  operator=(const expected_delete_assign_base &) = default;
+  expected_delete_assign_base &
+  operator=(expected_delete_assign_base &&) noexcept = delete;
+};
+
+template <class T, class E>
+struct expected_delete_assign_base<T, E, false, true> {
+  expected_delete_assign_base() = default;
+  expected_delete_assign_base(const expected_delete_assign_base &) = default;
+  expected_delete_assign_base(expected_delete_assign_base &&) noexcept =
+      default;
+  expected_delete_assign_base &
+  operator=(const expected_delete_assign_base &) = delete;
+  expected_delete_assign_base &
+  operator=(expected_delete_assign_base &&) noexcept = default;
+};
+
+template <class T, class E>
+struct expected_delete_assign_base<T, E, false, false> {
+  expected_delete_assign_base() = default;
+  expected_delete_assign_base(const expected_delete_assign_base &) = default;
+  expected_delete_assign_base(expected_delete_assign_base &&) noexcept =
+      default;
+  expected_delete_assign_base &
+  operator=(const expected_delete_assign_base &) = delete;
+  expected_delete_assign_base &
+  operator=(expected_delete_assign_base &&) noexcept = delete;
+};
+
+// This is needed to be able to construct the expected_default_ctor_base which
+// follows, while still conditionally deleting the default constructor.
+struct default_constructor_tag {
+  explicit constexpr default_constructor_tag() = default;
+};
+
+// expected_default_ctor_base will ensure that expected has a deleted default
+// constructor if T is not default constructible.
+// This specialization is for when T is default constructible
+template <class T, class E,
+          bool Enable =
+              std::is_default_constructible<T>::value || std::is_void<T>::value>
+struct expected_default_ctor_base {
+  constexpr expected_default_ctor_base() noexcept = default;
+  constexpr expected_default_ctor_base(
+      expected_default_ctor_base const &) noexcept = default;
+  constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept =
+      default;
+  expected_default_ctor_base &
+  operator=(expected_default_ctor_base const &) noexcept = default;
+  expected_default_ctor_base &
+  operator=(expected_default_ctor_base &&) noexcept = default;
+
+  constexpr explicit expected_default_ctor_base(default_constructor_tag) {}
+};
+
+// This specialization is for when T is not default constructible
+template <class T, class E> struct expected_default_ctor_base<T, E, false> {
+  constexpr expected_default_ctor_base() noexcept = delete;
+  constexpr expected_default_ctor_base(
+      expected_default_ctor_base const &) noexcept = default;
+  constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept =
+      default;
+  expected_default_ctor_base &
+  operator=(expected_default_ctor_base const &) noexcept = default;
+  expected_default_ctor_base &
+  operator=(expected_default_ctor_base &&) noexcept = default;
+
+  constexpr explicit expected_default_ctor_base(default_constructor_tag) {}
+};
+} // namespace detail
+
+template <class E> class bad_expected_access : public std::exception {
+public:
+  explicit bad_expected_access(E e) : m_val(std::move(e)) {}
+
+  virtual const char *what() const noexcept override {
+    return "Bad expected access";
+  }
+
+  const E &error() const & { return m_val; }
+  E &error() & { return m_val; }
+  const E &&error() const && { return std::move(m_val); }
+  E &&error() && { return std::move(m_val); }
+
+private:
+  E m_val;
+};
+
+/// An `expected<T, E>` object is an object that contains the storage for
+/// another object and manages the lifetime of this contained object `T`.
+/// Alternatively it could contain the storage for another unexpected object
+/// `E`. The contained object may not be initialized after the expected object
+/// has been initialized, and may not be destroyed before the expected object
+/// has been destroyed. The initialization state of the contained object is
+/// tracked by the expected object.
+template <class T, class E>
+class TL_EXPECTED_NODISCARD expected :
+                 private detail::expected_move_assign_base<T, E>,
+                 private detail::expected_delete_ctor_base<T, E>,
+                 private detail::expected_delete_assign_base<T, E>,
+                 private detail::expected_default_ctor_base<T, E> {
+  static_assert(!std::is_reference<T>::value, "T must not be a reference");
+  static_assert(!std::is_same<T, std::remove_cv<in_place_t>::type>::value,
+                "T must not be in_place_t");
+  static_assert(!std::is_same<T, std::remove_cv<unexpect_t>::type>::value,
+                "T must not be unexpect_t");
+  static_assert(
+      !std::is_same<T, typename std::remove_cv<unexpected<E>>::type>::value,
+      "T must not be unexpected<E>");
+  static_assert(!std::is_reference<E>::value, "E must not be a reference");
+
+  T *valptr() { return std::addressof(this->m_val); }
+  const T *valptr() const { return std::addressof(this->m_val); }
+  unexpected<E> *errptr() { return std::addressof(this->m_unexpect); }
+  const unexpected<E> *errptr() const {
+    return std::addressof(this->m_unexpect);
+  }
+
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  TL_EXPECTED_11_CONSTEXPR U &val() {
+    return this->m_val;
+  }
+  TL_EXPECTED_11_CONSTEXPR unexpected<E> &err() { return this->m_unexpect; }
+
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  constexpr const U &val() const {
+    return this->m_val;
+  }
+  constexpr const unexpected<E> &err() const { return this->m_unexpect; }
+
+  using impl_base = detail::expected_move_assign_base<T, E>;
+  using ctor_base = detail::expected_default_ctor_base<T, E>;
+
+public:
+  typedef T value_type;
+  typedef E error_type;
+  typedef unexpected<E> unexpected_type;
+
+#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) &&               \
+    !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55)
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) & {
+    return and_then_impl(*this, std::forward<F>(f));
+  }
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) && {
+    return and_then_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F> constexpr auto and_then(F &&f) const & {
+    return and_then_impl(*this, std::forward<F>(f));
+  }
+
+#ifndef TL_EXPECTED_NO_CONSTRR
+  template <class F> constexpr auto and_then(F &&f) const && {
+    return and_then_impl(std::move(*this), std::forward<F>(f));
+  }
+#endif
+
+#else
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR auto
+  and_then(F &&f) & -> decltype(and_then_impl(std::declval<expected &>(),
+                                              std::forward<F>(f))) {
+    return and_then_impl(*this, std::forward<F>(f));
+  }
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR auto
+  and_then(F &&f) && -> decltype(and_then_impl(std::declval<expected &&>(),
+                                               std::forward<F>(f))) {
+    return and_then_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F>
+  constexpr auto and_then(F &&f) const & -> decltype(and_then_impl(
+      std::declval<expected const &>(), std::forward<F>(f))) {
+    return and_then_impl(*this, std::forward<F>(f));
+  }
+
+#ifndef TL_EXPECTED_NO_CONSTRR
+  template <class F>
+  constexpr auto and_then(F &&f) const && -> decltype(and_then_impl(
+      std::declval<expected const &&>(), std::forward<F>(f))) {
+    return and_then_impl(std::move(*this), std::forward<F>(f));
+  }
+#endif
+#endif
+
+#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) &&               \
+    !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55)
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto map(F &&f) & {
+    return expected_map_impl(*this, std::forward<F>(f));
+  }
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto map(F &&f) && {
+    return expected_map_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F> constexpr auto map(F &&f) const & {
+    return expected_map_impl(*this, std::forward<F>(f));
+  }
+  template <class F> constexpr auto map(F &&f) const && {
+    return expected_map_impl(std::move(*this), std::forward<F>(f));
+  }
+#else
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(
+      std::declval<expected &>(), std::declval<F &&>()))
+  map(F &&f) & {
+    return expected_map_impl(*this, std::forward<F>(f));
+  }
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval<expected>(),
+                                                      std::declval<F &&>()))
+  map(F &&f) && {
+    return expected_map_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F>
+  constexpr decltype(expected_map_impl(std::declval<const expected &>(),
+                                       std::declval<F &&>()))
+  map(F &&f) const & {
+    return expected_map_impl(*this, std::forward<F>(f));
+  }
+
+#ifndef TL_EXPECTED_NO_CONSTRR
+  template <class F>
+  constexpr decltype(expected_map_impl(std::declval<const expected &&>(),
+                                       std::declval<F &&>()))
+  map(F &&f) const && {
+    return expected_map_impl(std::move(*this), std::forward<F>(f));
+  }
+#endif
+#endif
+
+#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) &&               \
+    !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55)
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) & {
+    return expected_map_impl(*this, std::forward<F>(f));
+  }
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) && {
+    return expected_map_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F> constexpr auto transform(F &&f) const & {
+    return expected_map_impl(*this, std::forward<F>(f));
+  }
+  template <class F> constexpr auto transform(F &&f) const && {
+    return expected_map_impl(std::move(*this), std::forward<F>(f));
+  }
+#else
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(
+      std::declval<expected &>(), std::declval<F &&>()))
+  transform(F &&f) & {
+    return expected_map_impl(*this, std::forward<F>(f));
+  }
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval<expected>(),
+                                                      std::declval<F &&>()))
+  transform(F &&f) && {
+    return expected_map_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F>
+  constexpr decltype(expected_map_impl(std::declval<const expected &>(),
+                                       std::declval<F &&>()))
+  transform(F &&f) const & {
+    return expected_map_impl(*this, std::forward<F>(f));
+  }
+
+#ifndef TL_EXPECTED_NO_CONSTRR
+  template <class F>
+  constexpr decltype(expected_map_impl(std::declval<const expected &&>(),
+                                       std::declval<F &&>()))
+  transform(F &&f) const && {
+    return expected_map_impl(std::move(*this), std::forward<F>(f));
+  }
+#endif
+#endif
+
+#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) &&               \
+    !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55)
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) & {
+    return map_error_impl(*this, std::forward<F>(f));
+  }
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) && {
+    return map_error_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F> constexpr auto map_error(F &&f) const & {
+    return map_error_impl(*this, std::forward<F>(f));
+  }
+  template <class F> constexpr auto map_error(F &&f) const && {
+    return map_error_impl(std::move(*this), std::forward<F>(f));
+  }
+#else
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval<expected &>(),
+                                                   std::declval<F &&>()))
+  map_error(F &&f) & {
+    return map_error_impl(*this, std::forward<F>(f));
+  }
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval<expected &&>(),
+                                                   std::declval<F &&>()))
+  map_error(F &&f) && {
+    return map_error_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F>
+  constexpr decltype(map_error_impl(std::declval<const expected &>(),
+                                    std::declval<F &&>()))
+  map_error(F &&f) const & {
+    return map_error_impl(*this, std::forward<F>(f));
+  }
+
+#ifndef TL_EXPECTED_NO_CONSTRR
+  template <class F>
+  constexpr decltype(map_error_impl(std::declval<const expected &&>(),
+                                    std::declval<F &&>()))
+  map_error(F &&f) const && {
+    return map_error_impl(std::move(*this), std::forward<F>(f));
+  }
+#endif
+#endif
+#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) &&               \
+    !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55)
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) & {
+    return map_error_impl(*this, std::forward<F>(f));
+  }
+  template <class F> TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) && {
+    return map_error_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F> constexpr auto transform_error(F &&f) const & {
+    return map_error_impl(*this, std::forward<F>(f));
+  }
+  template <class F> constexpr auto transform_error(F &&f) const && {
+    return map_error_impl(std::move(*this), std::forward<F>(f));
+  }
+#else
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval<expected &>(),
+                                                   std::declval<F &&>()))
+  transform_error(F &&f) & {
+    return map_error_impl(*this, std::forward<F>(f));
+  }
+  template <class F>
+  TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval<expected &&>(),
+                                                   std::declval<F &&>()))
+  transform_error(F &&f) && {
+    return map_error_impl(std::move(*this), std::forward<F>(f));
+  }
+  template <class F>
+  constexpr decltype(map_error_impl(std::declval<const expected &>(),
+                                    std::declval<F &&>()))
+  transform_error(F &&f) const & {
+    return map_error_impl(*this, std::forward<F>(f));
+  }
+
+#ifndef TL_EXPECTED_NO_CONSTRR
+  template <class F>
+  constexpr decltype(map_error_impl(std::declval<const expected &&>(),
+                                    std::declval<F &&>()))
+  transform_error(F &&f) const && {
+    return map_error_impl(std::move(*this), std::forward<F>(f));
+  }
+#endif
+#endif
+  template <class F> expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) & {
+    return or_else_impl(*this, std::forward<F>(f));
+  }
+
+  template <class F> expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) && {
+    return or_else_impl(std::move(*this), std::forward<F>(f));
+  }
+
+  template <class F> expected constexpr or_else(F &&f) const & {
+    return or_else_impl(*this, std::forward<F>(f));
+  }
+
+#ifndef TL_EXPECTED_NO_CONSTRR
+  template <class F> expected constexpr or_else(F &&f) const && {
+    return or_else_impl(std::move(*this), std::forward<F>(f));
+  }
+#endif
+  constexpr expected() = default;
+  constexpr expected(const expected &rhs) = default;
+  constexpr expected(expected &&rhs) = default;
+  expected &operator=(const expected &rhs) = default;
+  expected &operator=(expected &&rhs) = default;
+
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<T, Args &&...>::value> * =
+                nullptr>
+  constexpr expected(in_place_t, Args &&...args)
+      : impl_base(in_place, std::forward<Args>(args)...),
+        ctor_base(detail::default_constructor_tag{}) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                T, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr expected(in_place_t, std::initializer_list<U> il, Args &&...args)
+      : impl_base(in_place, il, std::forward<Args>(args)...),
+        ctor_base(detail::default_constructor_tag{}) {}
+
+  template <class G = E,
+            detail::enable_if_t<std::is_constructible<E, const G &>::value> * =
+                nullptr,
+            detail::enable_if_t<!std::is_convertible<const G &, E>::value> * =
+                nullptr>
+  explicit constexpr expected(const unexpected<G> &e)
+      : impl_base(unexpect, e.value()),
+        ctor_base(detail::default_constructor_tag{}) {}
+
+  template <
+      class G = E,
+      detail::enable_if_t<std::is_constructible<E, const G &>::value> * =
+          nullptr,
+      detail::enable_if_t<std::is_convertible<const G &, E>::value> * = nullptr>
+  constexpr expected(unexpected<G> const &e)
+      : impl_base(unexpect, e.value()),
+        ctor_base(detail::default_constructor_tag{}) {}
+
+  template <
+      class G = E,
+      detail::enable_if_t<std::is_constructible<E, G &&>::value> * = nullptr,
+      detail::enable_if_t<!std::is_convertible<G &&, E>::value> * = nullptr>
+  explicit constexpr expected(unexpected<G> &&e) noexcept(
+      std::is_nothrow_constructible<E, G &&>::value)
+      : impl_base(unexpect, std::move(e.value())),
+        ctor_base(detail::default_constructor_tag{}) {}
+
+  template <
+      class G = E,
+      detail::enable_if_t<std::is_constructible<E, G &&>::value> * = nullptr,
+      detail::enable_if_t<std::is_convertible<G &&, E>::value> * = nullptr>
+  constexpr expected(unexpected<G> &&e) noexcept(
+      std::is_nothrow_constructible<E, G &&>::value)
+      : impl_base(unexpect, std::move(e.value())),
+        ctor_base(detail::default_constructor_tag{}) {}
+
+  template <class... Args,
+            detail::enable_if_t<std::is_constructible<E, Args &&...>::value> * =
+                nullptr>
+  constexpr explicit expected(unexpect_t, Args &&...args)
+      : impl_base(unexpect, std::forward<Args>(args)...),
+        ctor_base(detail::default_constructor_tag{}) {}
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_constructible<
+                E, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  constexpr explicit expected(unexpect_t, std::initializer_list<U> il,
+                              Args &&...args)
+      : impl_base(unexpect, il, std::forward<Args>(args)...),
+        ctor_base(detail::default_constructor_tag{}) {}
+
+  template <class U, class G,
+            detail::enable_if_t<!(std::is_convertible<U const &, T>::value &&
+                                  std::is_convertible<G const &, E>::value)> * =
+                nullptr,
+            detail::expected_enable_from_other<T, E, U, G, const U &, const G &>
+                * = nullptr>
+  explicit TL_EXPECTED_11_CONSTEXPR expected(const expected<U, G> &rhs)
+      : ctor_base(detail::default_constructor_tag{}) {
+    if (rhs.has_value()) {
+      this->construct(*rhs);
+    } else {
+      this->construct_error(rhs.error());
+    }
+  }
+
+  template <class U, class G,
+            detail::enable_if_t<(std::is_convertible<U const &, T>::value &&
+                                 std::is_convertible<G const &, E>::value)> * =
+                nullptr,
+            detail::expected_enable_from_other<T, E, U, G, const U &, const G &>
+                * = nullptr>
+  TL_EXPECTED_11_CONSTEXPR expected(const expected<U, G> &rhs)
+      : ctor_base(detail::default_constructor_tag{}) {
+    if (rhs.has_value()) {
+      this->construct(*rhs);
+    } else {
+      this->construct_error(rhs.error());
+    }
+  }
+
+  template <
+      class U, class G,
+      detail::enable_if_t<!(std::is_convertible<U &&, T>::value &&
+                            std::is_convertible<G &&, E>::value)> * = nullptr,
+      detail::expected_enable_from_other<T, E, U, G, U &&, G &&> * = nullptr>
+  explicit TL_EXPECTED_11_CONSTEXPR expected(expected<U, G> &&rhs)
+      : ctor_base(detail::default_constructor_tag{}) {
+    if (rhs.has_value()) {
+      this->construct(std::move(*rhs));
+    } else {
+      this->construct_error(std::move(rhs.error()));
+    }
+  }
+
+  template <
+      class U, class G,
+      detail::enable_if_t<(std::is_convertible<U &&, T>::value &&
+                           std::is_convertible<G &&, E>::value)> * = nullptr,
+      detail::expected_enable_from_other<T, E, U, G, U &&, G &&> * = nullptr>
+  TL_EXPECTED_11_CONSTEXPR expected(expected<U, G> &&rhs)
+      : ctor_base(detail::default_constructor_tag{}) {
+    if (rhs.has_value()) {
+      this->construct(std::move(*rhs));
+    } else {
+      this->construct_error(std::move(rhs.error()));
+    }
+  }
+
+  template <
+      class U = T,
+      detail::enable_if_t<!std::is_convertible<U &&, T>::value> * = nullptr,
+      detail::expected_enable_forward_value<T, E, U> * = nullptr>
+  explicit TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v)
+      : expected(in_place, std::forward<U>(v)) {}
+
+  template <
+      class U = T,
+      detail::enable_if_t<std::is_convertible<U &&, T>::value> * = nullptr,
+      detail::expected_enable_forward_value<T, E, U> * = nullptr>
+  TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v)
+      : expected(in_place, std::forward<U>(v)) {}
+
+  template <
+      class U = T, class G = T,
+      detail::enable_if_t<std::is_nothrow_constructible<T, U &&>::value> * =
+          nullptr,
+      detail::enable_if_t<!std::is_void<G>::value> * = nullptr,
+      detail::enable_if_t<
+          (!std::is_same<expected<T, E>, detail::decay_t<U>>::value &&
+           !detail::conjunction<std::is_scalar<T>,
+                                std::is_same<T, detail::decay_t<U>>>::value &&
+           std::is_constructible<T, U>::value &&
+           std::is_assignable<G &, U>::value &&
+           std::is_nothrow_move_constructible<E>::value)> * = nullptr>
+  expected &operator=(U &&v) {
+    if (has_value()) {
+      val() = std::forward<U>(v);
+    } else {
+      err().~unexpected<E>();
+      ::new (valptr()) T(std::forward<U>(v));
+      this->m_has_val = true;
+    }
+
+    return *this;
+  }
+
+  template <
+      class U = T, class G = T,
+      detail::enable_if_t<!std::is_nothrow_constructible<T, U &&>::value> * =
+          nullptr,
+      detail::enable_if_t<!std::is_void<U>::value> * = nullptr,
+      detail::enable_if_t<
+          (!std::is_same<expected<T, E>, detail::decay_t<U>>::value &&
+           !detail::conjunction<std::is_scalar<T>,
+                                std::is_same<T, detail::decay_t<U>>>::value &&
+           std::is_constructible<T, U>::value &&
+           std::is_assignable<G &, U>::value &&
+           std::is_nothrow_move_constructible<E>::value)> * = nullptr>
+  expected &operator=(U &&v) {
+    if (has_value()) {
+      val() = std::forward<U>(v);
+    } else {
+      auto tmp = std::move(err());
+      err().~unexpected<E>();
+
+#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED
+      try {
+        ::new (valptr()) T(std::forward<U>(v));
+        this->m_has_val = true;
+      } catch (...) {
+        err() = std::move(tmp);
+        throw;
+      }
+#else
+      ::new (valptr()) T(std::forward<U>(v));
+      this->m_has_val = true;
+#endif
+    }
+
+    return *this;
+  }
+
+  template <class G = E,
+            detail::enable_if_t<std::is_nothrow_copy_constructible<G>::value &&
+                                std::is_assignable<G &, G>::value> * = nullptr>
+  expected &operator=(const unexpected<G> &rhs) {
+    if (!has_value()) {
+      err() = rhs;
+    } else {
+      this->destroy_val();
+      ::new (errptr()) unexpected<E>(rhs);
+      this->m_has_val = false;
+    }
+
+    return *this;
+  }
+
+  template <class G = E,
+            detail::enable_if_t<std::is_nothrow_move_constructible<G>::value &&
+                                std::is_move_assignable<G>::value> * = nullptr>
+  expected &operator=(unexpected<G> &&rhs) noexcept {
+    if (!has_value()) {
+      err() = std::move(rhs);
+    } else {
+      this->destroy_val();
+      ::new (errptr()) unexpected<E>(std::move(rhs));
+      this->m_has_val = false;
+    }
+
+    return *this;
+  }
+
+  template <class... Args, detail::enable_if_t<std::is_nothrow_constructible<
+                               T, Args &&...>::value> * = nullptr>
+  void emplace(Args &&...args) {
+    if (has_value()) {
+      val().~T();
+    } else {
+      err().~unexpected<E>();
+      this->m_has_val = true;
+    }
+    ::new (valptr()) T(std::forward<Args>(args)...);
+  }
+
+  template <class... Args, detail::enable_if_t<!std::is_nothrow_constructible<
+                               T, Args &&...>::value> * = nullptr>
+  void emplace(Args &&...args) {
+    if (has_value()) {
+      val().~T();
+      ::new (valptr()) T(std::forward<Args>(args)...);
+    } else {
+      auto tmp = std::move(err());
+      err().~unexpected<E>();
+
+#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED
+      try {
+        ::new (valptr()) T(std::forward<Args>(args)...);
+        this->m_has_val = true;
+      } catch (...) {
+        err() = std::move(tmp);
+        throw;
+      }
+#else
+      ::new (valptr()) T(std::forward<Args>(args)...);
+      this->m_has_val = true;
+#endif
+    }
+  }
+
+  template <class U, class... Args,
+            detail::enable_if_t<std::is_nothrow_constructible<
+                T, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  void emplace(std::initializer_list<U> il, Args &&...args) {
+    if (has_value()) {
+      T t(il, std::forward<Args>(args)...);
+      val() = std::move(t);
+    } else {
+      err().~unexpected<E>();
+      ::new (valptr()) T(il, std::forward<Args>(args)...);
+      this->m_has_val = true;
+    }
+  }
+
+  template <class U, class... Args,
+            detail::enable_if_t<!std::is_nothrow_constructible<
+                T, std::initializer_list<U> &, Args &&...>::value> * = nullptr>
+  void emplace(std::initializer_list<U> il, Args &&...args) {
+    if (has_value()) {
+      T t(il, std::forward<Args>(args)...);
+      val() = std::move(t);
+    } else {
+      auto tmp = std::move(err());
+      err().~unexpected<E>();
+
+#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED
+      try {
+        ::new (valptr()) T(il, std::forward<Args>(args)...);
+        this->m_has_val = true;
+      } catch (...) {
+        err() = std::move(tmp);
+        throw;
+      }
+#else
+      ::new (valptr()) T(il, std::forward<Args>(args)...);
+      this->m_has_val = true;
+#endif
+    }
+  }
+
+private:
+  using t_is_void = std::true_type;
+  using t_is_not_void = std::false_type;
+  using t_is_nothrow_move_constructible = std::true_type;
+  using move_constructing_t_can_throw = std::false_type;
+  using e_is_nothrow_move_constructible = std::true_type;
+  using move_constructing_e_can_throw = std::false_type;
+
+  void swap_where_both_have_value(expected & /*rhs*/, t_is_void) noexcept {
+    // swapping void is a no-op
+  }
+
+  void swap_where_both_have_value(expected &rhs, t_is_not_void) {
+    using std::swap;
+    swap(val(), rhs.val());
+  }
+
+  void swap_where_only_one_has_value(expected &rhs, t_is_void) noexcept(
+      std::is_nothrow_move_constructible<E>::value) {
+    ::new (errptr()) unexpected_type(std::move(rhs.err()));
+    rhs.err().~unexpected_type();
+    std::swap(this->m_has_val, rhs.m_has_val);
+  }
+
+  void swap_where_only_one_has_value(expected &rhs, t_is_not_void) {
+    swap_where_only_one_has_value_and_t_is_not_void(
+        rhs, typename std::is_nothrow_move_constructible<T>::type{},
+        typename std::is_nothrow_move_constructible<E>::type{});
+  }
+
+  void swap_where_only_one_has_value_and_t_is_not_void(
+      expected &rhs, t_is_nothrow_move_constructible,
+      e_is_nothrow_move_constructible) noexcept {
+    auto temp = std::move(val());
+    val().~T();
+    ::new (errptr()) unexpected_type(std::move(rhs.err()));
+    rhs.err().~unexpected_type();
+    ::new (rhs.valptr()) T(std::move(temp));
+    std::swap(this->m_has_val, rhs.m_has_val);
+  }
+
+  void swap_where_only_one_has_value_and_t_is_not_void(
+      expected &rhs, t_is_nothrow_move_constructible,
+      move_constructing_e_can_throw) {
+    auto temp = std::move(val());
+    val().~T();
+#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED
+    try {
+      ::new (errptr()) unexpected_type(std::move(rhs.err()));
+      rhs.err().~unexpected_type();
+      ::new (rhs.valptr()) T(std::move(temp));
+      std::swap(this->m_has_val, rhs.m_has_val);
+    } catch (...) {
+      val() = std::move(temp);
+      throw;
+    }
+#else
+    ::new (errptr()) unexpected_type(std::move(rhs.err()));
+    rhs.err().~unexpected_type();
+    ::new (rhs.valptr()) T(std::move(temp));
+    std::swap(this->m_has_val, rhs.m_has_val);
+#endif
+  }
+
+  void swap_where_only_one_has_value_and_t_is_not_void(
+      expected &rhs, move_constructing_t_can_throw,
+      e_is_nothrow_move_constructible) {
+    auto temp = std::move(rhs.err());
+    rhs.err().~unexpected_type();
+#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED
+    try {
+      ::new (rhs.valptr()) T(std::move(val()));
+      val().~T();
+      ::new (errptr()) unexpected_type(std::move(temp));
+      std::swap(this->m_has_val, rhs.m_has_val);
+    } catch (...) {
+      rhs.err() = std::move(temp);
+      throw;
+    }
+#else
+    ::new (rhs.valptr()) T(std::move(val()));
+    val().~T();
+    ::new (errptr()) unexpected_type(std::move(temp));
+    std::swap(this->m_has_val, rhs.m_has_val);
+#endif
+  }
+
+public:
+  template <class OT = T, class OE = E>
+  detail::enable_if_t<detail::is_swappable<OT>::value &&
+                      detail::is_swappable<OE>::value &&
+                      (std::is_nothrow_move_constructible<OT>::value ||
+                       std::is_nothrow_move_constructible<OE>::value)>
+  swap(expected &rhs) noexcept(
+      std::is_nothrow_move_constructible<T>::value
+          &&detail::is_nothrow_swappable<T>::value
+              &&std::is_nothrow_move_constructible<E>::value
+                  &&detail::is_nothrow_swappable<E>::value) {
+    if (has_value() && rhs.has_value()) {
+      swap_where_both_have_value(rhs, typename std::is_void<T>::type{});
+    } else if (!has_value() && rhs.has_value()) {
+      rhs.swap(*this);
+    } else if (has_value()) {
+      swap_where_only_one_has_value(rhs, typename std::is_void<T>::type{});
+    } else {
+      using std::swap;
+      swap(err(), rhs.err());
+    }
+  }
+
+  constexpr const T *operator->() const {
+    TL_ASSERT(has_value());
+    return valptr();
+  }
+  TL_EXPECTED_11_CONSTEXPR T *operator->() {
+    TL_ASSERT(has_value());
+    return valptr();
+  }
+
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  constexpr const U &operator*() const & {
+    TL_ASSERT(has_value());
+    return val();
+  }
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  TL_EXPECTED_11_CONSTEXPR U &operator*() & {
+    TL_ASSERT(has_value());
+    return val();
+  }
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  constexpr const U &&operator*() const && {
+    TL_ASSERT(has_value());
+    return std::move(val());
+  }
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  TL_EXPECTED_11_CONSTEXPR U &&operator*() && {
+    TL_ASSERT(has_value());
+    return std::move(val());
+  }
+
+  constexpr bool has_value() const noexcept { return this->m_has_val; }
+  constexpr explicit operator bool() const noexcept { return this->m_has_val; }
+
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  TL_EXPECTED_11_CONSTEXPR const U &value() const & {
+    if (!has_value())
+      TL_EXPECTED_THROW_EXCEPTION(bad_expected_access<E>(err().value()));
+    return val();
+  }
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  TL_EXPECTED_11_CONSTEXPR U &value() & {
+    if (!has_value())
+      TL_EXPECTED_THROW_EXCEPTION(bad_expected_access<E>(err().value()));
+    return val();
+  }
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  TL_EXPECTED_11_CONSTEXPR const U &&value() const && {
+    if (!has_value())
+      TL_EXPECTED_THROW_EXCEPTION(bad_expected_access<E>(std::move(err()).value()));
+    return std::move(val());
+  }
+  template <class U = T,
+            detail::enable_if_t<!std::is_void<U>::value> * = nullptr>
+  TL_EXPECTED_11_CONSTEXPR U &&value() && {
+    if (!has_value())
+      TL_EXPECTED_THROW_EXCEPTION(bad_expected_access<E>(std::move(err()).value()));
+    return std::move(val());
+  }
+
+  constexpr const E &error() const & {
+    TL_ASSERT(!has_value());
+    return err().value();
+  }
+  TL_EXPECTED_11_CONSTEXPR E &error() & {
+    TL_ASSERT(!has_value());
+    return err().value();
+  }
+  constexpr const E &&error() const && {
+    TL_ASSERT(!has_value());
+    return std::move(err().value());
+  }
+  TL_EXPECTED_11_CONSTEXPR E &&error() && {
+    TL_ASSERT(!has_value());
+    return std::move(err().value());
+  }
+
+  template <class U> constexpr T value_or(U &&v) const & {
+    static_assert(std::is_copy_constructible<T>::value &&
+                      std::is_convertible<U &&, T>::value,
+                  "T must be copy-constructible and convertible to from U&&");
+    return bool(*this) ? **this : static_cast<T>(std::forward<U>(v));
+  }
+  template <class U> TL_EXPECTED_11_CONSTEXPR T value_or(U &&v) && {
+    static_assert(std::is_move_constructible<T>::value &&
+                      std::is_convertible<U &&, T>::value,
+                  "T must be move-constructible and convertible to from U&&");
+    return bool(*this) ? std::move(**this) : static_cast<T>(std::forward<U>(v));
+  }
+};
+
+namespace detail {
+template <class Exp> using exp_t = typename detail::decay_t<Exp>::value_type;
+template <class Exp> using err_t = typename detail::decay_t<Exp>::error_type;
+template <class Exp, class Ret> using ret_t = expected<Ret, err_t<Exp>>;
+
+#ifdef TL_EXPECTED_CXX14
+template <class Exp, class F,
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              *std::declval<Exp>()))>
+constexpr auto and_then_impl(Exp &&exp, F &&f) {
+  static_assert(detail::is_expected<Ret>::value, "F must return an expected");
+
+  return exp.has_value()
+             ? detail::invoke(std::forward<F>(f), *std::forward<Exp>(exp))
+             : Ret(unexpect, std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>()))>
+constexpr auto and_then_impl(Exp &&exp, F &&f) {
+  static_assert(detail::is_expected<Ret>::value, "F must return an expected");
+
+  return exp.has_value() ? detail::invoke(std::forward<F>(f))
+                         : Ret(unexpect, std::forward<Exp>(exp).error());
+}
+#else
+template <class> struct TC;
+template <class Exp, class F,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              *std::declval<Exp>())),
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr>
+auto and_then_impl(Exp &&exp, F &&f) -> Ret {
+  static_assert(detail::is_expected<Ret>::value, "F must return an expected");
+
+  return exp.has_value()
+             ? detail::invoke(std::forward<F>(f), *std::forward<Exp>(exp))
+             : Ret(unexpect, std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          class Ret = decltype(detail::invoke(std::declval<F>())),
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr>
+constexpr auto and_then_impl(Exp &&exp, F &&f) -> Ret {
+  static_assert(detail::is_expected<Ret>::value, "F must return an expected");
+
+  return exp.has_value() ? detail::invoke(std::forward<F>(f))
+                         : Ret(unexpect, std::forward<Exp>(exp).error());
+}
+#endif
+
+#ifdef TL_EXPECTED_CXX14
+template <class Exp, class F,
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              *std::declval<Exp>())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+constexpr auto expected_map_impl(Exp &&exp, F &&f) {
+  using result = ret_t<Exp, detail::decay_t<Ret>>;
+  return exp.has_value() ? result(detail::invoke(std::forward<F>(f),
+                                                 *std::forward<Exp>(exp)))
+                         : result(unexpect, std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              *std::declval<Exp>())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+auto expected_map_impl(Exp &&exp, F &&f) {
+  using result = expected<void, err_t<Exp>>;
+  if (exp.has_value()) {
+    detail::invoke(std::forward<F>(f), *std::forward<Exp>(exp));
+    return result();
+  }
+
+  return result(unexpect, std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+constexpr auto expected_map_impl(Exp &&exp, F &&f) {
+  using result = ret_t<Exp, detail::decay_t<Ret>>;
+  return exp.has_value() ? result(detail::invoke(std::forward<F>(f)))
+                         : result(unexpect, std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+auto expected_map_impl(Exp &&exp, F &&f) {
+  using result = expected<void, err_t<Exp>>;
+  if (exp.has_value()) {
+    detail::invoke(std::forward<F>(f));
+    return result();
+  }
+
+  return result(unexpect, std::forward<Exp>(exp).error());
+}
+#else
+template <class Exp, class F,
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              *std::declval<Exp>())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+
+constexpr auto expected_map_impl(Exp &&exp, F &&f)
+    -> ret_t<Exp, detail::decay_t<Ret>> {
+  using result = ret_t<Exp, detail::decay_t<Ret>>;
+
+  return exp.has_value() ? result(detail::invoke(std::forward<F>(f),
+                                                 *std::forward<Exp>(exp)))
+                         : result(unexpect, std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              *std::declval<Exp>())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+
+auto expected_map_impl(Exp &&exp, F &&f) -> expected<void, err_t<Exp>> {
+  if (exp.has_value()) {
+    detail::invoke(std::forward<F>(f), *std::forward<Exp>(exp));
+    return {};
+  }
+
+  return unexpected<err_t<Exp>>(std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+
+constexpr auto expected_map_impl(Exp &&exp, F &&f)
+    -> ret_t<Exp, detail::decay_t<Ret>> {
+  using result = ret_t<Exp, detail::decay_t<Ret>>;
+
+  return exp.has_value() ? result(detail::invoke(std::forward<F>(f)))
+                         : result(unexpect, std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+
+auto expected_map_impl(Exp &&exp, F &&f) -> expected<void, err_t<Exp>> {
+  if (exp.has_value()) {
+    detail::invoke(std::forward<F>(f));
+    return {};
+  }
+
+  return unexpected<err_t<Exp>>(std::forward<Exp>(exp).error());
+}
+#endif
+
+#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) &&               \
+    !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55)
+template <class Exp, class F,
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+constexpr auto map_error_impl(Exp &&exp, F &&f) {
+  using result = expected<exp_t<Exp>, detail::decay_t<Ret>>;
+  return exp.has_value()
+             ? result(*std::forward<Exp>(exp))
+             : result(unexpect, detail::invoke(std::forward<F>(f),
+                                               std::forward<Exp>(exp).error()));
+}
+template <class Exp, class F,
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+auto map_error_impl(Exp &&exp, F &&f) {
+  using result = expected<exp_t<Exp>, monostate>;
+  if (exp.has_value()) {
+    return result(*std::forward<Exp>(exp));
+  }
+
+  detail::invoke(std::forward<F>(f), std::forward<Exp>(exp).error());
+  return result(unexpect, monostate{});
+}
+template <class Exp, class F,
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+constexpr auto map_error_impl(Exp &&exp, F &&f) {
+  using result = expected<exp_t<Exp>, detail::decay_t<Ret>>;
+  return exp.has_value()
+             ? result()
+             : result(unexpect, detail::invoke(std::forward<F>(f),
+                                               std::forward<Exp>(exp).error()));
+}
+template <class Exp, class F,
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+auto map_error_impl(Exp &&exp, F &&f) {
+  using result = expected<exp_t<Exp>, monostate>;
+  if (exp.has_value()) {
+    return result();
+  }
+
+  detail::invoke(std::forward<F>(f), std::forward<Exp>(exp).error());
+  return result(unexpect, monostate{});
+}
+#else
+template <class Exp, class F,
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+constexpr auto map_error_impl(Exp &&exp, F &&f)
+    -> expected<exp_t<Exp>, detail::decay_t<Ret>> {
+  using result = expected<exp_t<Exp>, detail::decay_t<Ret>>;
+
+  return exp.has_value()
+             ? result(*std::forward<Exp>(exp))
+             : result(unexpect, detail::invoke(std::forward<F>(f),
+                                               std::forward<Exp>(exp).error()));
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<!std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+auto map_error_impl(Exp &&exp, F &&f) -> expected<exp_t<Exp>, monostate> {
+  using result = expected<exp_t<Exp>, monostate>;
+  if (exp.has_value()) {
+    return result(*std::forward<Exp>(exp));
+  }
+
+  detail::invoke(std::forward<F>(f), std::forward<Exp>(exp).error());
+  return result(unexpect, monostate{});
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+constexpr auto map_error_impl(Exp &&exp, F &&f)
+    -> expected<exp_t<Exp>, detail::decay_t<Ret>> {
+  using result = expected<exp_t<Exp>, detail::decay_t<Ret>>;
+
+  return exp.has_value()
+             ? result()
+             : result(unexpect, detail::invoke(std::forward<F>(f),
+                                               std::forward<Exp>(exp).error()));
+}
+
+template <class Exp, class F,
+          detail::enable_if_t<std::is_void<exp_t<Exp>>::value> * = nullptr,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+auto map_error_impl(Exp &&exp, F &&f) -> expected<exp_t<Exp>, monostate> {
+  using result = expected<exp_t<Exp>, monostate>;
+  if (exp.has_value()) {
+    return result();
+  }
+
+  detail::invoke(std::forward<F>(f), std::forward<Exp>(exp).error());
+  return result(unexpect, monostate{});
+}
+#endif
+
+#ifdef TL_EXPECTED_CXX14
+template <class Exp, class F,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+constexpr auto or_else_impl(Exp &&exp, F &&f) {
+  static_assert(detail::is_expected<Ret>::value, "F must return an expected");
+  return exp.has_value() ? std::forward<Exp>(exp)
+                         : detail::invoke(std::forward<F>(f),
+                                          std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+detail::decay_t<Exp> or_else_impl(Exp &&exp, F &&f) {
+  return exp.has_value() ? std::forward<Exp>(exp)
+                         : (detail::invoke(std::forward<F>(f),
+                                           std::forward<Exp>(exp).error()),
+                            std::forward<Exp>(exp));
+}
+#else
+template <class Exp, class F,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<!std::is_void<Ret>::value> * = nullptr>
+auto or_else_impl(Exp &&exp, F &&f) -> Ret {
+  static_assert(detail::is_expected<Ret>::value, "F must return an expected");
+  return exp.has_value() ? std::forward<Exp>(exp)
+                         : detail::invoke(std::forward<F>(f),
+                                          std::forward<Exp>(exp).error());
+}
+
+template <class Exp, class F,
+          class Ret = decltype(detail::invoke(std::declval<F>(),
+                                              std::declval<Exp>().error())),
+          detail::enable_if_t<std::is_void<Ret>::value> * = nullptr>
+detail::decay_t<Exp> or_else_impl(Exp &&exp, F &&f) {
+  return exp.has_value() ? std::forward<Exp>(exp)
+                         : (detail::invoke(std::forward<F>(f),
+                                           std::forward<Exp>(exp).error()),
+                            std::forward<Exp>(exp));
+}
+#endif
+} // namespace detail
+
+template <class T, class E, class U, class F>
+constexpr bool operator==(const expected<T, E> &lhs,
+                          const expected<U, F> &rhs) {
+  return (lhs.has_value() != rhs.has_value())
+             ? false
+             : (!lhs.has_value() ? lhs.error() == rhs.error() : *lhs == *rhs);
+}
+template <class T, class E, class U, class F>
+constexpr bool operator!=(const expected<T, E> &lhs,
+                          const expected<U, F> &rhs) {
+  return (lhs.has_value() != rhs.has_value())
+             ? true
+             : (!lhs.has_value() ? lhs.error() != rhs.error() : *lhs != *rhs);
+}
+template <class E, class F>
+constexpr bool operator==(const expected<void, E> &lhs,
+                          const expected<void, F> &rhs) {
+  return (lhs.has_value() != rhs.has_value())
+             ? false
+             : (!lhs.has_value() ? lhs.error() == rhs.error() : true);
+}
+template <class E, class F>
+constexpr bool operator!=(const expected<void, E> &lhs,
+                          const expected<void, F> &rhs) {
+  return (lhs.has_value() != rhs.has_value())
+             ? true
+             : (!lhs.has_value() ? lhs.error() != rhs.error() : false);
+}
+
+template <class T, class E, class U>
+constexpr bool operator==(const expected<T, E> &x, const U &v) {
+  return x.has_value() ? *x == v : false;
+}
+template <class T, class E, class U>
+constexpr bool operator==(const U &v, const expected<T, E> &x) {
+  return x.has_value() ? *x == v : false;
+}
+template <class T, class E, class U>
+constexpr bool operator!=(const expected<T, E> &x, const U &v) {
+  return x.has_value() ? *x != v : true;
+}
+template <class T, class E, class U>
+constexpr bool operator!=(const U &v, const expected<T, E> &x) {
+  return x.has_value() ? *x != v : true;
+}
+
+template <class T, class E>
+constexpr bool operator==(const expected<T, E> &x, const unexpected<E> &e) {
+  return x.has_value() ? false : x.error() == e.value();
+}
+template <class T, class E>
+constexpr bool operator==(const unexpected<E> &e, const expected<T, E> &x) {
+  return x.has_value() ? false : x.error() == e.value();
+}
+template <class T, class E>
+constexpr bool operator!=(const expected<T, E> &x, const unexpected<E> &e) {
+  return x.has_value() ? true : x.error() != e.value();
+}
+template <class T, class E>
+constexpr bool operator!=(const unexpected<E> &e, const expected<T, E> &x) {
+  return x.has_value() ? true : x.error() != e.value();
+}
+
+template <class T, class E,
+          detail::enable_if_t<(std::is_void<T>::value ||
+                               std::is_move_constructible<T>::value) &&
+                              detail::is_swappable<T>::value &&
+                              std::is_move_constructible<E>::value &&
+                              detail::is_swappable<E>::value> * = nullptr>
+void swap(expected<T, E> &lhs,
+          expected<T, E> &rhs) noexcept(noexcept(lhs.swap(rhs))) {
+  lhs.swap(rhs);
+}
+} // namespace tl
+
+#endif
diff --git a/include/Settings.h b/include/Settings.h
index 3b5618ad5b8..a602dd56799 100644
--- a/include/Settings.h
+++ b/include/Settings.h
@@ -65,6 +65,11 @@ class Settings {
   // Copy constructor
   Settings(const Settings&) = default;
 
+  ~Settings();
+
+  // Safety checks
+  bool validate();
+
   /*
    * Ostream operator overloading to dump settings to the terminal
    */
@@ -147,6 +152,19 @@ class Settings {
   void readLoadAndSave(cv::FileStorage& fSettings);
   void readOtherParameters(cv::FileStorage& fSettings);
 
+  // For PinHole,       k = {fx, fy, cx, cy}, and dist can be 0, 4 or 5 params
+  // For Rectified,     k = {fx, fy, cx, cy}  and dist is ignored
+  // For KannalaBrandt, k = {fx, fy, cx, cy, k0, k1, k2, k3};
+  void setMonoCamera(CameraType type, const std::vector<float>& k,
+                     const std::vector<float>& dist = {});
+  void setRightCamera(const std::vector<float>& k2,
+                      const std::vector<float>& dist2, const cv::Mat& T_c1_c2,
+                      float thDepth);
+
+  void setStereoRectifiedCamera(const std::vector<float>& k, float baseline,
+                                float thDepth);
+
+  void setImageSize(int width, int height);
   void precomputeRectificationMaps();
 
   SensorType sensor_;
diff --git a/include/System.h b/include/System.h
index 791cd54e233..034092d09ae 100644
--- a/include/System.h
+++ b/include/System.h
@@ -41,6 +41,7 @@
 #include "MapDrawer.h"
 #include "ORBVocabulary.h"
 #include "Settings.h"
+#include "Thirdparty/tl/expected.hpp"
 #include "Tracking.h"
 #include "Viewer.h"
 
@@ -55,6 +56,13 @@ class LocalMapping;
 class LoopClosing;
 class Settings;
 
+typedef tl::expected<std::shared_ptr<System>, bool> FactoryExpected;
+
+class SystemFactory {
+ public:
+  static FactoryExpected create(const std::shared_ptr<Settings> &settings);
+};
+
 class System {
  public:
   // File type
diff --git a/src/Settings.cc b/src/Settings.cc
index 8f0681ecf50..a302668432f 100644
--- a/src/Settings.cc
+++ b/src/Settings.cc
@@ -21,6 +21,7 @@
 
 #include "Settings.h"
 
+#include <filesystem>  // NOLINT {build/c++17}
 #include <iostream>
 #include <memory>
 #include <opencv2/core/eigen.hpp>
@@ -136,7 +137,8 @@ Settings::Settings(const SensorType sensor)
       bNeedToResize1_(false),
       bNeedToResize2_(false),
       loopClosing_(true),
-      sensor_(sensor) {
+      sensor_(sensor),
+      imageViewerScale_(1.0f) {
   // if(bNeedToRectify_){
   //     precomputeRectificationMaps();
   //     cout << "\t-Computed rectification maps" << endl;
@@ -149,7 +151,8 @@ Settings::Settings(const std::string& configFile, const SensorType sensor)
       bNeedToResize1_(false),
       bNeedToResize2_(false),
       loopClosing_(true),
-      sensor_(sensor) {
+      sensor_(sensor),
+      imageViewerScale_(1.0f) {
   // Open settings file
   cv::FileStorage fSettings(configFile, cv::FileStorage::READ);
   if (!fSettings.isOpened()) {
@@ -159,49 +162,74 @@ Settings::Settings(const std::string& configFile, const SensorType sensor)
 
     exit(-1);
   } else {
-    spdlog::info("Loading settings from {configFile}");
+    spdlog::info("Loading settings from {}", configFile);
   }
 
   // Read first camera
   readCamera1(fSettings);
-  cout << "\t-Loaded camera 1" << endl;
+  spdlog::info("\t-Loaded camera 1");
 
   // Read second camera if stereo (not rectified)
   if (sensor_ == SensorType::STEREO || sensor_ == SensorType::IMU_STEREO) {
     readCamera2(fSettings);
-    cout << "\t-Loaded camera 2" << endl;
+    spdlog::info("\t-Loaded camera 2");
   }
 
   // Read image info
   readImageInfo(fSettings);
-  cout << "\t-Loaded image info" << endl;
+  spdlog::info("\t-Loaded image info");
 
   if (sensor_ == SensorType::IMU_MONOCULAR ||
       sensor_ == SensorType::IMU_STEREO || sensor_ == SensorType::IMU_RGBD) {
     readIMU(fSettings);
-    cout << "\t-Loaded IMU calibration" << endl;
+    spdlog::info("\t-Loaded IMU calibration");
   }
 
   if (sensor_ == SensorType::RGBD || sensor_ == SensorType::IMU_RGBD) {
     readRGBD(fSettings);
-    cout << "\t-Loaded RGB-D calibration" << endl;
+    spdlog::info("\t-Loaded RGB-D calibration");
   }
 
   readORB(fSettings);
-  cout << "\t-Loaded ORB settings" << endl;
+  spdlog::info("\t-Loaded ORB settings");
   readViewer(fSettings);
-  cout << "\t-Loaded viewer settings" << endl;
+  spdlog::info("\t-Loaded viewer settings");
   readLoadAndSave(fSettings);
-  cout << "\t-Loaded Atlas settings" << endl;
+  spdlog::info("\t-Loaded Atlas settings");
   readOtherParameters(fSettings);
-  cout << "\t-Loaded misc parameters" << endl;
+  spdlog::info("\t-Loaded misc parameters");
 
   if (bNeedToRectify_) {
     precomputeRectificationMaps();
-    cout << "\t-Computed rectification maps" << endl;
+    spdlog::info("\t-Computed rectification maps");
   }
 
-  cout << "----------------------------------" << endl;
+  spdlog::info("----------------------------------");
+}
+
+Settings::~Settings() { ; }
+
+bool Settings::validate(void) {
+  if (bNeedToRectify_) {
+    precomputeRectificationMaps();
+  }
+
+  // Check all of the variables that are assumed to be set
+  if (!calibration1_) return false;
+  if (!originalCalib1_) return false;
+
+  if (originalImSize_.width == 0) return false;
+  if (originalImSize_.height == 0) return false;
+
+  if (strVocFile_.size() == 0) {
+    spdlog::warn("Vocab file not specified");
+    return false;
+  } else if (!std::filesystem::exists(strVocFile_)) {
+    spdlog::warn("Vocab file {} does not exist.", strVocFile_);
+    return false;
+  }
+
+  return true;
 }
 
 void Settings::readCamera1(cv::FileStorage& fSettings) {
@@ -210,10 +238,8 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
   // Read camera model
   string cameraModel = readParameter<string>(fSettings, "Camera.type", found);
 
-  vector<float> vCalibration;
+  vector<float> vCalibration, vDistortion;
   if (cameraModel == "PinHole") {
-    cameraType_ = PinHole;
-
     // Read intrinsic parameters
     float fx = readParameter<float>(fSettings, "Camera1.fx", found);
     float fy = readParameter<float>(fSettings, "Camera1.fy", found);
@@ -222,39 +248,27 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy};
 
-    calibration1_ = std::make_shared<Pinhole>(vCalibration);
-    originalCalib1_ = std::make_shared<Pinhole>(vCalibration);
+    setMonoCamera(PinHole, vCalibration);
 
     // Check if it is a distorted PinHole
     readParameter<float>(fSettings, "Camera1.k1", found, false);
     if (found) {
       readParameter<float>(fSettings, "Camera1.k3", found, false);
       if (found) {
-        vPinHoleDistorsion1_.resize(5);
-        vPinHoleDistorsion1_[4] =
-            readParameter<float>(fSettings, "Camera1.k3", found);
+        vDistortion.resize(5);
+        vDistortion[4] = readParameter<float>(fSettings, "Camera1.k3", found);
       } else {
-        vPinHoleDistorsion1_.resize(4);
+        vDistortion.resize(4);
       }
-      vPinHoleDistorsion1_[0] =
-          readParameter<float>(fSettings, "Camera1.k1", found);
-      vPinHoleDistorsion1_[1] =
-          readParameter<float>(fSettings, "Camera1.k2", found);
-      vPinHoleDistorsion1_[2] =
-          readParameter<float>(fSettings, "Camera1.p1", found);
-      vPinHoleDistorsion1_[3] =
-          readParameter<float>(fSettings, "Camera1.p2", found);
+      vDistortion[0] = readParameter<float>(fSettings, "Camera1.k1", found);
+      vDistortion[1] = readParameter<float>(fSettings, "Camera1.k2", found);
+      vDistortion[2] = readParameter<float>(fSettings, "Camera1.p1", found);
+      vDistortion[3] = readParameter<float>(fSettings, "Camera1.p2", found);
     }
 
-    // Check if we need to correct distortion from the images
-    if ((sensor_ == SensorType::MONOCULAR ||
-         sensor_ == SensorType::IMU_MONOCULAR) &&
-        vPinHoleDistorsion1_.size() != 0) {
-      bNeedToUndistort_ = true;
-    }
-  } else if (cameraModel == "Rectified") {
-    cameraType_ = Rectified;
+    setMonoCamera(PinHole, vCalibration, vDistortion);
 
+  } else if (cameraModel == "Rectified") {
     // Read intrinsic parameters
     float fx = readParameter<float>(fSettings, "Camera1.fx", found);
     float fy = readParameter<float>(fSettings, "Camera1.fy", found);
@@ -263,13 +277,10 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy};
 
-    calibration1_ = std::make_shared<Pinhole>(vCalibration);
-    originalCalib1_ = std::make_shared<Pinhole>(vCalibration);
+    setMonoCamera(Rectified, vCalibration, {});
 
     // Rectified images are assumed to be ideal PinHole images (no distortion)
   } else if (cameraModel == "KannalaBrandt8") {
-    cameraType_ = KannalaBrandt;
-
     // Read intrinsic parameters
     float fx = readParameter<float>(fSettings, "Camera1.fx", found);
     float fy = readParameter<float>(fSettings, "Camera1.fy", found);
@@ -283,19 +294,18 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
 
-    calibration1_ = std::make_shared<KannalaBrandt8>(vCalibration);
-    originalCalib1_ = std::make_shared<KannalaBrandt8>(vCalibration);
+    setMonoCamera(KannalaBrandt, vCalibration, {});
 
-    if (sensor_ == SensorType::STEREO || sensor_ == SensorType::IMU_STEREO) {
-      int colBegin =
-          readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
-      int colEnd =
-          readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
-      vector<int> vOverlapping = {colBegin, colEnd};
+    // if (sensor_ == SensorType::STEREO || sensor_ == SensorType::IMU_STEREO) {
+    //   int colBegin =
+    //       readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
+    //   int colEnd =
+    //       readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
+    //   vector<int> vOverlapping = {colBegin, colEnd};
 
-      dynamic_cast<KannalaBrandt8&>(*calibration1_).mvLappingArea =
-          vOverlapping;
-    }
+    //   dynamic_cast<KannalaBrandt8&>(*calibration1_).mvLappingArea =
+    //       vOverlapping;
+    //}
   } else {
     cerr << "Error: " << cameraModel << " not known" << endl;
     exit(-1);
@@ -304,10 +314,8 @@ void Settings::readCamera1(cv::FileStorage& fSettings) {
 
 void Settings::readCamera2(cv::FileStorage& fSettings) {
   bool found;
-  vector<float> vCalibration;
+  vector<float> vCalibration, vDistortion;
   if (cameraType_ == PinHole) {
-    bNeedToRectify_ = true;
-
     // Read intrinsic parameters
     float fx = readParameter<float>(fSettings, "Camera2.fx", found);
     float fy = readParameter<float>(fSettings, "Camera2.fy", found);
@@ -316,29 +324,22 @@ void Settings::readCamera2(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy};
 
-    calibration2_ = std::make_shared<Pinhole>(vCalibration);
-    originalCalib2_ = std::make_shared<Pinhole>(vCalibration);
-
     // Check if it is a distorted PinHole
     readParameter<float>(fSettings, "Camera2.k1", found, false);
     if (found) {
       readParameter<float>(fSettings, "Camera2.k3", found, false);
       if (found) {
-        vPinHoleDistorsion2_.resize(5);
-        vPinHoleDistorsion2_[4] =
-            readParameter<float>(fSettings, "Camera2.k3", found);
+        vDistortion.resize(5);
+        vDistortion[4] = readParameter<float>(fSettings, "Camera2.k3", found);
       } else {
-        vPinHoleDistorsion2_.resize(4);
+        vDistortion.resize(4);
       }
-      vPinHoleDistorsion2_[0] =
-          readParameter<float>(fSettings, "Camera2.k1", found);
-      vPinHoleDistorsion2_[1] =
-          readParameter<float>(fSettings, "Camera2.k2", found);
-      vPinHoleDistorsion2_[2] =
-          readParameter<float>(fSettings, "Camera2.p1", found);
-      vPinHoleDistorsion2_[3] =
-          readParameter<float>(fSettings, "Camera2.p2", found);
+      vDistortion[0] = readParameter<float>(fSettings, "Camera2.k1", found);
+      vDistortion[1] = readParameter<float>(fSettings, "Camera2.k2", found);
+      vDistortion[2] = readParameter<float>(fSettings, "Camera2.p1", found);
+      vDistortion[3] = readParameter<float>(fSettings, "Camera2.p2", found);
     }
+
   } else if (cameraType_ == KannalaBrandt) {
     // Read intrinsic parameters
     float fx = readParameter<float>(fSettings, "Camera2.fx", found);
@@ -353,32 +354,147 @@ void Settings::readCamera2(cv::FileStorage& fSettings) {
 
     vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
 
-    calibration2_ = std::make_shared<KannalaBrandt8>(vCalibration);
-    originalCalib2_ = std::make_shared<KannalaBrandt8>(vCalibration);
+    // int colBegin =
+    //     readParameter<int>(fSettings, "Camera2.overlappingBegin", found);
+    // int colEnd = readParameter<int>(fSettings, "Camera2.overlappingEnd",
+    // found); vector<int> vOverlapping = {colBegin, colEnd};
 
-    int colBegin =
-        readParameter<int>(fSettings, "Camera2.overlappingBegin", found);
-    int colEnd = readParameter<int>(fSettings, "Camera2.overlappingEnd", found);
-    vector<int> vOverlapping = {colBegin, colEnd};
-
-    dynamic_cast<KannalaBrandt8&>(*calibration2_).mvLappingArea = vOverlapping;
+    // dynamic_cast<KannalaBrandt8&>(*calibration2_).mvLappingArea =
+    // vOverlapping;
   }
 
+  float thDepth = readParameter<float>(fSettings, "Stereo.ThDepth", found);
+
   // Load stereo extrinsic calibration
   if (cameraType_ == Rectified) {
-    b_ = readParameter<float>(fSettings, "Stereo.b", found);
+    const float baseline = readParameter<float>(fSettings, "Stereo.b", found);
+    // setRightCamera( vCalibration, vDistortion, baseline, thDepth );
+
+    b_ = baseline;
     bf_ = b_ * calibration1_->getParameter(0);
+
   } else {
     cv::Mat cvTlr = readParameter<cv::Mat>(fSettings, "Stereo.T_c1_c2", found);
-    Tlr_ = Converter::toSophus(cvTlr);
+    setRightCamera(vCalibration, vDistortion, cvTlr, thDepth);
+  }
+}
 
-    // TODO: also search for Trl and invert if necessary
+//===
 
-    b_ = Tlr_.translation().norm();
-    bf_ = b_ * calibration1_->getParameter(0);
+void Settings::setMonoCamera(CameraType type, const std::vector<float>& k,
+                             const std::vector<float>& dist) {
+  bool found;
+  cameraType_ = type;
+
+  if (cameraType_ == PinHole) {
+    calibration1_ = std::make_shared<Pinhole>(k);
+    originalCalib1_ = std::make_shared<Pinhole>(k);
+
+    vPinHoleDistorsion1_ = dist;
+
+    // Check if we need to correct distortion from the images
+    if (vPinHoleDistorsion1_.size() != 0) {
+      bNeedToUndistort_ = true;
+    }
+  } else if (cameraType_ == Rectified) {
+    calibration1_ = std::make_shared<Pinhole>(k);
+    originalCalib1_ = std::make_shared<Pinhole>(k);
+
+    // Rectified images are assumed to be ideal PinHole images (no distortion)
+  } else if (cameraType_ == KannalaBrandt) {
+    if (k.size() != 8) {
+      spdlog::error("Incorrect number of params for KannalaBrandt");
+      return;
+    }
+
+    calibration1_ = std::make_shared<KannalaBrandt8>(k);
+    originalCalib1_ = std::make_shared<KannalaBrandt8>(k);
+
+    // TBD
+    // if (sensor_.isStereo()) {
+    //   int colBegin =
+    //       readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
+    //   int colEnd =
+    //       readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
+    //   vector<int> vOverlapping = {colBegin, colEnd};
+
+    //   dynamic_cast<KannalaBrandt8&>(*calibration1_).mvLappingArea =
+    //       vOverlapping;
+    // }
+  } else {
+    spdlog::error("Error: {} not known", type);
+    exit(-1);
   }
+}
 
-  thDepth_ = readParameter<float>(fSettings, "Stereo.ThDepth", found);
+void Settings::setRightCamera(const std::vector<float>& k2,
+                              const std::vector<float>& dist2,
+                              const cv::Mat& T_c1_c2, float thDepth) {
+  if (cameraType_ == PinHole) {
+    bNeedToRectify_ = true;
+
+    calibration2_ = std::make_shared<Pinhole>(k2);
+    originalCalib2_ = std::make_shared<Pinhole>(k2);
+
+    vPinHoleDistorsion2_ = dist2;
+
+    // } else if (cameraType_ == Rectified) {
+    // Weird this wasn't set ... do they assume left and right camera
+    // params are equal for rectified cameras?
+    //   calibration2_ = std::make_shared<Rectified>(k2);
+    //   originalCalib2_ = std::make_shared<Rectified>(k2);
+  } else if (cameraType_ == KannalaBrandt) {
+    calibration2_ = std::make_shared<KannalaBrandt8>(k2);
+    originalCalib2_ = std::make_shared<KannalaBrandt8>(k2);
+
+    // TBD
+    // int colBegin =
+    //     readParameter<int>(fSettings, "Camera2.overlappingBegin", found);
+    // int colEnd = readParameter<int>(fSettings, "Camera2.overlappingEnd",
+    // found); vector<int> vOverlapping = {colBegin, colEnd};
+
+    // dynamic_cast<KannalaBrandt8&>(*calibration2_).mvLappingArea =
+    // vOverlapping;
+  }
+
+  cv::Mat cvTlr = T_c1_c2;
+  Tlr_ = Converter::toSophus(cvTlr);
+
+  // TODO: also search for Trl and invert if necessary
+
+  b_ = Tlr_.translation().norm();
+  bf_ = b_ * calibration1_->getParameter(0);
+
+  thDepth_ = thDepth;
+}
+
+void Settings::setStereoRectifiedCamera(const std::vector<float>& k,
+                                        float baseline, float thDepth) {
+  cameraType_ = Rectified;
+
+  calibration1_ = std::make_shared<Pinhole>(k);
+  originalCalib1_ = std::make_shared<Pinhole>(k);
+
+  b_ = baseline;
+  bf_ = b_ * calibration1_->getParameter(0);
+}
+
+//===
+
+void Settings::setImageSize(int width, int height) {
+  bool found;
+  // Read original and desired image dimensions
+  int originalRows = height;
+  int originalCols = width;
+
+  originalImSize_.width = originalCols;
+  originalImSize_.height = originalRows;
+
+  newImSize_ = originalImSize_;
+
+  // For now...
+  fps_ = 10;
+  bRGB_ = false;
 }
 
 void Settings::readImageInfo(cv::FileStorage& fSettings) {
diff --git a/src/System.cc b/src/System.cc
index 352cc9b7840..36f273b8774 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -48,6 +48,15 @@ namespace ORB_SLAM3 {
 
 Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL;
 
+FactoryExpected SystemFactory::create(
+    const std::shared_ptr<Settings> &settings) {
+  if (!settings->validate()) {
+    return tl::make_unexpected(false);
+  }
+
+  return std::make_shared<System>(settings);
+}
+
 System::System(const string &strVocFile, const string &strSettingsFile,
                const SensorType sensor, bool initFr, const string &strSequence)
     : mpViewer(nullptr),
@@ -123,8 +132,7 @@ void System::initialize(bool initFr, const string &strSequence) {
 
   if (mStrLoadAtlasFromFile.empty()) {
     // Load ORB Vocabulary
-    cout << endl
-         << "Loading ORB Vocabulary. This could take a while..." << endl;
+    spdlog::info("Loading ORB Vocabulary. This could take a while...");
 
     mpVocabulary = std::make_shared<ORBVocabulary>();
     bool bVocLoad = mpVocabulary->loadFromTextFile(mStrVocabularyFilePath);
@@ -133,18 +141,17 @@ void System::initialize(bool initFr, const string &strSequence) {
       cerr << "Falied to open at: " << mStrVocabularyFilePath << endl;
       exit(-1);
     }
-    cout << "Vocabulary loaded!" << endl << endl;
+    spdlog::info("Vocabulary loaded!");
 
     // Create KeyFrame Database
     mpKeyFrameDatabase = std::make_shared<KeyFrameDatabase>(mpVocabulary);
 
     // Create the Atlas
-    cout << "Initialization of Atlas from scratch " << endl;
+    spdlog::info("Initialization of Atlas from scratch ");
     mpAtlas = std::make_shared<Atlas>(0);
   } else {
     // Load ORB Vocabulary
-    cout << endl
-         << "Loading ORB Vocabulary. This could take a while..." << endl;
+    spdlog::info("Loading ORB Vocabulary. This could take a while...");
 
     mpVocabulary = std::make_shared<ORBVocabulary>();
     bool bVocLoad = mpVocabulary->loadFromTextFile(mStrVocabularyFilePath);
@@ -153,17 +160,15 @@ void System::initialize(bool initFr, const string &strSequence) {
       cerr << "Falied to open at: " << mStrVocabularyFilePath << endl;
       exit(-1);
     }
-    cout << "Vocabulary loaded!" << endl << endl;
+    spdlog::info("Vocabulary loaded!");
 
     // Create KeyFrame Database
     mpKeyFrameDatabase = std::make_shared<KeyFrameDatabase>(mpVocabulary);
 
-    cout << "Load File" << endl;
-
     // Load the file with an earlier session
     // clock_t start = clock();
-    cout << "Initialization of Atlas from file: " << mStrLoadAtlasFromFile
-         << endl;
+    spdlog::info("Initialization of Atlas from file: {}",
+                 mStrLoadAtlasFromFile);
     bool isRead = LoadAtlas(FileType::BINARY_FILE);
 
     if (!isRead) {
@@ -200,7 +205,7 @@ void System::initialize(bool initFr, const string &strSequence) {
   // Initialize the Tracking thread
   // (it will live in the main thread of execution, the one that called this
   // constructor)
-  cout << "Seq. Name: " << strSequence << endl;
+  spdlog::info("Seq. Name: {}", strSequence);
   mpTracker = std::make_shared<Tracking>(
       this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpAtlas,
       mpKeyFrameDatabase, settings_, strSequence);
@@ -264,9 +269,9 @@ Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight,
                                  const vector<IMU::Point> &vImuMeas,
                                  string filename) {
   if (!sensorType().isStereo()) {
-    cerr << "ERROR: you called TrackStereo but input sensor was not set to "
-            "Stereo nor Stereo-Inertial."
-         << endl;
+    spdlog::error(
+        "ERROR: you called TrackStereo but input sensor was not set to "
+        "Stereo nor Stereo-Inertial.");
     exit(-1);
   }
 
diff --git a/src/Tracking.cc b/src/Tracking.cc
index cecf1f9a690..b7ea43a5d2d 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -441,7 +441,7 @@ void Tracking::PrintTimeStats() {
             << std::endl
             << std::endl;
 
-  f << "Numb exec: " << mpLoopClosing->nLoop << std::endl;
+  f << "Num exec: " << mpLoopClosing->nLoop << std::endl;
   std::cout << "Num exec: " << mpLoopClosing->nLoop << std::endl;
   average = calcAverage(mpLoopClosing->vnLoopKFs);
   deviation = calcDeviation(mpLoopClosing->vnLoopKFs, average);
@@ -472,7 +472,7 @@ void Tracking::PrintTimeStats() {
             << std::endl
             << std::endl;
 
-  f << "Numb exec: " << mpLoopClosing->nMerges << std::endl;
+  f << "Num exec: " << mpLoopClosing->nMerges << std::endl;
   std::cout << "Num exec: " << mpLoopClosing->nMerges << std::endl;
   average = calcAverage(mpLoopClosing->vnMergeKFs);
   deviation = calcDeviation(mpLoopClosing->vnMergeKFs, average);
@@ -503,9 +503,9 @@ void Tracking::PrintTimeStats() {
             << std::endl
             << std::endl;
 
-  f << "Numb exec: " << mpLoopClosing->nFGBA_exec << std::endl;
+  f << "Num exec: " << mpLoopClosing->nFGBA_exec << std::endl;
   std::cout << "Num exec: " << mpLoopClosing->nFGBA_exec << std::endl;
-  f << "Numb abort: " << mpLoopClosing->nFGBA_abort << std::endl;
+  f << "Num abort: " << mpLoopClosing->nFGBA_abort << std::endl;
   std::cout << "Num abort: " << mpLoopClosing->nFGBA_abort << std::endl;
   average = calcAverage(mpLoopClosing->vnGBAKFs);
   deviation = calcDeviation(mpLoopClosing->vnGBAKFs, average);

From da459a119a1160bec354397486ea99d52f943497 Mon Sep 17 00:00:00 2001
From: Aaron Marburg <amarburg@uw.edu>
Date: Wed, 31 Dec 2025 15:28:35 +0000
Subject: [PATCH 2/9] Implement SettingsLoader

---
 include/Settings.h    |  88 +++----
 include/System.h      |  15 +-
 src/KeyFrame.cc       |  12 +-
 src/Settings.cc       | 464 -------------------------------------
 src/SettingsLoader.cc | 527 ++++++++++++++++++++++++++++++++++++++++++
 src/System.cc         |  66 +++---
 6 files changed, 630 insertions(+), 542 deletions(-)
 create mode 100644 src/SettingsLoader.cc

diff --git a/include/Settings.h b/include/Settings.h
index a602dd56799..b743d81a5ca 100644
--- a/include/Settings.h
+++ b/include/Settings.h
@@ -35,16 +35,64 @@
 #include <vector>
 
 #include "CameraModels/GeometricCamera.h"
+#include "Thirdparty/tl/expected.hpp"
 #include "Types.h"
 
 namespace ORB_SLAM3 {
 
 class System;
+class Settings;
 
 // TODO: change to double instead of float
 
+class SettingsLoader {
+ public:
+  typedef tl::expected<std::shared_ptr<Settings>, bool> Expected;
+  static Expected load(const std::string& configFile, const SensorType sensor);
+
+  explicit SettingsLoader(const SensorType sensor);
+  Expected load(const std::string& configFile);
+
+  void readCamera1(cv::FileStorage& fSettings);
+  void readCamera2(cv::FileStorage& fSettings);
+  void readImageInfo(cv::FileStorage& fSettings);
+  void readIMU(cv::FileStorage& fSettings);
+  void readRGBD(cv::FileStorage& fSettings);
+  void readORB(cv::FileStorage& fSettings);
+  void readViewer(cv::FileStorage& fSettings);
+  void readLoadAndSave(cv::FileStorage& fSettings);
+  void readOtherParameters(cv::FileStorage& fSettings);
+
+ private:
+  std::shared_ptr<Settings> settings_;
+
+  template <typename T>
+  T readParameter(cv::FileStorage& fSettings, const std::string& name,
+                  bool& found, const bool required = true) {
+    cv::FileNode node = fSettings[name];
+    if (node.empty()) {
+      if (required) {
+        std::cerr << name << " required parameter does not exist, aborting..."
+                  << std::endl;
+        exit(-1);
+      } else {
+        std::cerr << name << " optional parameter does not exist..."
+                  << std::endl;
+        found = false;
+        return T();
+      }
+
+    } else {
+      found = true;
+      return (T)node;
+    }
+  }
+};
+
 class Settings {
  public:
+  friend class SettingsLoader;
+
   /*
    * Enum for the different camera types implemented
    */
@@ -56,13 +104,6 @@ class Settings {
   Settings() = delete;
 
   explicit Settings(const SensorType sensor);
-
-  /*
-   * Constructor from file
-   */
-  Settings(const std::string& configFile, const SensorType sensor);
-
-  // Copy constructor
   Settings(const Settings&) = default;
 
   ~Settings();
@@ -142,16 +183,6 @@ class Settings {
   cv::Mat M1r() { return M1r_; }
   cv::Mat M2r() { return M2r_; }
 
-  void readCamera1(cv::FileStorage& fSettings);
-  void readCamera2(cv::FileStorage& fSettings);
-  void readImageInfo(cv::FileStorage& fSettings);
-  void readIMU(cv::FileStorage& fSettings);
-  void readRGBD(cv::FileStorage& fSettings);
-  void readORB(cv::FileStorage& fSettings);
-  void readViewer(cv::FileStorage& fSettings);
-  void readLoadAndSave(cv::FileStorage& fSettings);
-  void readOtherParameters(cv::FileStorage& fSettings);
-
   // For PinHole,       k = {fx, fy, cx, cy}, and dist can be 0, 4 or 5 params
   // For Rectified,     k = {fx, fy, cx, cy}  and dist is ignored
   // For KannalaBrandt, k = {fx, fy, cx, cy, k0, k1, k2, k3};
@@ -243,28 +274,5 @@ class Settings {
 
   bool loopClosing_;
   std::string strVocFile_;
-
- private:
-  template <typename T>
-  T readParameter(cv::FileStorage& fSettings, const std::string& name,
-                  bool& found, const bool required = true) {
-    cv::FileNode node = fSettings[name];
-    if (node.empty()) {
-      if (required) {
-        std::cerr << name << " required parameter does not exist, aborting..."
-                  << std::endl;
-        exit(-1);
-      } else {
-        std::cerr << name << " optional parameter does not exist..."
-                  << std::endl;
-        found = false;
-        return T();
-      }
-
-    } else {
-      found = true;
-      return (T)node;
-    }
-  }
 };
 };  // namespace ORB_SLAM3
diff --git a/include/System.h b/include/System.h
index 034092d09ae..a5a0ce7f270 100644
--- a/include/System.h
+++ b/include/System.h
@@ -56,11 +56,14 @@ class LocalMapping;
 class LoopClosing;
 class Settings;
 
-typedef tl::expected<std::shared_ptr<System>, bool> FactoryExpected;
-
 class SystemFactory {
  public:
-  static FactoryExpected create(const std::shared_ptr<Settings> &settings);
+  typedef tl::expected<std::shared_ptr<System>, bool> Expected;
+
+  static Expected create(const std::shared_ptr<Settings> &settings);
+
+  static Expected create(const std::string &configFile,
+                         const SensorType sensor);
 };
 
 class System {
@@ -75,9 +78,9 @@ class System {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and
   // Viewer threads.
-  System(const string &strVocFile, const string &strSettingsFile,
-         const SensorType sensor, bool initFr = false,
-         const string &strSequence = std::string());
+  // System(const string &strVocFile, const string &strSettingsFile,
+  //        const SensorType sensor, bool initFr = false,
+  //        const string &strSequence = std::string());
 
   // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and
   // Viewer threads.
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index 1be7766748b..857c42fd46f 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -71,10 +71,10 @@ KeyFrame::KeyFrame()
       mb(0),
       mThDepth(0),
       N(0),
-      mvKeys(static_cast<vector<cv::KeyPoint>>(NULL)),
-      mvKeysUn(static_cast<vector<cv::KeyPoint>>(NULL)),
-      mvuRight(static_cast<vector<float>>(NULL)),
-      mvDepth(static_cast<vector<float>>(NULL)),
+      mvKeys(),
+      mvKeysUn(),
+      mvuRight(),
+      mvDepth(),
       mnScaleLevels(0),
       mfScaleFactor(0),
       mfLogScaleFactor(0),
@@ -85,8 +85,8 @@ KeyFrame::KeyFrame()
       mnMinY(0),
       mnMaxX(0),
       mnMaxY(0),
-      mPrevKF(static_cast<KeyFrame *>(NULL)),
-      mNextKF(static_cast<KeyFrame *>(NULL)),
+      mPrevKF(nullptr),
+      mNextKF(nullptr),
       mbFirstConnection(true),
       mpParent(NULL),
       mbNotErase(false),
diff --git a/src/Settings.cc b/src/Settings.cc
index a302668432f..736aa162f97 100644
--- a/src/Settings.cc
+++ b/src/Settings.cc
@@ -35,102 +35,6 @@
 
 namespace ORB_SLAM3 {
 
-template <>
-float Settings::readParameter<float>(cv::FileStorage& fSettings,
-                                     const std::string& name, bool& found,
-                                     const bool required) {
-  cv::FileNode node = fSettings[name];
-  if (node.empty()) {
-    if (required) {
-      std::cerr << name << " required parameter does not exist, aborting..."
-                << std::endl;
-      exit(-1);
-    } else {
-      std::cerr << name << " optional parameter does not exist..." << std::endl;
-      found = false;
-      return 0.0f;
-    }
-  } else if (!node.isReal()) {
-    std::cerr << name << " parameter must be a real number, aborting..."
-              << std::endl;
-    exit(-1);
-  } else {
-    found = true;
-    return node.real();
-  }
-}
-
-template <>
-int Settings::readParameter<int>(cv::FileStorage& fSettings,
-                                 const std::string& name, bool& found,
-                                 const bool required) {
-  cv::FileNode node = fSettings[name];
-  if (node.empty()) {
-    if (required) {
-      std::cerr << name << " required parameter does not exist, aborting..."
-                << std::endl;
-      exit(-1);
-    } else {
-      std::cerr << name << " optional parameter does not exist..." << std::endl;
-      found = false;
-      return 0;
-    }
-  } else if (!node.isInt()) {
-    std::cerr << name << " parameter must be an integer number, aborting..."
-              << std::endl;
-    exit(-1);
-  } else {
-    found = true;
-    return node.operator int();
-  }
-}
-
-template <>
-string Settings::readParameter<string>(cv::FileStorage& fSettings,
-                                       const std::string& name, bool& found,
-                                       const bool required) {
-  cv::FileNode node = fSettings[name];
-  if (node.empty()) {
-    if (required) {
-      std::cerr << name << " required parameter does not exist, aborting..."
-                << std::endl;
-      exit(-1);
-    } else {
-      std::cerr << name << " optional parameter does not exist..." << std::endl;
-      found = false;
-      return string();
-    }
-  } else if (!node.isString()) {
-    std::cerr << name << " parameter must be a string, aborting..."
-              << std::endl;
-    exit(-1);
-  } else {
-    found = true;
-    return node.string();
-  }
-}
-
-template <>
-cv::Mat Settings::readParameter<cv::Mat>(cv::FileStorage& fSettings,
-                                         const std::string& name, bool& found,
-                                         const bool required) {
-  cv::FileNode node = fSettings[name];
-  if (node.empty()) {
-    if (required) {
-      std::cerr << name << " required parameter does not exist, aborting..."
-                << std::endl;
-      exit(-1);
-    } else {
-      std::cerr << name << " optional parameter does not exist..." << std::endl;
-      found = false;
-      return cv::Mat();
-    }
-  } else {
-    found = true;
-    return node.mat();
-  }
-}
-
 Settings::Settings(const SensorType sensor)
     : bNeedToUndistort_(false),
       bNeedToRectify_(false),
@@ -145,68 +49,6 @@ Settings::Settings(const SensorType sensor)
   // }
 }
 
-Settings::Settings(const std::string& configFile, const SensorType sensor)
-    : bNeedToUndistort_(false),
-      bNeedToRectify_(false),
-      bNeedToResize1_(false),
-      bNeedToResize2_(false),
-      loopClosing_(true),
-      sensor_(sensor),
-      imageViewerScale_(1.0f) {
-  // Open settings file
-  cv::FileStorage fSettings(configFile, cv::FileStorage::READ);
-  if (!fSettings.isOpened()) {
-    cerr << "[ERROR]: could not open configuration file at: " << configFile
-         << endl;
-    cerr << "Aborting..." << endl;
-
-    exit(-1);
-  } else {
-    spdlog::info("Loading settings from {}", configFile);
-  }
-
-  // Read first camera
-  readCamera1(fSettings);
-  spdlog::info("\t-Loaded camera 1");
-
-  // Read second camera if stereo (not rectified)
-  if (sensor_ == SensorType::STEREO || sensor_ == SensorType::IMU_STEREO) {
-    readCamera2(fSettings);
-    spdlog::info("\t-Loaded camera 2");
-  }
-
-  // Read image info
-  readImageInfo(fSettings);
-  spdlog::info("\t-Loaded image info");
-
-  if (sensor_ == SensorType::IMU_MONOCULAR ||
-      sensor_ == SensorType::IMU_STEREO || sensor_ == SensorType::IMU_RGBD) {
-    readIMU(fSettings);
-    spdlog::info("\t-Loaded IMU calibration");
-  }
-
-  if (sensor_ == SensorType::RGBD || sensor_ == SensorType::IMU_RGBD) {
-    readRGBD(fSettings);
-    spdlog::info("\t-Loaded RGB-D calibration");
-  }
-
-  readORB(fSettings);
-  spdlog::info("\t-Loaded ORB settings");
-  readViewer(fSettings);
-  spdlog::info("\t-Loaded viewer settings");
-  readLoadAndSave(fSettings);
-  spdlog::info("\t-Loaded Atlas settings");
-  readOtherParameters(fSettings);
-  spdlog::info("\t-Loaded misc parameters");
-
-  if (bNeedToRectify_) {
-    precomputeRectificationMaps();
-    spdlog::info("\t-Computed rectification maps");
-  }
-
-  spdlog::info("----------------------------------");
-}
-
 Settings::~Settings() { ; }
 
 bool Settings::validate(void) {
@@ -232,153 +74,6 @@ bool Settings::validate(void) {
   return true;
 }
 
-void Settings::readCamera1(cv::FileStorage& fSettings) {
-  bool found;
-
-  // Read camera model
-  string cameraModel = readParameter<string>(fSettings, "Camera.type", found);
-
-  vector<float> vCalibration, vDistortion;
-  if (cameraModel == "PinHole") {
-    // Read intrinsic parameters
-    float fx = readParameter<float>(fSettings, "Camera1.fx", found);
-    float fy = readParameter<float>(fSettings, "Camera1.fy", found);
-    float cx = readParameter<float>(fSettings, "Camera1.cx", found);
-    float cy = readParameter<float>(fSettings, "Camera1.cy", found);
-
-    vCalibration = {fx, fy, cx, cy};
-
-    setMonoCamera(PinHole, vCalibration);
-
-    // Check if it is a distorted PinHole
-    readParameter<float>(fSettings, "Camera1.k1", found, false);
-    if (found) {
-      readParameter<float>(fSettings, "Camera1.k3", found, false);
-      if (found) {
-        vDistortion.resize(5);
-        vDistortion[4] = readParameter<float>(fSettings, "Camera1.k3", found);
-      } else {
-        vDistortion.resize(4);
-      }
-      vDistortion[0] = readParameter<float>(fSettings, "Camera1.k1", found);
-      vDistortion[1] = readParameter<float>(fSettings, "Camera1.k2", found);
-      vDistortion[2] = readParameter<float>(fSettings, "Camera1.p1", found);
-      vDistortion[3] = readParameter<float>(fSettings, "Camera1.p2", found);
-    }
-
-    setMonoCamera(PinHole, vCalibration, vDistortion);
-
-  } else if (cameraModel == "Rectified") {
-    // Read intrinsic parameters
-    float fx = readParameter<float>(fSettings, "Camera1.fx", found);
-    float fy = readParameter<float>(fSettings, "Camera1.fy", found);
-    float cx = readParameter<float>(fSettings, "Camera1.cx", found);
-    float cy = readParameter<float>(fSettings, "Camera1.cy", found);
-
-    vCalibration = {fx, fy, cx, cy};
-
-    setMonoCamera(Rectified, vCalibration, {});
-
-    // Rectified images are assumed to be ideal PinHole images (no distortion)
-  } else if (cameraModel == "KannalaBrandt8") {
-    // Read intrinsic parameters
-    float fx = readParameter<float>(fSettings, "Camera1.fx", found);
-    float fy = readParameter<float>(fSettings, "Camera1.fy", found);
-    float cx = readParameter<float>(fSettings, "Camera1.cx", found);
-    float cy = readParameter<float>(fSettings, "Camera1.cy", found);
-
-    float k0 = readParameter<float>(fSettings, "Camera1.k1", found);
-    float k1 = readParameter<float>(fSettings, "Camera1.k2", found);
-    float k2 = readParameter<float>(fSettings, "Camera1.k3", found);
-    float k3 = readParameter<float>(fSettings, "Camera1.k4", found);
-
-    vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
-
-    setMonoCamera(KannalaBrandt, vCalibration, {});
-
-    // if (sensor_ == SensorType::STEREO || sensor_ == SensorType::IMU_STEREO) {
-    //   int colBegin =
-    //       readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
-    //   int colEnd =
-    //       readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
-    //   vector<int> vOverlapping = {colBegin, colEnd};
-
-    //   dynamic_cast<KannalaBrandt8&>(*calibration1_).mvLappingArea =
-    //       vOverlapping;
-    //}
-  } else {
-    cerr << "Error: " << cameraModel << " not known" << endl;
-    exit(-1);
-  }
-}
-
-void Settings::readCamera2(cv::FileStorage& fSettings) {
-  bool found;
-  vector<float> vCalibration, vDistortion;
-  if (cameraType_ == PinHole) {
-    // Read intrinsic parameters
-    float fx = readParameter<float>(fSettings, "Camera2.fx", found);
-    float fy = readParameter<float>(fSettings, "Camera2.fy", found);
-    float cx = readParameter<float>(fSettings, "Camera2.cx", found);
-    float cy = readParameter<float>(fSettings, "Camera2.cy", found);
-
-    vCalibration = {fx, fy, cx, cy};
-
-    // Check if it is a distorted PinHole
-    readParameter<float>(fSettings, "Camera2.k1", found, false);
-    if (found) {
-      readParameter<float>(fSettings, "Camera2.k3", found, false);
-      if (found) {
-        vDistortion.resize(5);
-        vDistortion[4] = readParameter<float>(fSettings, "Camera2.k3", found);
-      } else {
-        vDistortion.resize(4);
-      }
-      vDistortion[0] = readParameter<float>(fSettings, "Camera2.k1", found);
-      vDistortion[1] = readParameter<float>(fSettings, "Camera2.k2", found);
-      vDistortion[2] = readParameter<float>(fSettings, "Camera2.p1", found);
-      vDistortion[3] = readParameter<float>(fSettings, "Camera2.p2", found);
-    }
-
-  } else if (cameraType_ == KannalaBrandt) {
-    // Read intrinsic parameters
-    float fx = readParameter<float>(fSettings, "Camera2.fx", found);
-    float fy = readParameter<float>(fSettings, "Camera2.fy", found);
-    float cx = readParameter<float>(fSettings, "Camera2.cx", found);
-    float cy = readParameter<float>(fSettings, "Camera2.cy", found);
-
-    float k0 = readParameter<float>(fSettings, "Camera1.k1", found);
-    float k1 = readParameter<float>(fSettings, "Camera1.k2", found);
-    float k2 = readParameter<float>(fSettings, "Camera1.k3", found);
-    float k3 = readParameter<float>(fSettings, "Camera1.k4", found);
-
-    vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
-
-    // int colBegin =
-    //     readParameter<int>(fSettings, "Camera2.overlappingBegin", found);
-    // int colEnd = readParameter<int>(fSettings, "Camera2.overlappingEnd",
-    // found); vector<int> vOverlapping = {colBegin, colEnd};
-
-    // dynamic_cast<KannalaBrandt8&>(*calibration2_).mvLappingArea =
-    // vOverlapping;
-  }
-
-  float thDepth = readParameter<float>(fSettings, "Stereo.ThDepth", found);
-
-  // Load stereo extrinsic calibration
-  if (cameraType_ == Rectified) {
-    const float baseline = readParameter<float>(fSettings, "Stereo.b", found);
-    // setRightCamera( vCalibration, vDistortion, baseline, thDepth );
-
-    b_ = baseline;
-    bf_ = b_ * calibration1_->getParameter(0);
-
-  } else {
-    cv::Mat cvTlr = readParameter<cv::Mat>(fSettings, "Stereo.T_c1_c2", found);
-    setRightCamera(vCalibration, vDistortion, cvTlr, thDepth);
-  }
-}
-
 //===
 
 void Settings::setMonoCamera(CameraType type, const std::vector<float>& k,
@@ -497,165 +192,6 @@ void Settings::setImageSize(int width, int height) {
   bRGB_ = false;
 }
 
-void Settings::readImageInfo(cv::FileStorage& fSettings) {
-  bool found;
-  // Read original and desired image dimensions
-  int originalRows = readParameter<int>(fSettings, "Camera.height", found);
-  int originalCols = readParameter<int>(fSettings, "Camera.width", found);
-  originalImSize_.width = originalCols;
-  originalImSize_.height = originalRows;
-
-  newImSize_ = originalImSize_;
-  int newHeigh =
-      readParameter<int>(fSettings, "Camera.newHeight", found, false);
-  if (found) {
-    bNeedToResize1_ = true;
-    newImSize_.height = newHeigh;
-
-    if (!bNeedToRectify_) {
-      // Update calibration
-      float scaleRowFactor = static_cast<float>(newImSize_.height) /
-                             static_cast<float>(originalImSize_.height);
-      calibration1_->setParameter(
-          calibration1_->getParameter(1) * scaleRowFactor, 1);
-      calibration1_->setParameter(
-          calibration1_->getParameter(3) * scaleRowFactor, 3);
-
-      if ((sensor_ == SensorType::STEREO ||
-           sensor_ == SensorType::IMU_STEREO) &&
-          cameraType_ != Rectified) {
-        calibration2_->setParameter(
-            calibration2_->getParameter(1) * scaleRowFactor, 1);
-        calibration2_->setParameter(
-            calibration2_->getParameter(3) * scaleRowFactor, 3);
-      }
-    }
-  }
-
-  int newWidth = readParameter<int>(fSettings, "Camera.newWidth", found, false);
-  if (found) {
-    bNeedToResize1_ = true;
-    newImSize_.width = newWidth;
-
-    if (!bNeedToRectify_) {
-      // Update calibration
-      float scaleColFactor = static_cast<float>(newImSize_.width) /
-                             static_cast<float>(originalImSize_.width);
-      calibration1_->setParameter(
-          calibration1_->getParameter(0) * scaleColFactor, 0);
-      calibration1_->setParameter(
-          calibration1_->getParameter(2) * scaleColFactor, 2);
-
-      if ((sensor_ == SensorType::STEREO ||
-           sensor_ == SensorType::IMU_STEREO) &&
-          cameraType_ != Rectified) {
-        calibration2_->setParameter(
-            calibration2_->getParameter(0) * scaleColFactor, 0);
-        calibration2_->setParameter(
-            calibration2_->getParameter(2) * scaleColFactor, 2);
-
-        if (cameraType_ == KannalaBrandt) {
-          dynamic_cast<KannalaBrandt8*>(calibration1_.get())
-              ->mvLappingArea[0] *= scaleColFactor;
-          dynamic_cast<KannalaBrandt8*>(calibration1_.get())
-              ->mvLappingArea[1] *= scaleColFactor;
-
-          dynamic_cast<KannalaBrandt8*>(calibration2_.get())
-              ->mvLappingArea[0] *= scaleColFactor;
-          dynamic_cast<KannalaBrandt8*>(calibration2_.get())
-              ->mvLappingArea[1] *= scaleColFactor;
-        }
-      }
-    }
-  }
-
-  fps_ = readParameter<int>(fSettings, "Camera.fps", found);
-  bRGB_ = static_cast<bool>(readParameter<int>(fSettings, "Camera.RGB", found));
-}
-
-void Settings::readIMU(cv::FileStorage& fSettings) {
-  bool found;
-  noiseGyro_ = readParameter<float>(fSettings, "IMU.NoiseGyro", found);
-  noiseAcc_ = readParameter<float>(fSettings, "IMU.NoiseAcc", found);
-  gyroWalk_ = readParameter<float>(fSettings, "IMU.GyroWalk", found);
-  accWalk_ = readParameter<float>(fSettings, "IMU.AccWalk", found);
-  imuFrequency_ = readParameter<float>(fSettings, "IMU.Frequency", found);
-
-  cv::Mat cvTbc = readParameter<cv::Mat>(fSettings, "IMU.T_b_c1", found);
-  Tbc_ = Converter::toSophus(cvTbc);
-
-  readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false);
-  if (found) {
-    insertKFsWhenLost_ = static_cast<bool>(
-        readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false));
-  } else {
-    insertKFsWhenLost_ = true;
-  }
-}
-
-void Settings::readRGBD(cv::FileStorage& fSettings) {
-  bool found;
-
-  depthMapFactor_ =
-      readParameter<float>(fSettings, "RGBD.DepthMapFactor", found);
-  thDepth_ = readParameter<float>(fSettings, "Stereo.ThDepth", found);
-  b_ = readParameter<float>(fSettings, "Stereo.b", found);
-  bf_ = b_ * calibration1_->getParameter(0);
-}
-
-void Settings::readORB(cv::FileStorage& fSettings) {
-  bool found;
-
-  nFeatures_ = readParameter<int>(fSettings, "ORBextractor.nFeatures", found);
-  scaleFactor_ =
-      readParameter<float>(fSettings, "ORBextractor.scaleFactor", found);
-  nLevels_ = readParameter<int>(fSettings, "ORBextractor.nLevels", found);
-  initThFAST_ = readParameter<int>(fSettings, "ORBextractor.iniThFAST", found);
-  minThFAST_ = readParameter<int>(fSettings, "ORBextractor.minThFAST", found);
-}
-
-void Settings::readViewer(cv::FileStorage& fSettings) {
-  bool found;
-
-  useViewer_ = readParameter<int>(fSettings, "Viewer.Enable", found);
-  keyFrameSize_ = readParameter<float>(fSettings, "Viewer.KeyFrameSize", found);
-  keyFrameLineWidth_ =
-      readParameter<float>(fSettings, "Viewer.KeyFrameLineWidth", found);
-  graphLineWidth_ =
-      readParameter<float>(fSettings, "Viewer.GraphLineWidth", found);
-  pointSize_ = readParameter<float>(fSettings, "Viewer.PointSize", found);
-  cameraSize_ = readParameter<float>(fSettings, "Viewer.CameraSize", found);
-  cameraLineWidth_ =
-      readParameter<float>(fSettings, "Viewer.CameraLineWidth", found);
-  viewPointX_ = readParameter<float>(fSettings, "Viewer.ViewpointX", found);
-  viewPointY_ = readParameter<float>(fSettings, "Viewer.ViewpointY", found);
-  viewPointZ_ = readParameter<float>(fSettings, "Viewer.ViewpointZ", found);
-  viewPointF_ = readParameter<float>(fSettings, "Viewer.ViewpointF", found);
-  imageViewerScale_ =
-      readParameter<float>(fSettings, "Viewer.imageViewScale", found, false);
-
-  if (!found) imageViewerScale_ = 1.0f;
-}
-
-void Settings::readLoadAndSave(cv::FileStorage& fSettings) {
-  bool found;
-
-  sLoadFrom_ = readParameter<string>(fSettings, "System.LoadAtlasFromFile",
-                                     found, false);
-  sSaveto_ =
-      readParameter<string>(fSettings, "System.SaveAtlasToFile", found, false);
-}
-
-void Settings::readOtherParameters(cv::FileStorage& fSettings) {
-  bool found;
-
-  thFarPoints_ =
-      readParameter<float>(fSettings, "System.thFarPoints", found, false);
-
-  loopClosing_ = static_cast<bool>(
-      readParameter<int>(fSettings, "System.loopClosing", found, true));
-}
-
 void Settings::precomputeRectificationMaps() {
   // Precompute rectification maps, new calibrations, ...
   cv::Mat K1 = dynamic_cast<Pinhole&>(*calibration1_).toK();
diff --git a/src/SettingsLoader.cc b/src/SettingsLoader.cc
new file mode 100644
index 00000000000..4baf6108172
--- /dev/null
+++ b/src/SettingsLoader.cc
@@ -0,0 +1,527 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
+ * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+ * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
+ * University of Zaragoza.
+ *
+ * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
+ * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
+ * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <filesystem>  // NOLINT {build/c++17}
+#include <iostream>
+#include <memory>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/core/persistence.hpp>
+#include <string>
+#include <vector>
+
+#include "CameraModels/KannalaBrandt8.h"
+#include "CameraModels/Pinhole.h"
+#include "Settings.h"
+#include "System.h"
+
+namespace ORB_SLAM3 {
+
+template <>
+float SettingsLoader::readParameter<float>(cv::FileStorage& fSettings,
+                                           const std::string& name, bool& found,
+                                           const bool required) {
+  cv::FileNode node = fSettings[name];
+  if (node.empty()) {
+    if (required) {
+      std::cerr << name << " required parameter does not exist, aborting..."
+                << std::endl;
+      exit(-1);
+    } else {
+      std::cerr << name << " optional parameter does not exist..." << std::endl;
+      found = false;
+      return 0.0f;
+    }
+  } else if (!node.isReal()) {
+    std::cerr << name << " parameter must be a real number, aborting..."
+              << std::endl;
+    exit(-1);
+  } else {
+    found = true;
+    return node.real();
+  }
+}
+
+template <>
+int SettingsLoader::readParameter<int>(cv::FileStorage& fSettings,
+                                       const std::string& name, bool& found,
+                                       const bool required) {
+  cv::FileNode node = fSettings[name];
+  if (node.empty()) {
+    if (required) {
+      std::cerr << name << " required parameter does not exist, aborting..."
+                << std::endl;
+      exit(-1);
+    } else {
+      std::cerr << name << " optional parameter does not exist..." << std::endl;
+      found = false;
+      return 0;
+    }
+  } else if (!node.isInt()) {
+    std::cerr << name << " parameter must be an integer number, aborting..."
+              << std::endl;
+    exit(-1);
+  } else {
+    found = true;
+    return node.operator int();
+  }
+}
+
+template <>
+string SettingsLoader::readParameter<string>(cv::FileStorage& fSettings,
+                                             const std::string& name,
+                                             bool& found, const bool required) {
+  cv::FileNode node = fSettings[name];
+  if (node.empty()) {
+    if (required) {
+      std::cerr << name << " required parameter does not exist, aborting..."
+                << std::endl;
+      exit(-1);
+    } else {
+      std::cerr << name << " optional parameter does not exist..." << std::endl;
+      found = false;
+      return string();
+    }
+  } else if (!node.isString()) {
+    std::cerr << name << " parameter must be a string, aborting..."
+              << std::endl;
+    exit(-1);
+  } else {
+    found = true;
+    return node.string();
+  }
+}
+
+template <>
+cv::Mat SettingsLoader::readParameter<cv::Mat>(cv::FileStorage& fSettings,
+                                               const std::string& name,
+                                               bool& found,
+                                               const bool required) {
+  cv::FileNode node = fSettings[name];
+  if (node.empty()) {
+    if (required) {
+      std::cerr << name << " required parameter does not exist, aborting..."
+                << std::endl;
+      exit(-1);
+    } else {
+      std::cerr << name << " optional parameter does not exist..." << std::endl;
+      found = false;
+      return cv::Mat();
+    }
+  } else {
+    found = true;
+    return node.mat();
+  }
+}
+
+SettingsLoader::Expected SettingsLoader::load(const std::string& configFile,
+                                              const SensorType sensor) {
+  SettingsLoader loader(sensor);
+  return loader.load(configFile);
+}
+
+SettingsLoader::SettingsLoader(const SensorType sensor)
+    : settings_(std::make_shared<Settings>(sensor)) {}
+
+SettingsLoader::Expected SettingsLoader::load(const std::string& configFile) {
+  // Open settings file
+  cv::FileStorage fSettings(configFile, cv::FileStorage::READ);
+  if (!fSettings.isOpened()) {
+    cerr << "[ERROR]: could not open configuration file at: " << configFile
+         << endl;
+    cerr << "Aborting..." << endl;
+
+    exit(-1);
+  } else {
+    spdlog::info("Loading settings from {}", configFile);
+  }
+
+  // Read first camera
+  readCamera1(fSettings);
+  spdlog::info("\t-Loaded camera 1");
+
+  // Read second camera if stereo (not rectified)
+  if (settings_->sensor_.isStereo()) {
+    readCamera2(fSettings);
+    spdlog::info("\t-Loaded camera 2");
+  }
+
+  // Read image info
+  readImageInfo(fSettings);
+  spdlog::info("\t-Loaded image info");
+
+  if (settings_->sensor_.isImu()) {
+    readIMU(fSettings);
+    spdlog::info("\t-Loaded IMU calibration");
+  }
+
+  if (settings_->sensor_.isRGBD()) {
+    readRGBD(fSettings);
+    spdlog::info("\t-Loaded RGB-D calibration");
+  }
+
+  readORB(fSettings);
+  spdlog::info("\t-Loaded ORB settings");
+  readViewer(fSettings);
+  spdlog::info("\t-Loaded viewer settings");
+  readLoadAndSave(fSettings);
+  spdlog::info("\t-Loaded Atlas settings");
+  readOtherParameters(fSettings);
+  spdlog::info("\t-Loaded misc parameters");
+
+  if (settings_->bNeedToRectify_) {
+    settings_->precomputeRectificationMaps();
+    spdlog::info("\t-Computed rectification maps");
+  }
+
+  spdlog::info("----------------------------------");
+
+  if (settings_->validate()) {
+    return settings_;
+  } else {
+    return tl::make_unexpected(false);
+  }
+}
+
+void SettingsLoader::readCamera1(cv::FileStorage& fSettings) {
+  bool found;
+
+  // Read camera model
+  string cameraModel = readParameter<string>(fSettings, "Camera.type", found);
+
+  vector<float> vCalibration, vDistortion;
+  if (cameraModel == "PinHole") {
+    // Read intrinsic parameters
+    float fx = readParameter<float>(fSettings, "Camera1.fx", found);
+    float fy = readParameter<float>(fSettings, "Camera1.fy", found);
+    float cx = readParameter<float>(fSettings, "Camera1.cx", found);
+    float cy = readParameter<float>(fSettings, "Camera1.cy", found);
+
+    vCalibration = {fx, fy, cx, cy};
+
+    settings_->setMonoCamera(Settings::PinHole, vCalibration);
+
+    // Check if it is a distorted PinHole
+    readParameter<float>(fSettings, "Camera1.k1", found, false);
+    if (found) {
+      readParameter<float>(fSettings, "Camera1.k3", found, false);
+      if (found) {
+        vDistortion.resize(5);
+        vDistortion[4] = readParameter<float>(fSettings, "Camera1.k3", found);
+      } else {
+        vDistortion.resize(4);
+      }
+      vDistortion[0] = readParameter<float>(fSettings, "Camera1.k1", found);
+      vDistortion[1] = readParameter<float>(fSettings, "Camera1.k2", found);
+      vDistortion[2] = readParameter<float>(fSettings, "Camera1.p1", found);
+      vDistortion[3] = readParameter<float>(fSettings, "Camera1.p2", found);
+    }
+
+    settings_->setMonoCamera(Settings::PinHole, vCalibration, vDistortion);
+
+  } else if (cameraModel == "Rectified") {
+    // Read intrinsic parameters
+    float fx = readParameter<float>(fSettings, "Camera1.fx", found);
+    float fy = readParameter<float>(fSettings, "Camera1.fy", found);
+    float cx = readParameter<float>(fSettings, "Camera1.cx", found);
+    float cy = readParameter<float>(fSettings, "Camera1.cy", found);
+
+    vCalibration = {fx, fy, cx, cy};
+
+    settings_->setMonoCamera(Settings::Rectified, vCalibration, {});
+
+    // Rectified images are assumed to be ideal PinHole images (no distortion)
+  } else if (cameraModel == "KannalaBrandt8") {
+    // Read intrinsic parameters
+    float fx = readParameter<float>(fSettings, "Camera1.fx", found);
+    float fy = readParameter<float>(fSettings, "Camera1.fy", found);
+    float cx = readParameter<float>(fSettings, "Camera1.cx", found);
+    float cy = readParameter<float>(fSettings, "Camera1.cy", found);
+
+    float k0 = readParameter<float>(fSettings, "Camera1.k1", found);
+    float k1 = readParameter<float>(fSettings, "Camera1.k2", found);
+    float k2 = readParameter<float>(fSettings, "Camera1.k3", found);
+    float k3 = readParameter<float>(fSettings, "Camera1.k4", found);
+
+    vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
+
+    settings_->setMonoCamera(Settings::KannalaBrandt, vCalibration, {});
+
+    // if (sensor_ == SensorType::STEREO || sensor_ == SensorType::IMU_STEREO) {
+    //   int colBegin =
+    //       readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
+    //   int colEnd =
+    //       readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
+    //   vector<int> vOverlapping = {colBegin, colEnd};
+
+    //   dynamic_cast<KannalaBrandt8&>(*calibration1_).mvLappingArea =
+    //       vOverlapping;
+    //}
+  } else {
+    cerr << "Error: " << cameraModel << " not known" << endl;
+    exit(-1);
+  }
+}
+
+void SettingsLoader::readCamera2(cv::FileStorage& fSettings) {
+  bool found;
+  vector<float> vCalibration, vDistortion;
+  if (settings_->cameraType_ == Settings::PinHole) {
+    // Read intrinsic parameters
+    float fx = readParameter<float>(fSettings, "Camera2.fx", found);
+    float fy = readParameter<float>(fSettings, "Camera2.fy", found);
+    float cx = readParameter<float>(fSettings, "Camera2.cx", found);
+    float cy = readParameter<float>(fSettings, "Camera2.cy", found);
+
+    vCalibration = {fx, fy, cx, cy};
+
+    // Check if it is a distorted PinHole
+    readParameter<float>(fSettings, "Camera2.k1", found, false);
+    if (found) {
+      readParameter<float>(fSettings, "Camera2.k3", found, false);
+      if (found) {
+        vDistortion.resize(5);
+        vDistortion[4] = readParameter<float>(fSettings, "Camera2.k3", found);
+      } else {
+        vDistortion.resize(4);
+      }
+      vDistortion[0] = readParameter<float>(fSettings, "Camera2.k1", found);
+      vDistortion[1] = readParameter<float>(fSettings, "Camera2.k2", found);
+      vDistortion[2] = readParameter<float>(fSettings, "Camera2.p1", found);
+      vDistortion[3] = readParameter<float>(fSettings, "Camera2.p2", found);
+    }
+
+  } else if (settings_->cameraType_ == Settings::KannalaBrandt) {
+    // Read intrinsic parameters
+    float fx = readParameter<float>(fSettings, "Camera2.fx", found);
+    float fy = readParameter<float>(fSettings, "Camera2.fy", found);
+    float cx = readParameter<float>(fSettings, "Camera2.cx", found);
+    float cy = readParameter<float>(fSettings, "Camera2.cy", found);
+
+    float k0 = readParameter<float>(fSettings, "Camera1.k1", found);
+    float k1 = readParameter<float>(fSettings, "Camera1.k2", found);
+    float k2 = readParameter<float>(fSettings, "Camera1.k3", found);
+    float k3 = readParameter<float>(fSettings, "Camera1.k4", found);
+
+    vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
+
+    // int colBegin =
+    //     readParameter<int>(fSettings, "Camera2.overlappingBegin", found);
+    // int colEnd = readParameter<int>(fSettings, "Camera2.overlappingEnd",
+    // found); vector<int> vOverlapping = {colBegin, colEnd};
+
+    // dynamic_cast<KannalaBrandt8&>(*calibration2_).mvLappingArea =
+    // vOverlapping;
+  }
+
+  float thDepth = readParameter<float>(fSettings, "Stereo.ThDepth", found);
+
+  // Load stereo extrinsic calibration
+  if (settings_->cameraType_ == Settings::Rectified) {
+    const float baseline = readParameter<float>(fSettings, "Stereo.b", found);
+    // setRightCamera( vCalibration, vDistortion, baseline, thDepth );
+
+    settings_->b_ = baseline;
+    settings_->bf_ = baseline * settings_->calibration1_->getParameter(0);
+
+  } else {
+    cv::Mat cvTlr = readParameter<cv::Mat>(fSettings, "Stereo.T_c1_c2", found);
+    settings_->setRightCamera(vCalibration, vDistortion, cvTlr, thDepth);
+  }
+}
+
+//===
+
+void SettingsLoader::readImageInfo(cv::FileStorage& fSettings) {
+  bool found;
+  // Read original and desired image dimensions
+  int originalRows = readParameter<int>(fSettings, "Camera.height", found);
+  int originalCols = readParameter<int>(fSettings, "Camera.width", found);
+
+  settings_->setImageSize(originalRows, originalCols);
+
+  // Disable image resizing for now...
+
+  // int newHeigh =
+  //     readParameter<int>(fSettings, "Camera.newHeight", found, false);
+  // if (found) {
+  //   bNeedToResize1_ = true;
+  //   newImSize_.height = newHeigh;
+
+  //   if (!bNeedToRectify_) {
+  //     // Update calibration
+  //     float scaleRowFactor = static_cast<float>(newImSize_.height) /
+  //                            static_cast<float>(originalImSize_.height);
+  //     calibration1_->setParameter(
+  //         calibration1_->getParameter(1) * scaleRowFactor, 1);
+  //     calibration1_->setParameter(
+  //         calibration1_->getParameter(3) * scaleRowFactor, 3);
+
+  //     if ((sensor_ == SensorType::STEREO ||
+  //          sensor_ == SensorType::IMU_STEREO) &&
+  //         cameraType_ != Rectified) {
+  //       calibration2_->setParameter(
+  //           calibration2_->getParameter(1) * scaleRowFactor, 1);
+  //       calibration2_->setParameter(
+  //           calibration2_->getParameter(3) * scaleRowFactor, 3);
+  //     }
+  //   }
+  // }
+
+  // int newWidth = readParameter<int>(fSettings, "Camera.newWidth", found,
+  // false); if (found) {
+  //   bNeedToResize1_ = true;
+  //   newImSize_.width = newWidth;
+
+  //   if (!bNeedToRectify_) {
+  //     // Update calibration
+  //     float scaleColFactor = static_cast<float>(newImSize_.width) /
+  //                            static_cast<float>(originalImSize_.width);
+  //     calibration1_->setParameter(
+  //         calibration1_->getParameter(0) * scaleColFactor, 0);
+  //     calibration1_->setParameter(
+  //         calibration1_->getParameter(2) * scaleColFactor, 2);
+
+  //     if ((sensor_ == SensorType::STEREO ||
+  //          sensor_ == SensorType::IMU_STEREO) &&
+  //         cameraType_ != Rectified) {
+  //       calibration2_->setParameter(
+  //           calibration2_->getParameter(0) * scaleColFactor, 0);
+  //       calibration2_->setParameter(
+  //           calibration2_->getParameter(2) * scaleColFactor, 2);
+
+  //       if (cameraType_ == KannalaBrandt) {
+  //         dynamic_cast<KannalaBrandt8*>(calibration1_.get())
+  //             ->mvLappingArea[0] *= scaleColFactor;
+  //         dynamic_cast<KannalaBrandt8*>(calibration1_.get())
+  //             ->mvLappingArea[1] *= scaleColFactor;
+
+  //         dynamic_cast<KannalaBrandt8*>(calibration2_.get())
+  //             ->mvLappingArea[0] *= scaleColFactor;
+  //         dynamic_cast<KannalaBrandt8*>(calibration2_.get())
+  //             ->mvLappingArea[1] *= scaleColFactor;
+  //       }
+  //     }
+  //   }
+  // }
+
+  settings_->fps_ = readParameter<int>(fSettings, "Camera.fps", found);
+  settings_->bRGB_ =
+      static_cast<bool>(readParameter<int>(fSettings, "Camera.RGB", found));
+}
+
+void SettingsLoader::readIMU(cv::FileStorage& fSettings) {
+  bool found;
+  settings_->noiseGyro_ =
+      readParameter<float>(fSettings, "IMU.NoiseGyro", found);
+  settings_->noiseAcc_ = readParameter<float>(fSettings, "IMU.NoiseAcc", found);
+  settings_->gyroWalk_ = readParameter<float>(fSettings, "IMU.GyroWalk", found);
+  settings_->accWalk_ = readParameter<float>(fSettings, "IMU.AccWalk", found);
+  settings_->imuFrequency_ =
+      readParameter<float>(fSettings, "IMU.Frequency", found);
+
+  cv::Mat cvTbc = readParameter<cv::Mat>(fSettings, "IMU.T_b_c1", found);
+  settings_->Tbc_ = Converter::toSophus(cvTbc);
+
+  readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false);
+  if (found) {
+    settings_->insertKFsWhenLost_ = static_cast<bool>(
+        readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false));
+  } else {
+    settings_->insertKFsWhenLost_ = true;
+  }
+}
+
+void SettingsLoader::readRGBD(cv::FileStorage& fSettings) {
+  bool found;
+
+  settings_->depthMapFactor_ =
+      readParameter<float>(fSettings, "RGBD.DepthMapFactor", found);
+  settings_->thDepth_ =
+      readParameter<float>(fSettings, "Stereo.ThDepth", found);
+  settings_->b_ = readParameter<float>(fSettings, "Stereo.b", found);
+  settings_->bf_ = settings_->b_ * settings_->calibration1_->getParameter(0);
+}
+
+void SettingsLoader::readORB(cv::FileStorage& fSettings) {
+  bool found;
+
+  settings_->nFeatures_ =
+      readParameter<int>(fSettings, "ORBextractor.nFeatures", found);
+  settings_->scaleFactor_ =
+      readParameter<float>(fSettings, "ORBextractor.scaleFactor", found);
+  settings_->nLevels_ =
+      readParameter<int>(fSettings, "ORBextractor.nLevels", found);
+  settings_->initThFAST_ =
+      readParameter<int>(fSettings, "ORBextractor.iniThFAST", found);
+  settings_->minThFAST_ =
+      readParameter<int>(fSettings, "ORBextractor.minThFAST", found);
+}
+
+void SettingsLoader::readViewer(cv::FileStorage& fSettings) {
+  bool found;
+
+  settings_->useViewer_ = readParameter<int>(fSettings, "Viewer.Enable", found);
+  settings_->keyFrameSize_ =
+      readParameter<float>(fSettings, "Viewer.KeyFrameSize", found);
+  settings_->keyFrameLineWidth_ =
+      readParameter<float>(fSettings, "Viewer.KeyFrameLineWidth", found);
+  settings_->graphLineWidth_ =
+      readParameter<float>(fSettings, "Viewer.GraphLineWidth", found);
+  settings_->pointSize_ =
+      readParameter<float>(fSettings, "Viewer.PointSize", found);
+  settings_->cameraSize_ =
+      readParameter<float>(fSettings, "Viewer.CameraSize", found);
+  settings_->cameraLineWidth_ =
+      readParameter<float>(fSettings, "Viewer.CameraLineWidth", found);
+  settings_->viewPointX_ =
+      readParameter<float>(fSettings, "Viewer.ViewpointX", found);
+  settings_->viewPointY_ =
+      readParameter<float>(fSettings, "Viewer.ViewpointY", found);
+  settings_->viewPointZ_ =
+      readParameter<float>(fSettings, "Viewer.ViewpointZ", found);
+  settings_->viewPointF_ =
+      readParameter<float>(fSettings, "Viewer.ViewpointF", found);
+  settings_->imageViewerScale_ =
+      readParameter<float>(fSettings, "Viewer.imageViewScale", found, false);
+
+  if (!found) settings_->imageViewerScale_ = 1.0f;
+}
+
+void SettingsLoader::readLoadAndSave(cv::FileStorage& fSettings) {
+  bool found;
+
+  settings_->sLoadFrom_ = readParameter<string>(
+      fSettings, "System.LoadAtlasFromFile", found, false);
+  settings_->sSaveto_ =
+      readParameter<string>(fSettings, "System.SaveAtlasToFile", found, false);
+}
+
+void SettingsLoader::readOtherParameters(cv::FileStorage& fSettings) {
+  bool found;
+
+  settings_->thFarPoints_ =
+      readParameter<float>(fSettings, "System.thFarPoints", found, false);
+
+  settings_->loopClosing_ = static_cast<bool>(
+      readParameter<int>(fSettings, "System.loopClosing", found, true));
+}
+
+};  // namespace ORB_SLAM3
diff --git a/src/System.cc b/src/System.cc
index 36f273b8774..0c474941c9c 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -48,7 +48,7 @@ namespace ORB_SLAM3 {
 
 Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL;
 
-FactoryExpected SystemFactory::create(
+SystemFactory::Expected SystemFactory::create(
     const std::shared_ptr<Settings> &settings) {
   if (!settings->validate()) {
     return tl::make_unexpected(false);
@@ -57,36 +57,50 @@ FactoryExpected SystemFactory::create(
   return std::make_shared<System>(settings);
 }
 
-System::System(const string &strVocFile, const string &strSettingsFile,
-               const SensorType sensor, bool initFr, const string &strSequence)
-    : mpViewer(nullptr),
-      mbReset(false),
-      mbResetActiveMap(false),
-      mbActivateLocalizationMode(false),
-      mbDeactivateLocalizationMode(false),
-      mbShutDown(false) {
-  // Check settings file
-  cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
-  if (!fsSettings.isOpened()) {
-    cerr << "Failed to open settings file at: " << strSettingsFile << endl;
-    exit(-1);
-  }
+SystemFactory::Expected SystemFactory::create(const std::string &configFile,
+                                              const SensorType sensor) {
+  auto settings = SettingsLoader::load(configFile, sensor);
 
-  cv::FileNode node = fsSettings["File.version"];
-  if (node.empty() || (node.isString() && node.string() != "1.0")) {
-    std::cerr << "UNABLE TO LOAD CONFIG FILE THAT IS NOT VERSION 1.0"
-              << std::endl;
+  if (settings.has_value()) {
+    return std::make_shared<System>(settings.value());
   }
 
-  settings_ = std::make_shared<Settings>(strSettingsFile, sensor);
-
-  // This is currently not loaded from the settings file
-  settings_->strVocFile_ = strVocFile;
-
-  printBanner();
-  initialize(initFr, strSequence);
+  return tl::make_unexpected(false);
 }
 
+//===================================================================
+
+// System::System(const string &strVocFile, const string &strSettingsFile,
+//                const SensorType sensor, bool initFr, const string
+//                &strSequence)
+//     : mpViewer(nullptr),
+//       mbReset(false),
+//       mbResetActiveMap(false),
+//       mbActivateLocalizationMode(false),
+//       mbDeactivateLocalizationMode(false),
+//       mbShutDown(false) {
+//   // Check settings file
+//   cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
+//   if (!fsSettings.isOpened()) {
+//     cerr << "Failed to open settings file at: " << strSettingsFile << endl;
+//     exit(-1);
+//   }
+
+//   cv::FileNode node = fsSettings["File.version"];
+//   if (node.empty() || (node.isString() && node.string() != "1.0")) {
+//     std::cerr << "UNABLE TO LOAD CONFIG FILE THAT IS NOT VERSION 1.0"
+//               << std::endl;
+//   }
+
+//   settings_ = std::make_shared<Settings>(strSettingsFile, sensor);
+
+//   // This is currently not loaded from the settings file
+//   settings_->strVocFile_ = strVocFile;
+
+//   printBanner();
+//   initialize(initFr, strSequence);
+// }
+
 System::System(const std::shared_ptr<Settings> &settings, bool initFr,
                const string &strSequence)
     : mpViewer(),

From 747b05e53ceaf9260dc6e5152ad493641e615055 Mon Sep 17 00:00:00 2001
From: Aaron Marburg <amarburg@uw.edu>
Date: Thu, 1 Jan 2026 00:18:01 +0000
Subject: [PATCH 3/9] shared_ptr for IMU preintegration

---
 include/Frame.h       |  4 ++--
 include/G2oTypes.h    |  9 ++++----
 include/ImuTypes.h    | 10 ++++++---
 include/KeyFrame.h    |  6 +++---
 include/System.h      | 14 ++++---------
 include/Tracking.h    |  2 +-
 src/Frame.cc          | 48 +++++++++++++++++++++----------------------
 src/G2oTypes.cc       |  5 +++--
 src/ImuTypes.cc       |  9 ++++----
 src/KeyFrame.cc       | 12 +++++++++--
 src/SettingsLoader.cc |  1 +
 src/System.cc         | 42 ++++++++++++++++++++++---------------
 src/Tracking.cc       | 40 ++++++++++++++++--------------------
 13 files changed, 109 insertions(+), 93 deletions(-)

diff --git a/include/Frame.h b/include/Frame.h
index 35545e74767..522f752103c 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -273,12 +273,12 @@ class Frame {
   IMU::Calib mImuCalib;
 
   // Imu preintegration from last keyframe
-  IMU::Preintegrated *mpImuPreintegrated;
+  std::shared_ptr<IMU::Preintegrated> mpImuPreintegrated;
   KeyFrame *mpLastKeyFrame;
 
   // Pointer to previous frame
   Frame *mpPrevFrame;
-  IMU::Preintegrated *mpImuPreintegratedFrame;
+  std::shared_ptr<IMU::Preintegrated> mpImuPreintegratedFrame;
 
   // Current and Next Frame id.
   static long unsigned int nNextId;
diff --git a/include/G2oTypes.h b/include/G2oTypes.h
index e29d3ff230c..5bfd4a1d430 100644
--- a/include/G2oTypes.h
+++ b/include/G2oTypes.h
@@ -28,6 +28,7 @@
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
 #include <cmath>
+#include <memory>
 #include <opencv2/core/core.hpp>
 #include <vector>
 
@@ -458,7 +459,7 @@ class EdgeInertial : public g2o::BaseMultiEdge<9, Vector9d> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  explicit EdgeInertial(IMU::Preintegrated* pInt);
+  explicit EdgeInertial(const std::shared_ptr<IMU::Preintegrated>& pInt);
 
   virtual bool read(std::istream& is) { return false; }
   virtual bool write(std::ostream& os) const { return false; }
@@ -499,7 +500,7 @@ class EdgeInertial : public g2o::BaseMultiEdge<9, Vector9d> {
 
   const Eigen::Matrix3d JRg, JVg, JPg;
   const Eigen::Matrix3d JVa, JPa;
-  IMU::Preintegrated* mpInt;
+  std::shared_ptr<IMU::Preintegrated> mpInt;
   const double dt;
   Eigen::Vector3d g;
 };
@@ -510,7 +511,7 @@ class EdgeInertialGS : public g2o::BaseMultiEdge<9, Vector9d> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  explicit EdgeInertialGS(IMU::Preintegrated* pInt);
+  explicit EdgeInertialGS(const std::shared_ptr<IMU::Preintegrated>& pInt);
 
   virtual bool read(std::istream& is) { return false; }
   virtual bool write(std::ostream& os) const { return false; }
@@ -520,7 +521,7 @@ class EdgeInertialGS : public g2o::BaseMultiEdge<9, Vector9d> {
 
   const Eigen::Matrix3d JRg, JVg, JPg;
   const Eigen::Matrix3d JVa, JPa;
-  IMU::Preintegrated* mpInt;
+  std::shared_ptr<IMU::Preintegrated> mpInt;
   const double dt;
   Eigen::Vector3d g, gI;
 
diff --git a/include/ImuTypes.h b/include/ImuTypes.h
index ebad11f769f..6569f8d1398 100644
--- a/include/ImuTypes.h
+++ b/include/ImuTypes.h
@@ -27,6 +27,7 @@
 #include <boost/serialization/serialization.hpp>
 #include <boost/serialization/vector.hpp>
 #include <iostream>
+#include <memory>
 #include <mutex>
 #include <opencv2/core/core.hpp>
 #include <sophus/se3.hpp>
@@ -178,16 +179,19 @@ class Preintegrated {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   Preintegrated(const Bias &b_, const Calib &calib);
-  explicit Preintegrated(Preintegrated *pImuPre);
+
+  // \todo{}  Why not the default copy constructor?
+  explicit Preintegrated(const std::shared_ptr<Preintegrated> &pImuPre);
+
   Preintegrated() = default;
 
   ~Preintegrated() {}
-  void CopyFrom(Preintegrated *pImuPre);
+  void CopyFrom(const std::shared_ptr<Preintegrated> &pImuPre);
   void Initialize(const Bias &b_);
   void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration,
                                const Eigen::Vector3f &angVel, const float &dt);
   void Reintegrate();
-  void MergePrevious(Preintegrated *pPrev);
+  void MergePrevious(const std::shared_ptr<Preintegrated> &pPrev);
   void SetNewBias(const Bias &bu_);
   IMU::Bias GetDeltaBias(const Bias &b_);
 
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
index 5e660d15ac9..f0727ebf612 100644
--- a/include/KeyFrame.h
+++ b/include/KeyFrame.h
@@ -184,7 +184,7 @@ class KeyFrame {
 
     // Inertial variables
     ar & mImuBias;
-    ar & mBackupImuPreintegrated;
+    ar&(*mpBackupImuPreintegrated);
     ar & mImuCalib;
     ar & mBackupPrevKFId;
     ar & mBackupNextKFId;
@@ -414,7 +414,7 @@ class KeyFrame {
   KeyFrame* mPrevKF;
   KeyFrame* mNextKF;
 
-  IMU::Preintegrated* mpImuPreintegrated;
+  std::shared_ptr<IMU::Preintegrated> mpImuPreintegrated;
   IMU::Calib mImuCalib;
 
   unsigned int mnOriginMapId;
@@ -493,7 +493,7 @@ class KeyFrame {
   // Backup variables for inertial
   long long int mBackupPrevKFId;
   long long int mBackupNextKFId;
-  IMU::Preintegrated mBackupImuPreintegrated;
+  std::shared_ptr<IMU::Preintegrated> mpBackupImuPreintegrated;
 
   // Backup for Cameras
   unsigned int mnBackupIdCamera, mnBackupIdCamera2;
diff --git a/include/System.h b/include/System.h
index a5a0ce7f270..14d8a733a94 100644
--- a/include/System.h
+++ b/include/System.h
@@ -162,7 +162,7 @@ class System {
   // http://www.cvlibs.net/datasets/kitti/eval_odometry.php
   void SaveTrajectoryKITTI(const string &filename);
 
-  // TODO: Save/Load functions
+  // \todo{} Serialization is currently broken
   // SaveMap(const string &filename);
   // LoadMap(const string &filename);
 
@@ -236,9 +236,9 @@ class System {
   // System threads: Local Mapping, Loop Closing, Viewer.
   // The Tracking thread "lives" in the main execution thread that creates the
   // System object.
-  std::thread *mptLocalMapping;
-  std::thread *mptLoopClosing;
-  std::thread *mptViewer;
+  std::unique_ptr<std::thread> mptLocalMapping;
+  std::unique_ptr<std::thread> mptLoopClosing;
+  std::unique_ptr<std::thread> mptViewer;
 
   // Reset flag
   std::mutex mMutexReset;
@@ -259,12 +259,6 @@ class System {
   std::vector<cv::KeyPoint> mTrackedKeyPointsUn;
   std::mutex mMutexState;
 
-  //
-  string mStrLoadAtlasFromFile;
-  string mStrSaveAtlasToFile;
-
-  string mStrVocabularyFilePath;
-
   std::shared_ptr<Settings> settings_;
 };
 
diff --git a/include/Tracking.h b/include/Tracking.h
index 3f10a86593a..cc4b73bd31b 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -245,7 +245,7 @@ class Tracking {
   bool mbMapUpdated;
 
   // Imu preintegration from last frame
-  IMU::Preintegrated *mpImuPreintegratedFromLastKF;
+  std::shared_ptr<IMU::Preintegrated> mpImuPreintegratedFromLastKF;
 
   // Queue of IMU measurements between frames
   std::list<IMU::Point> mlQueueImuData;
diff --git a/src/Frame.cc b/src/Frame.cc
index 36fc904d344..335f0de634d 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -51,11 +51,11 @@ float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv;
 cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING);
 
 Frame::Frame()
-    : mpcpi(NULL),
-      mpImuPreintegrated(NULL),
-      mpPrevFrame(NULL),
-      mpImuPreintegratedFrame(NULL),
-      mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+    : mpcpi(),
+      mpImuPreintegrated(),
+      mpPrevFrame(),
+      mpImuPreintegratedFrame(),
+      mpReferenceKF(),
       mbIsSet(false),
       mbImuPreintegrated(false),
       mbHasPose(false),
@@ -159,7 +159,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
              const std::shared_ptr<GeometricCamera> &pCamera, Frame *pPrevF,
              const IMU::Calib &ImuCalib)
-    : mpcpi(NULL),
+    : mpcpi(nullptr),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractorLeft),
       mpORBextractorRight(extractorRight),
@@ -170,10 +170,10 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mbf(bf),
       mThDepth(thDepth),
       mImuCalib(ImuCalib),
-      mpImuPreintegrated(NULL),
+      mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
-      mpImuPreintegratedFrame(NULL),
-      mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mpImuPreintegratedFrame(nullptr),
+      mpReferenceKF(static_cast<KeyFrame *>(nullptr)),
       mbIsSet(false),
       mbImuPreintegrated(false),
       mpCamera(pCamera),
@@ -231,7 +231,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
           .count();
 #endif
 
-  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
+  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(nullptr));
   mvbOutlier = vector<bool>(N, false);
   mmProjectPoints.clear();
   mmMatchedInImage.clear();
@@ -285,7 +285,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
              const std::shared_ptr<GeometricCamera> &pCamera, Frame *pPrevF,
              const IMU::Calib &ImuCalib)
-    : mpcpi(NULL),
+    : mpcpi(nullptr),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractor),
       mpORBextractorRight(nullptr),
@@ -296,10 +296,10 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
       mbf(bf),
       mThDepth(thDepth),
       mImuCalib(ImuCalib),
-      mpImuPreintegrated(NULL),
+      mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
-      mpImuPreintegratedFrame(NULL),
-      mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mpImuPreintegratedFrame(nullptr),
+      mpReferenceKF(static_cast<KeyFrame *>(nullptr)),
       mbIsSet(false),
       mbImuPreintegrated(false),
       mpCamera(pCamera),
@@ -342,7 +342,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
 
   ComputeStereoFromRGBD(imDepth);
 
-  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
+  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(nullptr));
 
   mmProjectPoints.clear();
   mmMatchedInImage.clear();
@@ -397,7 +397,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
              const std::shared_ptr<GeometricCamera> &pCamera, cv::Mat &distCoef,
              const float &bf, const float &thDepth, Frame *pPrevF,
              const IMU::Calib &ImuCalib)
-    : mpcpi(NULL),
+    : mpcpi(nullptr),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractor),
       mpORBextractorRight(nullptr),
@@ -408,9 +408,9 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
       mbf(bf),
       mThDepth(thDepth),
       mImuCalib(ImuCalib),
-      mpImuPreintegrated(NULL),
+      mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
-      mpImuPreintegratedFrame(NULL),
+      mpImuPreintegratedFrame(nullptr),
       mpReferenceKF(nullptr),
       mbIsSet(false),
       mbImuPreintegrated(false),
@@ -456,10 +456,10 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
   mvDepth = vector<float>(N, -1);
   mnCloseMPs = 0;
 
-  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(NULL));
+  mvpMapPoints = vector<MapPoint *>(N, static_cast<MapPoint *>(nullptr));
 
   mmProjectPoints.clear();  // = map<long unsigned int, cv::Point2f>(N,
-                            // static_cast<cv::Point2f>(NULL));
+                            // static_cast<cv::Point2f>(nullptr));
   mmMatchedInImage.clear();
 
   mvbOutlier = vector<bool>(N, false);
@@ -1120,7 +1120,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
              const std::shared_ptr<GeometricCamera> &pCamera,
              const std::shared_ptr<GeometricCamera> &pCamera2,
              Sophus::SE3f &Tlr, Frame *pPrevF, const IMU::Calib &ImuCalib)
-    : mpcpi(NULL),
+    : mpcpi(nullptr),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractorLeft),
       mpORBextractorRight(extractorRight),
@@ -1131,10 +1131,10 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mbf(bf),
       mThDepth(thDepth),
       mImuCalib(ImuCalib),
-      mpImuPreintegrated(NULL),
+      mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
-      mpImuPreintegratedFrame(NULL),
-      mpReferenceKF(static_cast<KeyFrame *>(NULL)),
+      mpImuPreintegratedFrame(nullptr),
+      mpReferenceKF(static_cast<KeyFrame *>(nullptr)),
       mbImuPreintegrated(false),
       mpCamera(pCamera),
       mpCamera2(pCamera2),
diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc
index 6a23490813a..fd853207556 100644
--- a/src/G2oTypes.cc
+++ b/src/G2oTypes.cc
@@ -21,6 +21,7 @@
 
 #include "G2oTypes.h"
 
+#include <memory>
 #include <vector>
 
 #include "Converter.h"
@@ -471,7 +472,7 @@ VertexAccBias::VertexAccBias(Frame* pF) {
   setEstimate(ba);
 }
 
-EdgeInertial::EdgeInertial(IMU::Preintegrated* pInt)
+EdgeInertial::EdgeInertial(const std::shared_ptr<IMU::Preintegrated>& pInt)
     : JRg(pInt->JRg.cast<double>()),
       JVg(pInt->JVg.cast<double>()),
       JPg(pInt->JPg.cast<double>()),
@@ -586,7 +587,7 @@ void EdgeInertial::linearizeOplus() {
   _jacobianOplus[5].block<3, 3>(3, 0) = Rbw1;  // OK
 }
 
-EdgeInertialGS::EdgeInertialGS(IMU::Preintegrated* pInt)
+EdgeInertialGS::EdgeInertialGS(const std::shared_ptr<IMU::Preintegrated>& pInt)
     : JRg(pInt->JRg.cast<double>()),
       JVg(pInt->JVg.cast<double>()),
       JPg(pInt->JPg.cast<double>()),
diff --git a/src/ImuTypes.cc b/src/ImuTypes.cc
index b57b197c965..2df10e9d249 100644
--- a/src/ImuTypes.cc
+++ b/src/ImuTypes.cc
@@ -22,6 +22,7 @@
 #include "ImuTypes.h"
 
 #include <iostream>
+#include <memory>
 #include <vector>
 
 #include "Converter.h"
@@ -111,7 +112,7 @@ Preintegrated::Preintegrated(const Bias &b_, const Calib &calib) {
 }
 
 // Copy constructor
-Preintegrated::Preintegrated(Preintegrated *pImuPre)
+Preintegrated::Preintegrated(const std::shared_ptr<Preintegrated> &pImuPre)
     : dT(pImuPre->dT),
       C(pImuPre->C),
       Info(pImuPre->Info),
@@ -132,7 +133,7 @@ Preintegrated::Preintegrated(Preintegrated *pImuPre)
       db(pImuPre->db),
       mvMeasurements(pImuPre->mvMeasurements) {}
 
-void Preintegrated::CopyFrom(Preintegrated *pImuPre) {
+void Preintegrated::CopyFrom(const std::shared_ptr<Preintegrated> &pImuPre) {
   dT = pImuPre->dT;
   C = pImuPre->C;
   Info = pImuPre->Info;
@@ -246,8 +247,8 @@ void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration,
   dT += dt;
 }
 
-void Preintegrated::MergePrevious(Preintegrated *pPrev) {
-  if (pPrev == this) return;
+void Preintegrated::MergePrevious(const std::shared_ptr<Preintegrated> &pPrev) {
+  if (pPrev.get() == this) return;
 
   std::unique_lock<std::mutex> lock1(mMutex);
   std::unique_lock<std::mutex> lock2(pPrev->mMutex);
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index 857c42fd46f..4e264f7925d 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -88,6 +88,7 @@ KeyFrame::KeyFrame()
       mPrevKF(nullptr),
       mNextKF(nullptr),
       mbFirstConnection(true),
+      mpBackupImuPreintegrated(std::make_shared<IMU::Preintegrated>()),
       mpParent(NULL),
       mbNotErase(false),
       mbToBeErased(false),
@@ -153,6 +154,7 @@ KeyFrame::KeyFrame(Frame &F, const std::shared_ptr<Map> &pMap,
       mPrevKF(NULL),
       mNextKF(NULL),
       mpImuPreintegrated(F.mpImuPreintegrated),
+      mpBackupImuPreintegrated(std::make_shared<IMU::Preintegrated>()),
       mImuCalib(F.mImuCalib),
       mvpMapPoints(F.mvpMapPoints),
       mpKeyFrameDB(pKFDB),
@@ -939,7 +941,8 @@ void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP,
   if (mNextKF && spKF.find(mNextKF) != spKF.end())
     mBackupNextKFId = mNextKF->mnId;
 
-  if (mpImuPreintegrated) mBackupImuPreintegrated.CopyFrom(mpImuPreintegrated);
+  if (mpImuPreintegrated)
+    mpBackupImuPreintegrated->CopyFrom(mpImuPreintegrated);
 }
 
 void KeyFrame::PostLoad(
@@ -1021,7 +1024,12 @@ void KeyFrame::PostLoad(
   if (mBackupNextKFId != -1) {
     mNextKF = mpKFid[mBackupNextKFId];
   }
-  mpImuPreintegrated = &mBackupImuPreintegrated;
+
+  // \todo{} WAS:
+  // mpImuPreintegrated = &mBackupImuPreintegrated;
+  //
+  // Which doesn't work with shared ptrs.  Switched to:
+  mpImuPreintegrated = mpBackupImuPreintegrated;
 
   // Remove all backup container
   mvBackupMapPointsId.clear();
diff --git a/src/SettingsLoader.cc b/src/SettingsLoader.cc
index 4baf6108172..7b4fa9057c3 100644
--- a/src/SettingsLoader.cc
+++ b/src/SettingsLoader.cc
@@ -19,6 +19,7 @@
  * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <Eigen/Eigen>
 #include <filesystem>  // NOLINT {build/c++17}
 #include <iostream>
 #include <memory>
diff --git a/src/System.cc b/src/System.cc
index 0c474941c9c..eb684a696f8 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -61,7 +61,7 @@ SystemFactory::Expected SystemFactory::create(const std::string &configFile,
                                               const SensorType sensor) {
   auto settings = SettingsLoader::load(configFile, sensor);
 
-  if (settings.has_value()) {
+  if (settings) {
     return std::make_shared<System>(settings.value());
   }
 
@@ -130,17 +130,17 @@ void System::printBanner() {
        << "under certain conditions. See LICENSE.txt." << endl
        << endl;
 
-  cout << "Input sensor was set to: " << sensorType().toString();
+  spdlog::info("Input sensor was set to: {}", sensorType().toString());
 }
 
 void System::initialize(bool initFr, const string &strSequence) {
-  mStrLoadAtlasFromFile = settings_->atlasLoadFile();
-  mStrSaveAtlasToFile = settings_->atlasSaveFile();
+  const string mStrLoadAtlasFromFile = settings_->atlasLoadFile();
+  const string mStrSaveAtlasToFile = settings_->atlasSaveFile();
 
   cout << (*settings_) << endl;
 
   const bool activeLC = settings_->loopClosing_;
-  mStrVocabularyFilePath = settings_->strVocFile_;
+  const string vocabularyFilePath = settings_->strVocFile_;
 
   bool loadedAtlas = false;
 
@@ -149,10 +149,10 @@ void System::initialize(bool initFr, const string &strSequence) {
     spdlog::info("Loading ORB Vocabulary. This could take a while...");
 
     mpVocabulary = std::make_shared<ORBVocabulary>();
-    bool bVocLoad = mpVocabulary->loadFromTextFile(mStrVocabularyFilePath);
+    bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyFilePath);
     if (!bVocLoad) {
       cerr << "Wrong path to vocabulary. " << endl;
-      cerr << "Falied to open at: " << mStrVocabularyFilePath << endl;
+      cerr << "Falied to open at: " << vocabularyFilePath << endl;
       exit(-1);
     }
     spdlog::info("Vocabulary loaded!");
@@ -168,10 +168,10 @@ void System::initialize(bool initFr, const string &strSequence) {
     spdlog::info("Loading ORB Vocabulary. This could take a while...");
 
     mpVocabulary = std::make_shared<ORBVocabulary>();
-    bool bVocLoad = mpVocabulary->loadFromTextFile(mStrVocabularyFilePath);
+    bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyFilePath);
     if (!bVocLoad) {
       cerr << "Wrong path to vocabulary. " << endl;
-      cerr << "Falied to open at: " << mStrVocabularyFilePath << endl;
+      cerr << "Falied to open at: " << vocabularyFilePath << endl;
       exit(-1);
     }
     spdlog::info("Vocabulary loaded!");
@@ -228,7 +228,8 @@ void System::initialize(bool initFr, const string &strSequence) {
   mpLocalMapper =
       std::make_shared<LocalMapping>(this, mpAtlas, sensorType().isMonocular(),
                                      sensorType().isImu(), strSequence);
-  mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run, mpLocalMapper);
+  mptLocalMapping =
+      std::make_unique<thread>(&ORB_SLAM3::LocalMapping::Run, mpLocalMapper);
   mpLocalMapper->mInitFr = initFr;
   mpLocalMapper->mThFarPoints = settings_->thFarPoints();
 
@@ -249,7 +250,8 @@ void System::initialize(bool initFr, const string &strSequence) {
       std::make_shared<LoopClosing>(mpAtlas, mpKeyFrameDatabase, mpVocabulary,
                                     sensorType() != SensorType::MONOCULAR,
                                     activeLC);  // sensorType()!=MONOCULAR);
-  mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
+  mptLoopClosing =
+      std::make_unique<thread>(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
 
   // Set pointers between threads
   mpTracker->SetLocalMapper(mpLocalMapper);
@@ -267,7 +269,7 @@ void System::initialize(bool initFr, const string &strSequence) {
   if (settings_->useViewer_) {
     mpViewer = std::make_shared<Viewer>(this, mpFrameDrawer, mpMapDrawer,
                                         mpTracker, settings_);
-    mptViewer = new thread(&Viewer::Run, mpViewer);
+    mptViewer = std::make_unique<thread>(&Viewer::Run, mpViewer);
     mpTracker->SetViewer(mpViewer);
     mpLoopCloser->mpViewer = mpViewer;
     mpViewer->both = mpFrameDrawer->both;
@@ -565,6 +567,7 @@ void System::Shutdown() {
   /*usleep(5000);
 }*/
 
+  const string mStrSaveAtlasToFile = settings_->atlasSaveFile();
   if (!mStrSaveAtlasToFile.empty()) {
     Verbose::PrintMess("Atlas saving to file " + mStrSaveAtlasToFile,
                        Verbose::VERBOSITY_NORMAL);
@@ -1431,6 +1434,8 @@ void System::InsertTrackTime(double &time) {
 #endif
 
 void System::SaveAtlas(int type) {
+  const string mStrSaveAtlasToFile = settings_->atlasSaveFile();
+
   if (!mStrSaveAtlasToFile.empty()) {
     // clock_t start = clock();
 
@@ -1441,10 +1446,12 @@ void System::SaveAtlas(int type) {
     pathSaveFileName = pathSaveFileName.append(mStrSaveAtlasToFile);
     pathSaveFileName = pathSaveFileName.append(".osa");
 
+    const string vocabularyFilePath = settings_->strVocFile_;
+
     string strVocabularyChecksum =
-        CalculateCheckSum(mStrVocabularyFilePath, TEXT_FILE);
-    std::size_t found = mStrVocabularyFilePath.find_last_of("/\\");
-    string strVocabularyName = mStrVocabularyFilePath.substr(found + 1);
+        CalculateCheckSum(vocabularyFilePath, TEXT_FILE);
+    std::size_t found = vocabularyFilePath.find_last_of("/\\");
+    string strVocabularyName = vocabularyFilePath.substr(found + 1);
 
     if (type == TEXT_FILE) {
       // File text
@@ -1475,6 +1482,9 @@ void System::SaveAtlas(int type) {
 
 bool System::LoadAtlas(int type) {
   string strFileVoc, strVocChecksum;
+
+  const string mStrLoadAtlasFromFile = settings_->atlasLoadFile();
+  const string vocabularyFilePath = settings_->strVocFile_;
   bool isRead = false;
 
   string pathLoadFileName = "./";
@@ -1514,7 +1524,7 @@ bool System::LoadAtlas(int type) {
   if (isRead) {
     // Check if the vocabulary is the same
     string strInputVocabularyChecksum =
-        CalculateCheckSum(mStrVocabularyFilePath, TEXT_FILE);
+        CalculateCheckSum(vocabularyFilePath, TEXT_FILE);
 
     if (strInputVocabularyChecksum.compare(strVocChecksum) != 0) {
       cout << "The vocabulary load isn't the same which the load session was "
diff --git a/src/Tracking.cc b/src/Tracking.cc
index b7ea43a5d2d..099e9206f64 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -613,7 +613,7 @@ void Tracking::newParameterLoader(const std::shared_ptr<Settings>& settings) {
   mpImuCalib = new IMU::Calib(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf);
 
   mpImuPreintegratedFromLastKF =
-      new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
+      std::make_shared<IMU::Preintegrated>(IMU::Bias(), *mpImuCalib);
 }
 
 Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
@@ -848,8 +848,9 @@ void Tracking::PreintegrateIMU() {
     return;
   }
 
-  IMU::Preintegrated* pImuPreintegratedFromLastFrame =
-      new IMU::Preintegrated(mLastFrame.mImuBias, mCurrentFrame.mImuCalib);
+  std::shared_ptr<IMU::Preintegrated> pImuPreintegratedFromLastFrame =
+      std::make_shared<IMU::Preintegrated>(mLastFrame.mImuBias,
+                                           mCurrentFrame.mImuCalib);
 
   for (int i = 0; i < n; i++) {
     float tstep;
@@ -891,7 +892,7 @@ void Tracking::PreintegrateIMU() {
     }
 
     if (!mpImuPreintegratedFromLastKF)
-      cout << "mpImuPreintegratedFromLastKF does not exist" << endl;
+      spdlog::warn("mpImuPreintegratedFromLastKF does not exist");
     mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc, angVel, tstep);
     pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc, angVel, tstep);
   }
@@ -1356,8 +1357,12 @@ void Tracking::Track() {
       pF->mpPrevFrame = new Frame(mLastFrame);
 
       // Load preintegration
-      pF->mpImuPreintegratedFrame =
-          new IMU::Preintegrated(mCurrentFrame.mpImuPreintegratedFrame);
+      //
+      // \todo{AMM}.  I believe this was correct, want to make a deep copy of
+      // the Preintegration?
+      pF->mpImuPreintegratedFrame = std::make_shared<IMU::Preintegrated>();
+      pF->mpImuPreintegratedFrame->CopyFrom(
+          mCurrentFrame.mpImuPreintegratedFrame);
     }
 
     if (pCurrentMap->isImuInitialized()) {
@@ -1528,10 +1533,8 @@ void Tracking::StereoInitialization() {
         return;
       }
 
-      if (mpImuPreintegratedFromLastKF) delete mpImuPreintegratedFromLastKF;
-
       mpImuPreintegratedFromLastKF =
-          new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
+          std::make_shared<IMU::Preintegrated>(IMU::Bias(), *mpImuCalib);
       mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
     }
 
@@ -1637,11 +1640,8 @@ void Tracking::MonocularInitialization() {
       fill(mvIniMatches.begin(), mvIniMatches.end(), -1);
 
       if (mSensor == SensorType::IMU_MONOCULAR) {
-        if (mpImuPreintegratedFromLastKF) {
-          delete mpImuPreintegratedFromLastKF;
-        }
         mpImuPreintegratedFromLastKF =
-            new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
+            std::make_shared<IMU::Preintegrated>(IMU::Bias(), *mpImuCalib);
         mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
       }
 
@@ -1698,8 +1698,7 @@ void Tracking::CreateInitialMapMonocular() {
   KeyFrame* pKFcur =
       new KeyFrame(mCurrentFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
 
-  if (mSensor == SensorType::IMU_MONOCULAR)
-    pKFini->mpImuPreintegrated = (IMU::Preintegrated*)(NULL);
+  if (mSensor == SensorType::IMU_MONOCULAR) pKFini->mpImuPreintegrated.reset();
 
   pKFini->ComputeBoW();
   pKFcur->ComputeBoW();
@@ -1781,7 +1780,7 @@ void Tracking::CreateInitialMapMonocular() {
     pKFini->mNextKF = pKFcur;
     pKFcur->mpImuPreintegrated = mpImuPreintegratedFromLastKF;
 
-    mpImuPreintegratedFromLastKF = new IMU::Preintegrated(
+    mpImuPreintegratedFromLastKF = std::make_shared<IMU::Preintegrated>(
         pKFcur->mpImuPreintegrated->GetUpdatedBias(), pKFcur->mImuCalib);
   }
 
@@ -1849,12 +1848,9 @@ void Tracking::CreateMapInAtlas() {
     mbReadyToInitializate = false;
   }
 
-  if ((mSensor == SensorType::IMU_MONOCULAR ||
-       mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) &&
-      mpImuPreintegratedFromLastKF) {
-    delete mpImuPreintegratedFromLastKF;
+  if (mSensor.isImu() && mpImuPreintegratedFromLastKF) {
     mpImuPreintegratedFromLastKF =
-        new IMU::Preintegrated(IMU::Bias(), *mpImuCalib);
+        std::make_shared<IMU::Preintegrated>(IMU::Bias(), *mpImuCalib);
   }
 
   if (mpLastKeyFrame) mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
@@ -2449,7 +2445,7 @@ void Tracking::CreateNewKeyFrame() {
   if (mSensor == SensorType::IMU_MONOCULAR ||
       mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) {
     mpImuPreintegratedFromLastKF =
-        new IMU::Preintegrated(pKF->GetImuBias(), pKF->mImuCalib);
+        std::make_shared<IMU::Preintegrated>(pKF->GetImuBias(), pKF->mImuCalib);
   }
 
   // TODO check if incluide imu_stereo

From c4c625ad16dc0f308d837d9800f895a24fc100ca Mon Sep 17 00:00:00 2001
From: Aaron Marburg <amarburg@uw.edu>
Date: Fri, 2 Jan 2026 21:43:39 +0000
Subject: [PATCH 4/9] Transition KeyFrame and Frame to shared_ptrs

---
 include/Atlas.h            |  18 +-
 include/Frame.h            |  16 +-
 include/FrameDrawer.h      |   4 +-
 include/G2oTypes.h         |  43 +-
 include/KeyFrame.h         |  75 +--
 include/KeyFrameDatabase.h |  34 +-
 include/LocalMapping.h     |  16 +-
 include/LoopClosing.h      |  72 +--
 include/MLPnPsolver.h      |   3 +-
 include/Map.h              |  27 +-
 include/MapPoint.h         |  37 +-
 include/ORBmatcher.h       |  51 +-
 include/Optimizer.h        |  65 ++-
 include/Sim3Solver.h       |  14 +-
 include/System.h           |  34 +-
 include/Tracking.h         |  27 +-
 src/Atlas.cc               |  29 +-
 src/Frame.cc               |  28 +-
 src/FrameDrawer.cc         |  21 +-
 src/G2oTypes.cc            |  20 +-
 src/KeyFrame.cc            | 399 ++++++++--------
 src/KeyFrameDatabase.cc    | 382 +++++++--------
 src/LocalMapping.cc        | 179 ++++---
 src/LoopClosing.cc         | 245 +++++-----
 src/MLPnPsolver.cpp        |  23 +-
 src/Map.cc                 |  42 +-
 src/MapDrawer.cc           |  44 +-
 src/MapPoint.cc            |  95 ++--
 src/ORBmatcher.cc          | 318 ++++++------
 src/Optimizer.cc           | 556 +++++++++------------
 src/Sim3Solver.cc          |  10 +-
 src/System.cc              | 337 +++++--------
 src/Tracking.cc            | 955 ++++++++++++++++++-------------------
 33 files changed, 2045 insertions(+), 2174 deletions(-)

diff --git a/include/Atlas.h b/include/Atlas.h
index 4de0428b456..7cdbdfa2b13 100644
--- a/include/Atlas.h
+++ b/include/Atlas.h
@@ -86,14 +86,14 @@ class Atlas {
   void SetViewer(const std::shared_ptr<Viewer> &pViewer);
 
   // Method for change components in the current map
-  void AddKeyFrame(KeyFrame *pKF);
+  void AddKeyFrame(const std::shared_ptr<KeyFrame> &pKF);
   void AddMapPoint(MapPoint *pMP);
   // void EraseMapPoint(MapPoint* pMP);
   // void EraseKeyFrame(KeyFrame* pKF);
 
   std::shared_ptr<GeometricCamera> AddCamera(
       const std::shared_ptr<GeometricCamera> &pCam);
-  std::vector<std::shared_ptr<GeometricCamera> > GetAllCameras();
+  std::vector<std::shared_ptr<GeometricCamera>> GetAllCameras();
 
   /* All methods without Map pointer work on current map */
   void SetReferenceMapPoints(const std::vector<MapPoint *> &vpMPs);
@@ -104,11 +104,11 @@ class Atlas {
   long unsigned KeyFramesInMap();
 
   // Method for get data in current map
-  std::vector<KeyFrame *> GetAllKeyFrames();
+  std::vector<std::shared_ptr<KeyFrame>> GetAllKeyFrames();
   std::vector<MapPoint *> GetAllMapPoints();
   std::vector<MapPoint *> GetReferenceMapPoints();
 
-  vector<std::shared_ptr<Map> > GetAllMaps();
+  vector<std::shared_ptr<Map>> GetAllMaps();
 
   int CountMaps();
 
@@ -130,7 +130,7 @@ class Atlas {
   void PreSave();
   void PostLoad();
 
-  map<long unsigned int, KeyFrame *> GetAtlasKeyframes();
+  map<long unsigned int, std::shared_ptr<KeyFrame>> GetAtlasKeyframes();
 
   void SetKeyFrameDababase(const std::shared_ptr<KeyFrameDatabase> &pKFDB);
   std::shared_ptr<KeyFrameDatabase> GetKeyFrameDatabase();
@@ -143,15 +143,15 @@ class Atlas {
   long unsigned int GetNumLivedMP();
 
  protected:
-  std::set<std::shared_ptr<Map> > mspMaps;
-  std::set<std::shared_ptr<Map> > mspBadMaps;
+  std::set<std::shared_ptr<Map>> mspMaps;
+  std::set<std::shared_ptr<Map>> mspBadMaps;
   // Its necessary change the container from set to vector because libboost 1.58
   // and Ubuntu 16.04 have an error with this cointainer
-  std::vector<std::shared_ptr<Map> > mvpBackupMaps;
+  std::vector<std::shared_ptr<Map>> mvpBackupMaps;
 
   std::shared_ptr<Map> mpCurrentMap;
 
-  std::vector<std::shared_ptr<GeometricCamera> > mvpCameras;
+  std::vector<std::shared_ptr<GeometricCamera>> mvpCameras;
 
   unsigned long int mnLastInitKFidMap;
 
diff --git a/include/Frame.h b/include/Frame.h
index 522f752103c..0ce8fde0a7c 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -48,7 +48,7 @@ class ConstraintPoseImu;
 class GeometricCamera;
 class ORBextractor;
 
-class Frame {
+class Frame : public std::enable_shared_from_this<Frame> {
  public:
   Frame();
 
@@ -62,7 +62,7 @@ class Frame {
         const std::shared_ptr<ORBVocabulary> &voc, cv::Mat &K,
         cv::Mat &distCoef, const float &bf, const float &thDepth,
         const std::shared_ptr<GeometricCamera> &pCamera,
-        Frame *pPrevF = static_cast<Frame *>(NULL),
+        const std::shared_ptr<Frame> &pPrevF = nullptr,
         const IMU::Calib &ImuCalib = IMU::Calib());
 
   // Constructor for RGB-D cameras.
@@ -71,7 +71,7 @@ class Frame {
         const std::shared_ptr<ORBVocabulary> &voc, cv::Mat &K,
         cv::Mat &distCoef, const float &bf, const float &thDepth,
         const std::shared_ptr<GeometricCamera> &pCamera,
-        Frame *pPrevF = static_cast<Frame *>(NULL),
+        const std::shared_ptr<Frame> &pPrevF = nullptr,
         const IMU::Calib &ImuCalib = IMU::Calib());
 
   // Constructor for Monocular cameras.
@@ -80,7 +80,7 @@ class Frame {
         const std::shared_ptr<ORBVocabulary> &voc,
         const std::shared_ptr<GeometricCamera> &pCamera, cv::Mat &distCoef,
         const float &bf, const float &thDepth,
-        Frame *pPrevF = static_cast<Frame *>(NULL),
+        const std::shared_ptr<Frame> &pPrevF = nullptr,
         const IMU::Calib &ImuCalib = IMU::Calib());
 
   // Destructor
@@ -274,10 +274,10 @@ class Frame {
 
   // Imu preintegration from last keyframe
   std::shared_ptr<IMU::Preintegrated> mpImuPreintegrated;
-  KeyFrame *mpLastKeyFrame;
+  std::shared_ptr<KeyFrame> mpLastKeyFrame;
 
   // Pointer to previous frame
-  Frame *mpPrevFrame;
+  std::shared_ptr<Frame> mpPrevFrame;
   std::shared_ptr<IMU::Preintegrated> mpImuPreintegratedFrame;
 
   // Current and Next Frame id.
@@ -285,7 +285,7 @@ class Frame {
   long unsigned int mnId;
 
   // Reference Keyframe.
-  KeyFrame *mpReferenceKF;
+  std::shared_ptr<KeyFrame> mpReferenceKF;
 
   // Scale pyramid info.
   int mnScaleLevels;
@@ -364,7 +364,7 @@ class Frame {
         cv::Mat &distCoef, const float &bf, const float &thDepth,
         const std::shared_ptr<GeometricCamera> &pCamera,
         const std::shared_ptr<GeometricCamera> &pCamera2, Sophus::SE3f &Tlr,
-        Frame *pPrevF = static_cast<Frame *>(NULL),
+        const std::shared_ptr<Frame> &pPrevF = nullptr,
         const IMU::Calib &ImuCalib = IMU::Calib());
 
   // Stereo fisheye
diff --git a/include/FrameDrawer.h b/include/FrameDrawer.h
index bf21ca6f336..7a894d85e87 100644
--- a/include/FrameDrawer.h
+++ b/include/FrameDrawer.h
@@ -46,7 +46,7 @@ class FrameDrawer {
                        bool draw_both = false);
 
   // Update info from the last processed frame.
-  void Update(Tracking *pTracker);
+  void Update(const std::shared_ptr<Tracking> &pTracker);
 
   // Draw last processed frame.
   cv::Mat DrawFrame(float imageScale = 1.f);
@@ -75,7 +75,7 @@ class FrameDrawer {
   std::mutex mMutex;
   vector<pair<cv::Point2f, cv::Point2f> > mvTracks;
 
-  Frame mCurrentFrame;
+  std::shared_ptr<Frame> mCurrentFrame;
   vector<MapPoint *> mvpLocalMap;
   vector<cv::KeyPoint> mvMatchedKeys;
   vector<MapPoint *> mvpMatchedMPs;
diff --git a/include/G2oTypes.h b/include/G2oTypes.h
index 5bfd4a1d430..b72deafd352 100644
--- a/include/G2oTypes.h
+++ b/include/G2oTypes.h
@@ -78,9 +78,10 @@ class ImuCamPose {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   ImuCamPose() {}
-  explicit ImuCamPose(KeyFrame* pKF);
-  explicit ImuCamPose(Frame* pF);
-  ImuCamPose(Eigen::Matrix3d& _Rwc, Eigen::Vector3d& _twc, KeyFrame* pKF);
+  explicit ImuCamPose(const std::shared_ptr<KeyFrame>& pKF);
+  explicit ImuCamPose(const std::shared_ptr<Frame>& pF);
+  ImuCamPose(Eigen::Matrix3d& _Rwc, Eigen::Vector3d& _twc,
+             const std::shared_ptr<KeyFrame>& pKF);
 
   void SetParam(const std::vector<Eigen::Matrix3d>& _Rcw,
                 const std::vector<Eigen::Vector3d>& _tcw,
@@ -119,7 +120,8 @@ class InvDepthPoint {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   InvDepthPoint() {}
-  InvDepthPoint(double _rho, double _u, double _v, KeyFrame* pHostKF);
+  InvDepthPoint(double _rho, double _u, double _v,
+                const std::shared_ptr<KeyFrame>& pHostKF);
 
   void Update(const double* pu);
 
@@ -136,8 +138,12 @@ class VertexPose : public g2o::BaseVertex<6, ImuCamPose> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   VertexPose() {}
-  explicit VertexPose(KeyFrame* pKF) { setEstimate(ImuCamPose(pKF)); }
-  explicit VertexPose(Frame* pF) { setEstimate(ImuCamPose(pF)); }
+  explicit VertexPose(const std::shared_ptr<KeyFrame>& pKF) {
+    setEstimate(ImuCamPose(pKF));
+  }
+  explicit VertexPose(const std::shared_ptr<Frame>& pF) {
+    setEstimate(ImuCamPose(pF));
+  }
 
   virtual bool read(std::istream& is);
   virtual bool write(std::ostream& os) const;
@@ -155,9 +161,14 @@ class VertexPose4DoF : public g2o::BaseVertex<4, ImuCamPose> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   VertexPose4DoF() {}
-  explicit VertexPose4DoF(KeyFrame* pKF) { setEstimate(ImuCamPose(pKF)); }
-  explicit VertexPose4DoF(Frame* pF) { setEstimate(ImuCamPose(pF)); }
-  VertexPose4DoF(Eigen::Matrix3d& _Rwc, Eigen::Vector3d& _twc, KeyFrame* pKF) {
+  explicit VertexPose4DoF(const std::shared_ptr<KeyFrame>& pKF) {
+    setEstimate(ImuCamPose(pKF));
+  }
+  explicit VertexPose4DoF(const std::shared_ptr<Frame>& pF) {
+    setEstimate(ImuCamPose(pF));
+  }
+  VertexPose4DoF(Eigen::Matrix3d& _Rwc, Eigen::Vector3d& _twc,
+                 const std::shared_ptr<KeyFrame>& pKF) {
     setEstimate(ImuCamPose(_Rwc, _twc, pKF));
   }
 
@@ -183,8 +194,8 @@ class VertexVelocity : public g2o::BaseVertex<3, Eigen::Vector3d> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   VertexVelocity() {}
-  explicit VertexVelocity(KeyFrame* pKF);
-  explicit VertexVelocity(Frame* pF);
+  explicit VertexVelocity(const std::shared_ptr<KeyFrame>& pKF);
+  explicit VertexVelocity(const std::shared_ptr<Frame>& pF);
 
   virtual bool read(std::istream& is) { return false; }
   virtual bool write(std::ostream& os) const { return false; }
@@ -202,8 +213,8 @@ class VertexGyroBias : public g2o::BaseVertex<3, Eigen::Vector3d> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   VertexGyroBias() {}
-  explicit VertexGyroBias(KeyFrame* pKF);
-  explicit VertexGyroBias(Frame* pF);
+  explicit VertexGyroBias(const std::shared_ptr<KeyFrame>& pKF);
+  explicit VertexGyroBias(const std::shared_ptr<Frame>& pF);
 
   virtual bool read(std::istream& is) { return false; }
   virtual bool write(std::ostream& os) const { return false; }
@@ -221,8 +232,8 @@ class VertexAccBias : public g2o::BaseVertex<3, Eigen::Vector3d> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   VertexAccBias() {}
-  explicit VertexAccBias(KeyFrame* pKF);
-  explicit VertexAccBias(Frame* pF);
+  explicit VertexAccBias(const std::shared_ptr<KeyFrame>& pKF);
+  explicit VertexAccBias(const std::shared_ptr<Frame>& pF);
 
   virtual bool read(std::istream& is) { return false; }
   virtual bool write(std::ostream& os) const { return false; }
@@ -290,7 +301,7 @@ class VertexInvDepth : public g2o::BaseVertex<1, InvDepthPoint> {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   VertexInvDepth() {}
   explicit VertexInvDepth(double invDepth, double u, double v,
-                          KeyFrame* pHostKF) {
+                          const std::shared_ptr<KeyFrame>& pHostKF) {
     setEstimate(InvDepthPoint(invDepth, u, v, pHostKF));
   }
 
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
index f0727ebf612..ea82e778f1a 100644
--- a/include/KeyFrame.h
+++ b/include/KeyFrame.h
@@ -53,7 +53,7 @@ class KeyFrameDatabase;
 
 class GeometricCamera;
 
-class KeyFrame {
+class KeyFrame : public std::enable_shared_from_this<KeyFrame> {
   friend class boost::serialization::access;
 
   template <class Archive>
@@ -196,8 +196,8 @@ class KeyFrame {
 
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  KeyFrame();
-  KeyFrame(Frame& F, const std::shared_ptr<Map>& pMap,
+  // KeyFrame();
+  KeyFrame(const std::shared_ptr<Frame>& F, const std::shared_ptr<Map>& pMap,
            const std::shared_ptr<KeyFrameDatabase>& pKFDB);
 
   // Pose functions
@@ -221,33 +221,33 @@ class KeyFrame {
   void ComputeBoW();
 
   // Covisibility graph functions
-  void AddConnection(KeyFrame* pKF, const int& weight);
-  void EraseConnection(KeyFrame* pKF);
+  void AddConnection(const std::shared_ptr<KeyFrame>& pKF, const int& weight);
+  void EraseConnection(const std::shared_ptr<KeyFrame>& pKF);
 
   void UpdateConnections(bool upParent = true);
   void UpdateBestCovisibles();
-  std::set<KeyFrame*> GetConnectedKeyFrames();
-  std::vector<KeyFrame*> GetVectorCovisibleKeyFrames();
-  std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int& N);
-  std::vector<KeyFrame*> GetCovisiblesByWeight(const int& w);
-  int GetWeight(KeyFrame* pKF);
+  std::set<std::shared_ptr<KeyFrame>> GetConnectedKeyFrames();
+  std::vector<std::shared_ptr<KeyFrame>> GetVectorCovisibleKeyFrames();
+  std::vector<std::shared_ptr<KeyFrame>> GetBestCovisibilityKeyFrames(int N);
+  std::vector<std::shared_ptr<KeyFrame>> GetCovisiblesByWeight(int w);
+  int GetWeight(const std::shared_ptr<KeyFrame>& pKF);
 
   // Spanning tree functions
-  void AddChild(KeyFrame* pKF);
-  void EraseChild(KeyFrame* pKF);
-  void ChangeParent(KeyFrame* pKF);
-  std::set<KeyFrame*> GetChilds();
-  KeyFrame* GetParent();
-  bool hasChild(KeyFrame* pKF);
+  void AddChild(const std::shared_ptr<KeyFrame>& pKF);
+  void EraseChild(const std::shared_ptr<KeyFrame>& pKF);
+  void ChangeParent(const std::shared_ptr<KeyFrame>& pKF);
+  std::set<std::shared_ptr<KeyFrame>> GetChilds();
+  std::shared_ptr<KeyFrame> GetParent();
+  bool hasChild(const std::shared_ptr<KeyFrame>& pKF);
   void SetFirstConnection(bool bFirst);
 
   // Loop Edges
-  void AddLoopEdge(KeyFrame* pKF);
-  std::set<KeyFrame*> GetLoopEdges();
+  void AddLoopEdge(const std::shared_ptr<KeyFrame>& pKF);
+  std::set<std::shared_ptr<KeyFrame>> GetLoopEdges();
 
   // Merge Edges
-  void AddMergeEdge(KeyFrame* pKF);
-  set<KeyFrame*> GetMergeEdges();
+  void AddMergeEdge(const std::shared_ptr<KeyFrame>& pKF);
+  set<std::shared_ptr<KeyFrame>> GetMergeEdges();
 
   // MapPoint observation functions
   int GetNumberMPs();
@@ -282,7 +282,8 @@ class KeyFrame {
 
   static bool weightComp(int a, int b) { return a > b; }
 
-  static bool lId(KeyFrame* pKF1, KeyFrame* pKF2) {
+  static bool lId(const std::shared_ptr<KeyFrame>& pKF1,
+                  const std::shared_ptr<KeyFrame>& pKF2) {
     return pKF1->mnId < pKF2->mnId;
   }
 
@@ -300,11 +301,11 @@ class KeyFrame {
   bool ProjectPointUnDistort(MapPoint* pMP, cv::Point2f& kp, float& u,
                              float& v);
 
-  void PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP,
-               set<std::shared_ptr<GeometricCamera> >& spCam);
-  void PostLoad(map<unsigned long int, KeyFrame*>& mpKFid,
+  void PreSave(set<std::shared_ptr<KeyFrame>>& spKF, set<MapPoint*>& spMP,
+               set<std::shared_ptr<GeometricCamera>>& spCam);
+  void PostLoad(map<unsigned long int, std::shared_ptr<KeyFrame>>& mpKFid,
                 map<unsigned long int, MapPoint*>& mpMPid,
-                map<unsigned int, std::shared_ptr<GeometricCamera> >& mpCamId);
+                map<unsigned int, std::shared_ptr<GeometricCamera>>& mpCamId);
 
   void SetORBVocabulary(const std::shared_ptr<ORBVocabulary>& pORBVoc);
   void SetKeyFrameDatabase(const std::shared_ptr<KeyFrameDatabase>& pKFDB);
@@ -411,8 +412,8 @@ class KeyFrame {
   const int mnMaxY;
 
   // Preintegrated IMU measurements from previous keyframe
-  KeyFrame* mPrevKF;
-  KeyFrame* mNextKF;
+  std::shared_ptr<KeyFrame> mPrevKF;
+  std::shared_ptr<KeyFrame> mNextKF;
 
   std::shared_ptr<IMU::Preintegrated> mpImuPreintegrated;
   IMU::Calib mImuCalib;
@@ -423,8 +424,8 @@ class KeyFrame {
 
   int mnDataset;
 
-  std::vector<KeyFrame*> mvpLoopCandKFs;
-  std::vector<KeyFrame*> mvpMergeCandKFs;
+  std::vector<std::shared_ptr<KeyFrame>> mvpLoopCandKFs;
+  std::vector<std::shared_ptr<KeyFrame>> mvpMergeCandKFs;
 
   // bool mbHasHessian;
   // cv::Mat mHessianPose;
@@ -461,20 +462,20 @@ class KeyFrame {
   std::shared_ptr<ORBVocabulary> mpORBvocabulary;
 
   // Grid over the image to speed up feature matching
-  std::vector<std::vector<std::vector<size_t> > > mGrid;
+  std::vector<std::vector<std::vector<size_t>>> mGrid;
 
-  std::map<KeyFrame*, int> mConnectedKeyFrameWeights;
-  std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;
+  std::map<std::shared_ptr<KeyFrame>, int> mConnectedKeyFrameWeights;
+  std::vector<std::shared_ptr<KeyFrame>> mvpOrderedConnectedKeyFrames;
   std::vector<int> mvOrderedWeights;
   // For save relation without pointer, this is necessary for save/load function
   std::map<unsigned long int, int> mBackupConnectedKeyFrameIdWeights;
 
   // Spanning Tree and Loop Edges
   bool mbFirstConnection;
-  KeyFrame* mpParent;
-  std::set<KeyFrame*> mspChildrens;
-  std::set<KeyFrame*> mspLoopEdges;
-  std::set<KeyFrame*> mspMergeEdges;
+  std::shared_ptr<KeyFrame> mpParent;
+  std::set<std::shared_ptr<KeyFrame>> mspChildrens;
+  std::set<std::shared_ptr<KeyFrame>> mspLoopEdges;
+  std::set<std::shared_ptr<KeyFrame>> mspMergeEdges;
   // For save relation without pointer, this is necessary for save/load function
   long long int mBackupParentId;
   std::vector<unsigned long int> mvBackupChildrensId;
@@ -521,7 +522,7 @@ class KeyFrame {
 
   const int NLeft, NRight;
 
-  std::vector<std::vector<std::vector<size_t> > > mGridRight;
+  std::vector<std::vector<std::vector<size_t>>> mGridRight;
 
   Sophus::SE3<float> GetRightPose();
   Sophus::SE3<float> GetRightPoseInverse();
diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h
index 7ce2770cf7d..239f44e9425 100644
--- a/include/KeyFrameDatabase.h
+++ b/include/KeyFrameDatabase.h
@@ -55,32 +55,36 @@ class KeyFrameDatabase {
   KeyFrameDatabase() = default;
   explicit KeyFrameDatabase(const std::shared_ptr<ORBVocabulary> &voc);
 
-  void add(KeyFrame *pKF);
+  void add(const std::shared_ptr<KeyFrame> &pKF);
 
-  void erase(KeyFrame *pKF);
+  void erase(const std::shared_ptr<KeyFrame> &pKF);
 
   void clear();
   void clearMap(const std::shared_ptr<Map> &pMap);
 
   // Loop Detection(DEPRECATED)
-  std::vector<KeyFrame *> DetectLoopCandidates(KeyFrame *pKF, float minScore);
+  std::vector<std::shared_ptr<KeyFrame>> DetectLoopCandidates(
+      const std::shared_ptr<KeyFrame> &pKF, float minScore);
 
   // Loop and Merge Detection
-  void DetectCandidates(KeyFrame *pKF, float minScore,
-                        vector<KeyFrame *> &vpLoopCand,
-                        vector<KeyFrame *> &vpMergeCand);
-  void DetectBestCandidates(KeyFrame *pKF, vector<KeyFrame *> &vpLoopCand,
-                            vector<KeyFrame *> &vpMergeCand, int nMinWords);
-  void DetectNBestCandidates(KeyFrame *pKF, vector<KeyFrame *> &vpLoopCand,
-                             vector<KeyFrame *> &vpMergeCand,
+  void DetectCandidates(const std::shared_ptr<KeyFrame> &pKF, float minScore,
+                        vector<std::shared_ptr<KeyFrame>> &vpLoopCand,
+                        vector<std::shared_ptr<KeyFrame>> &vpMergeCand);
+  void DetectBestCandidates(const std::shared_ptr<KeyFrame> &pKF,
+                            vector<std::shared_ptr<KeyFrame>> &vpLoopCand,
+                            vector<std::shared_ptr<KeyFrame>> &vpMergeCand,
+                            int nMinWords);
+  void DetectNBestCandidates(const std::shared_ptr<KeyFrame> &pKF,
+                             vector<std::shared_ptr<KeyFrame>> &vpLoopCand,
+                             vector<std::shared_ptr<KeyFrame>> &vpMergeCand,
                              int nNumCandidates);
 
   // Relocalization
-  std::vector<KeyFrame *> DetectRelocalizationCandidates(
-      Frame *F, const std::shared_ptr<Map> &pMap);
+  std::vector<std::shared_ptr<KeyFrame>> DetectRelocalizationCandidates(
+      const std::shared_ptr<Frame> &F, const std::shared_ptr<Map> &pMap);
 
   void PreSave();
-  void PostLoad(map<long unsigned int, KeyFrame *> mpKFid);
+  void PostLoad(map<long unsigned int, std::shared_ptr<KeyFrame>> mpKFid);
   void SetORBVocabulary(const std::shared_ptr<ORBVocabulary> &pORBVoc);
 
  protected:
@@ -88,10 +92,10 @@ class KeyFrameDatabase {
   std::shared_ptr<ORBVocabulary> mpVoc;
 
   // Inverted file
-  std::vector<list<KeyFrame *> > mvInvertedFile;
+  std::vector<list<std::shared_ptr<KeyFrame>>> mvInvertedFile;
 
   // For save relation without pointer, this is necessary for save/load function
-  std::vector<list<long unsigned int> > mvBackupInvertedFileId;
+  std::vector<list<long unsigned int>> mvBackupInvertedFileId;
 
   // Mutex
   std::mutex mMutex;
diff --git a/include/LocalMapping.h b/include/LocalMapping.h
index fef73e4a4f6..ae2e6c8b2bd 100644
--- a/include/LocalMapping.h
+++ b/include/LocalMapping.h
@@ -44,9 +44,9 @@ class Atlas;
 class LocalMapping {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  LocalMapping(System *pSys, const std::shared_ptr<Atlas> &pAtlas,
-               const float bMonocular, bool bInertial,
-               const string &_strSeqName = std::string());
+  LocalMapping(const std::shared_ptr<System> &pSys,
+               const std::shared_ptr<Atlas> &pAtlas, const float bMonocular,
+               bool bInertial, const string &_strSeqName = std::string());
 
   void SetLoopCloser(const std::shared_ptr<LoopClosing> &pLoopCloser);
 
@@ -55,7 +55,7 @@ class LocalMapping {
   // Main function
   void Run();
 
-  void InsertKeyFrame(KeyFrame *pKF);
+  void InsertKeyFrame(const std::shared_ptr<KeyFrame> &pKF);
   void EmptyQueue();
 
   // Thread Synch
@@ -82,7 +82,7 @@ class LocalMapping {
 
   bool IsInitializing();
   double GetCurrKFTime();
-  KeyFrame *GetCurrKF();
+  std::shared_ptr<KeyFrame> GetCurrKF();
 
   std::mutex mMutexImuInit;
 
@@ -142,7 +142,7 @@ class LocalMapping {
   void SearchInNeighbors();
   void KeyFrameCulling();
 
-  System *mpSystem;
+  std::shared_ptr<System> mpSystem;
 
   bool mbMonocular;
   bool mbInertial;
@@ -164,9 +164,9 @@ class LocalMapping {
   std::shared_ptr<LoopClosing> mpLoopCloser;
   std::shared_ptr<Tracking> mpTracker;
 
-  std::list<KeyFrame *> mlNewKeyFrames;
+  std::list<std::shared_ptr<KeyFrame>> mlNewKeyFrames;
 
-  KeyFrame *mpCurrentKeyFrame;
+  std::shared_ptr<KeyFrame> mpCurrentKeyFrame;
 
   std::list<MapPoint *> mlpRecentAddedMapPoints;
 
diff --git a/include/LoopClosing.h b/include/LoopClosing.h
index 9f1421da23f..2f3b5d22e9b 100644
--- a/include/LoopClosing.h
+++ b/include/LoopClosing.h
@@ -50,9 +50,11 @@ class Map;
 
 class LoopClosing : public enable_shared_from_this<LoopClosing> {
  public:
-  typedef pair<set<KeyFrame *>, int> ConsistentGroup;
-  typedef map<KeyFrame *, g2o::Sim3, std::less<KeyFrame *>,
-              Eigen::aligned_allocator<std::pair<KeyFrame *const, g2o::Sim3> > >
+  typedef pair<set<std::shared_ptr<KeyFrame> >, int> ConsistentGroup;
+  typedef map<std::shared_ptr<KeyFrame>, g2o::Sim3,
+              std::less<std::shared_ptr<KeyFrame> >,
+              Eigen::aligned_allocator<
+                  std::pair<const std::shared_ptr<KeyFrame>, g2o::Sim3> > >
       KeyFrameAndPose;
 
  public:
@@ -71,7 +73,7 @@ class LoopClosing : public enable_shared_from_this<LoopClosing> {
   // Main function
   void Run();
 
-  void InsertKeyFrame(KeyFrame *pKF);
+  void InsertKeyFrame(const std::shared_ptr<KeyFrame> &pKF);
 
   void RequestReset();
   void RequestResetActiveMap(const std::shared_ptr<Map> &pMap);
@@ -133,22 +135,24 @@ class LoopClosing : public enable_shared_from_this<LoopClosing> {
 
   // Methods to implement the new place recognition algorithm
   bool NewDetectCommonRegions();
-  bool DetectAndReffineSim3FromLastKF(KeyFrame *pCurrentKF,
-                                      KeyFrame *pMatchedKF, g2o::Sim3 &gScw,
-                                      int &nNumProjMatches,
-                                      std::vector<MapPoint *> &vpMPs,
-                                      std::vector<MapPoint *> &vpMatchedMPs);
-  bool DetectCommonRegionsFromBoW(std::vector<KeyFrame *> &vpBowCand,
-                                  KeyFrame *&pMatchedKF,
-                                  KeyFrame *&pLastCurrentKF, g2o::Sim3 &g2oScw,
-                                  int &nNumCoincidences,
-                                  std::vector<MapPoint *> &vpMPs,
-                                  std::vector<MapPoint *> &vpMatchedMPs);
-  bool DetectCommonRegionsFromLastKF(KeyFrame *pCurrentKF, KeyFrame *pMatchedKF,
-                                     g2o::Sim3 &gScw, int &nNumProjMatches,
-                                     std::vector<MapPoint *> &vpMPs,
-                                     std::vector<MapPoint *> &vpMatchedMPs);
-  int FindMatchesByProjection(KeyFrame *pCurrentKF, KeyFrame *pMatchedKFw,
+  bool DetectAndReffineSim3FromLastKF(
+      const std::shared_ptr<KeyFrame> &pCurrentKF,
+      std::shared_ptr<KeyFrame> &pMatchedKF, g2o::Sim3 &gScw,
+      int &nNumProjMatches, std::vector<MapPoint *> &vpMPs,
+      std::vector<MapPoint *> &vpMatchedMPs);
+  bool DetectCommonRegionsFromBoW(
+      std::vector<std::shared_ptr<KeyFrame> > &vpBowCand,
+      std::shared_ptr<KeyFrame> &pMatchedKF,
+      std::shared_ptr<KeyFrame> &pLastCurrentKF, g2o::Sim3 &g2oScw,
+      int &nNumCoincidences, std::vector<MapPoint *> &vpMPs,
+      std::vector<MapPoint *> &vpMatchedMPs);
+  bool DetectCommonRegionsFromLastKF(
+      const std::shared_ptr<KeyFrame> &pCurrentKF,
+      const std::shared_ptr<KeyFrame> &pMatchedKF, g2o::Sim3 &gScw,
+      int &nNumProjMatches, std::vector<MapPoint *> &vpMPs,
+      std::vector<MapPoint *> &vpMatchedMPs);
+  int FindMatchesByProjection(const std::shared_ptr<KeyFrame> &pCurrentKF,
+                              const std::shared_ptr<KeyFrame> &pMatchedKFw,
                               g2o::Sim3 &g2oScw,
                               set<MapPoint *> &spMatchedMPinOrigin,
                               vector<MapPoint *> &vpMapPoints,
@@ -156,7 +160,7 @@ class LoopClosing : public enable_shared_from_this<LoopClosing> {
 
   void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap,
                      vector<MapPoint *> &vpMapPoints);
-  void SearchAndFuse(const vector<KeyFrame *> &vConectedKFs,
+  void SearchAndFuse(const vector<std::shared_ptr<KeyFrame> > &vConectedKFs,
                      vector<MapPoint *> &vpMapPoints);
 
   void CorrectLoop();
@@ -164,8 +168,8 @@ class LoopClosing : public enable_shared_from_this<LoopClosing> {
   void MergeLocal();
   void MergeLocal2();
 
-  void CheckObservations(set<KeyFrame *> &spKFsMap1,
-                         set<KeyFrame *> &spKFsMap2);
+  void CheckObservations(set<std::shared_ptr<KeyFrame> > &spKFsMap1,
+                         set<std::shared_ptr<KeyFrame> > &spKFsMap2);
 
   void ResetIfRequested();
   bool mbResetRequested;
@@ -187,7 +191,7 @@ class LoopClosing : public enable_shared_from_this<LoopClosing> {
 
   std::shared_ptr<LocalMapping> mpLocalMapper;
 
-  std::list<KeyFrame *> mlpLoopKeyFrameQueue;
+  std::list<std::shared_ptr<KeyFrame> > mlpLoopKeyFrameQueue;
 
   std::mutex mMutexLoopQueue;
 
@@ -195,12 +199,12 @@ class LoopClosing : public enable_shared_from_this<LoopClosing> {
   float mnCovisibilityConsistencyTh;
 
   // Loop detector variables
-  KeyFrame *mpCurrentKF;
-  KeyFrame *mpLastCurrentKF;
-  KeyFrame *mpMatchedKF;
+  std::shared_ptr<KeyFrame> mpCurrentKF;
+  std::shared_ptr<KeyFrame> mpLastCurrentKF;
+  std::shared_ptr<KeyFrame> mpMatchedKF;
   std::vector<ConsistentGroup> mvConsistentGroups;
-  std::vector<KeyFrame *> mvpEnoughConsistentCandidates;
-  std::vector<KeyFrame *> mvpCurrentConnectedKFs;
+  std::vector<std::shared_ptr<KeyFrame> > mvpEnoughConsistentCandidates;
+  std::vector<std::shared_ptr<KeyFrame> > mvpCurrentConnectedKFs;
   std::vector<MapPoint *> mvpCurrentMatchedPoints;
   std::vector<MapPoint *> mvpLoopMapPoints;
   cv::Mat mScw;
@@ -212,23 +216,23 @@ class LoopClosing : public enable_shared_from_this<LoopClosing> {
   bool mbLoopDetected;
   int mnLoopNumCoincidences;
   int mnLoopNumNotFound;
-  KeyFrame *mpLoopLastCurrentKF;
+  std::shared_ptr<KeyFrame> mpLoopLastCurrentKF;
   g2o::Sim3 mg2oLoopSlw;
   g2o::Sim3 mg2oLoopScw;
-  KeyFrame *mpLoopMatchedKF;
+  std::shared_ptr<KeyFrame> mpLoopMatchedKF;
   std::vector<MapPoint *> mvpLoopMPs;
   std::vector<MapPoint *> mvpLoopMatchedMPs;
   bool mbMergeDetected;
   int mnMergeNumCoincidences;
   int mnMergeNumNotFound;
-  KeyFrame *mpMergeLastCurrentKF;
+  std::shared_ptr<KeyFrame> mpMergeLastCurrentKF;
   g2o::Sim3 mg2oMergeSlw;
   g2o::Sim3 mg2oMergeSmw;
   g2o::Sim3 mg2oMergeScw;
-  KeyFrame *mpMergeMatchedKF;
+  std::shared_ptr<KeyFrame> mpMergeMatchedKF;
   std::vector<MapPoint *> mvpMergeMPs;
   std::vector<MapPoint *> mvpMergeMatchedMPs;
-  std::vector<KeyFrame *> mvpMergeConnectedKFs;
+  std::vector<std::shared_ptr<KeyFrame> > mvpMergeConnectedKFs;
 
   g2o::Sim3 mSold_new;
   //-------
diff --git a/include/MLPnPsolver.h b/include/MLPnPsolver.h
index e9f685cc3fa..a91b3e02687 100644
--- a/include/MLPnPsolver.h
+++ b/include/MLPnPsolver.h
@@ -64,7 +64,8 @@ class MLPnPsolver {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  MLPnPsolver(const Frame& F, const vector<MapPoint*>& vpMapPointMatches);
+  MLPnPsolver(const std::shared_ptr<Frame>& F,
+              const vector<MapPoint*>& vpMapPointMatches);
 
   ~MLPnPsolver();
 
diff --git a/include/Map.h b/include/Map.h
index 452d5fd5caa..39fb8c6ea98 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -77,15 +77,15 @@ class Map : public std::enable_shared_from_this<Map> {
 
   ~Map();
 
-  void AddKeyFrame(KeyFrame* pKF);
+  void AddKeyFrame(const std::shared_ptr<KeyFrame>& pKF);
   void AddMapPoint(MapPoint* pMP);
   void EraseMapPoint(MapPoint* pMP);
-  void EraseKeyFrame(KeyFrame* pKF);
+  void EraseKeyFrame(const std::shared_ptr<KeyFrame>& pKF);
   void SetReferenceMapPoints(const std::vector<MapPoint*>& vpMPs);
   void InformNewBigChange();
   int GetLastBigChangeIdx();
 
-  std::vector<KeyFrame*> GetAllKeyFrames();
+  std::vector<std::shared_ptr<KeyFrame> > GetAllKeyFrames();
   std::vector<MapPoint*> GetAllMapPoints();
   std::vector<MapPoint*> GetReferenceMapPoints();
 
@@ -98,7 +98,7 @@ class Map : public std::enable_shared_from_this<Map> {
   void SetInitKFid(unsigned long int initKFif);
   unsigned long int GetMaxKFid();
 
-  KeyFrame* GetOriginKF();
+  std::shared_ptr<KeyFrame> GetOriginKF();
 
   void SetCurrentMap();
   void SetStoredMap();
@@ -142,13 +142,14 @@ class Map : public std::enable_shared_from_this<Map> {
           pORBVoc /*, map<unsigned long int, KeyFrame*>& mpKeyFrameId*/,
       map<unsigned int, std::shared_ptr<GeometricCamera> >& mpCams);
 
-  void printReprojectionError(list<KeyFrame*>& lpLocalWindowKFs,
-                              KeyFrame* mpCurrentKF, string& name,
-                              string& name_folder);
+  void printReprojectionError(
+      list<std::shared_ptr<KeyFrame> >& lpLocalWindowKFs,
+      const std::shared_ptr<KeyFrame>& mpCurrentKF, string& name,
+      string& name_folder);
 
-  vector<KeyFrame*> mvpKeyFrameOrigins;
+  vector<std::shared_ptr<KeyFrame> > mvpKeyFrameOrigins;
   vector<unsigned long int> mvBackupKeyFrameOriginsId;
-  KeyFrame* mpFirstRegionKF;
+  std::shared_ptr<KeyFrame> mpFirstRegionKF;
   std::mutex mMutexMapUpdate;
 
   // This avoid that two points are created simultaneously in separate threads
@@ -171,15 +172,15 @@ class Map : public std::enable_shared_from_this<Map> {
   unsigned long int mnId;
 
   std::set<MapPoint*> mspMapPoints;
-  std::set<KeyFrame*> mspKeyFrames;
+  std::set<std::shared_ptr<KeyFrame> > mspKeyFrames;
 
   // Save/load, the set structure is broken in libboost 1.58 for ubuntu 16.04, a
   // vector is serializated
   std::vector<MapPoint*> mvpBackupMapPoints;
-  std::vector<KeyFrame*> mvpBackupKeyFrames;
+  std::vector<std::shared_ptr<KeyFrame> > mvpBackupKeyFrames;
 
-  KeyFrame* mpKFinitial;
-  KeyFrame* mpKFlowerID;
+  std::shared_ptr<KeyFrame> mpKFinitial;
+  std::shared_ptr<KeyFrame> mpKFlowerID;
 
   unsigned long int mnBackupKFinitialID;
   unsigned long int mnBackupKFlowerID;
diff --git a/include/MapPoint.h b/include/MapPoint.h
index 7b4108c9679..34e033a51b9 100644
--- a/include/MapPoint.h
+++ b/include/MapPoint.h
@@ -105,12 +105,14 @@ class MapPoint {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   MapPoint();
 
-  MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF,
+  MapPoint(const Eigen::Vector3f& Pos, const std::shared_ptr<KeyFrame>& pRefKF,
+           const std::shared_ptr<Map>& pMap);
+  MapPoint(const double invDepth, cv::Point2f uv_init,
+           const std::shared_ptr<KeyFrame>& pRefKF,
+           const std::shared_ptr<KeyFrame>& pHostKF,
            const std::shared_ptr<Map>& pMap);
-  MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF,
-           KeyFrame* pHostKF, const std::shared_ptr<Map>& pMap);
   MapPoint(const Eigen::Vector3f& Pos, const std::shared_ptr<Map>& pMap,
-           Frame* pFrame, const int& idxF);
+           const std::shared_ptr<Frame>& pFrame, const int& idxF);
 
   void SetWorldPos(const Eigen::Vector3f& Pos);
   Eigen::Vector3f GetWorldPos();
@@ -118,16 +120,16 @@ class MapPoint {
   Eigen::Vector3f GetNormal();
   void SetNormalVector(const Eigen::Vector3f& normal);
 
-  KeyFrame* GetReferenceKeyFrame();
+  std::shared_ptr<KeyFrame> GetReferenceKeyFrame();
 
-  std::map<KeyFrame*, std::tuple<int, int>> GetObservations();
+  std::map<std::shared_ptr<KeyFrame>, std::tuple<int, int>> GetObservations();
   int Observations();
 
-  void AddObservation(KeyFrame* pKF, int idx);
-  void EraseObservation(KeyFrame* pKF);
+  void AddObservation(const std::shared_ptr<KeyFrame>& pKF, int idx);
+  void EraseObservation(const std::shared_ptr<KeyFrame>& pKF);
 
-  std::tuple<int, int> GetIndexInKeyFrame(KeyFrame* pKF);
-  bool IsInKeyFrame(KeyFrame* pKF);
+  std::tuple<int, int> GetIndexInKeyFrame(const std::shared_ptr<KeyFrame>& pKF);
+  bool IsInKeyFrame(const std::shared_ptr<KeyFrame>& pKF);
 
   void SetBadFlag();
   bool isBad();
@@ -148,16 +150,17 @@ class MapPoint {
 
   float GetMinDistanceInvariance();
   float GetMaxDistanceInvariance();
-  int PredictScale(const float& currentDist, KeyFrame* pKF);
-  int PredictScale(const float& currentDist, Frame* pF);
+  int PredictScale(const float& currentDist,
+                   const std::shared_ptr<KeyFrame>& pKF);
+  int PredictScale(const float& currentDist, const std::shared_ptr<Frame>& pF);
 
   std::shared_ptr<Map> GetMap();
   void UpdateMap(const std::shared_ptr<Map>& pMap);
 
   void PrintObservations();
 
-  void PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP);
-  void PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
+  void PreSave(set<std::shared_ptr<KeyFrame>>& spKF, set<MapPoint*>& spMP);
+  void PostLoad(map<long unsigned int, std::shared_ptr<KeyFrame>>& mpKFid,
                 map<long unsigned int, MapPoint*>& mpMPid);
 
  public:
@@ -200,7 +203,7 @@ class MapPoint {
   double mInvDepth;
   double mInitU;
   double mInitV;
-  KeyFrame* mpHostKF;
+  std::shared_ptr<KeyFrame> mpHostKF;
 
   static std::mutex mGlobalMutex;
 
@@ -211,7 +214,7 @@ class MapPoint {
   Eigen::Vector3f mWorldPos;
 
   // Keyframes observing the point and associated index in keyframe
-  std::map<KeyFrame*, std::tuple<int, int>> mObservations;
+  std::map<std::shared_ptr<KeyFrame>, std::tuple<int, int>> mObservations;
   // For save relation without pointer, this is necessary for save/load function
   std::map<long unsigned int, int> mBackupObservationsId1;
   std::map<long unsigned int, int> mBackupObservationsId2;
@@ -223,7 +226,7 @@ class MapPoint {
   cv::Mat mDescriptor;
 
   // Reference KeyFrame
-  KeyFrame* mpRefKF;
+  std::shared_ptr<KeyFrame> mpRefKF;
   long unsigned int mBackupRefKFId;
 
   // Tracking counters
diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h
index e0f4b908eaa..2ac3c42a706 100644
--- a/include/ORBmatcher.h
+++ b/include/ORBmatcher.h
@@ -21,6 +21,7 @@
 
 #pragma once
 
+#include <memory>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <set>
@@ -43,54 +44,64 @@ class ORBmatcher {
 
   // Search matches between Frame keypoints and projected MapPoints. Returns
   // number of matches Used to track the local map (Tracking)
-  int SearchByProjection(Frame &F, const std::vector<MapPoint *> &vpMapPoints,
+  int SearchByProjection(const std::shared_ptr<Frame> &F,
+                         const std::vector<MapPoint *> &vpMapPoints,
                          const float th = 3, const bool bFarPoints = false,
                          const float thFarPoints = 50.0f);
 
   // Project MapPoints tracked in last frame into the current frame and search
   // matches. Used to track from previous frame (Tracking)
-  int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
+  int SearchByProjection(const std::shared_ptr<Frame> &CurrentFrame,
+                         const std::shared_ptr<Frame> &LastFrame,
                          const float th, const bool bMono);
 
   // Project MapPoints seen in KeyFrame into the Frame and search matches.
   // Used in relocalisation (Tracking)
-  int SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
+  int SearchByProjection(const std::shared_ptr<Frame> &CurrentFrame,
+                         const std::shared_ptr<KeyFrame> &pKF,
                          const std::set<MapPoint *> &sAlreadyFound,
                          const float th, const int ORBdist);
 
   // Project MapPoints using a Similarity Transformation and search matches.
   // Used in loop detection (Loop Closing)
-  int SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
+  int SearchByProjection(const std::shared_ptr<KeyFrame> &pKF,
+                         Sophus::Sim3<float> &Scw,
                          const std::vector<MapPoint *> &vpPoints,
                          std::vector<MapPoint *> &vpMatched, int th,
                          float ratioHamming = 1.0);
 
   // Project MapPoints using a Similarity Transformation and search matches.
   // Used in Place Recognition (Loop Closing and Merging)
-  int SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
-                         const std::vector<MapPoint *> &vpPoints,
-                         const std::vector<KeyFrame *> &vpPointsKFs,
-                         std::vector<MapPoint *> &vpMatched,
-                         std::vector<KeyFrame *> &vpMatchedKF, int th,
-                         float ratioHamming = 1.0);
+  int SearchByProjection(
+      const std::shared_ptr<KeyFrame> &pKF, Sophus::Sim3<float> &Scw,
+      const std::vector<MapPoint *> &vpPoints,
+      const std::vector<std::shared_ptr<KeyFrame>> &vpPointsKFs,
+      std::vector<MapPoint *> &vpMatched,
+      std::vector<std::shared_ptr<KeyFrame>> &vpMatchedKF, int th,
+      float ratioHamming = 1.0);
 
   // Search matches between MapPoints in a KeyFrame and ORB in a Frame.
   // 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,
+  int SearchByBoW(const std::shared_ptr<KeyFrame> &pKF,
+                  const std::shared_ptr<Frame> &F,
                   std::vector<MapPoint *> &vpMapPointMatches);
-  int SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2,
+
+  int SearchByBoW(const std::shared_ptr<KeyFrame> &pKF1,
+                  const std::shared_ptr<KeyFrame> &pKF2,
                   std::vector<MapPoint *> &vpMatches12);
 
   // Matching for the Map Initialization (only used in the monocular case)
-  int SearchForInitialization(Frame &F1, Frame &F2,
+  int SearchForInitialization(const std::shared_ptr<Frame> &F1,
+                              const std::shared_ptr<Frame> &F2,
                               std::vector<cv::Point2f> &vbPrevMatched,
                               std::vector<int> &vnMatches12,
                               int windowSize = 10);
 
   // Matching to triangulate new MapPoints. Check Epipolar Constraint.
-  int SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2,
-                             std::vector<pair<size_t, size_t> > &vMatchedPairs,
+  int SearchForTriangulation(const std::shared_ptr<KeyFrame> &pKF1,
+                             const std::shared_ptr<KeyFrame> &pKF2,
+                             std::vector<pair<size_t, size_t>> &vMatchedPairs,
                              const bool bOnlyStereo,
                              const bool bCoarse = false);
 
@@ -99,17 +110,19 @@ class ORBmatcher {
   // SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *>
   // &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12,
   // const float th);
-  int SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2,
+  int SearchBySim3(const std::shared_ptr<KeyFrame> &pKF1,
+                   const std::shared_ptr<KeyFrame> &pKF2,
                    std::vector<MapPoint *> &vpMatches12,
                    const Sophus::Sim3f &S12, const float th);
 
   // Project MapPoints into KeyFrame and search for duplicated MapPoints.
-  int Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints,
-           const float th = 3.0, const bool bRight = false);
+  int Fuse(const std::shared_ptr<KeyFrame> &pKF,
+           const vector<MapPoint *> &vpMapPoints, const float th = 3.0,
+           const bool bRight = false);
 
   // Project MapPoints into KeyFrame using a given Sim3 and search for
   // duplicated MapPoints.
-  int Fuse(KeyFrame *pKF, Sophus::Sim3f &Scw,
+  int Fuse(const std::shared_ptr<KeyFrame> &pKF, Sophus::Sim3f &Scw,
            const std::vector<MapPoint *> &vpPoints, float th,
            vector<MapPoint *> &vpReplacePoint);
 
diff --git a/include/Optimizer.h b/include/Optimizer.h
index dcb6ecdb23f..c20f7642972 100644
--- a/include/Optimizer.h
+++ b/include/Optimizer.h
@@ -47,11 +47,11 @@ class LoopClosing;
 
 class Optimizer {
  public:
-  static void BundleAdjustment(const std::vector<KeyFrame *> &vpKF,
-                               const std::vector<MapPoint *> &vpMP,
-                               int nIterations = 5, bool *pbStopFlag = NULL,
-                               const unsigned long nLoopKF = 0,
-                               const bool bRobust = true);
+  static void BundleAdjustment(
+      const std::vector<std::shared_ptr<KeyFrame>> &vpKF,
+      const std::vector<MapPoint *> &vpMP, int nIterations = 5,
+      bool *pbStopFlag = NULL, const unsigned long nLoopKF = 0,
+      const bool bRobust = true);
   static void GlobalBundleAdjustemnt(const std::shared_ptr<Map> &pMap,
                                      int nIterations = 5,
                                      bool *pbStopFlag = NULL,
@@ -65,41 +65,51 @@ class Optimizer {
                              Eigen::VectorXd *vSingVal = NULL,
                              bool *bHess = NULL);
 
-  static void LocalBundleAdjustment(KeyFrame *pKF, bool *pbStopFlag,
+  static void LocalBundleAdjustment(const std::shared_ptr<KeyFrame> &pKF,
+                                    bool *pbStopFlag,
                                     const std::shared_ptr<Map> &pMap,
                                     int &num_fixedKF, int &num_OptKF,
                                     int &num_MPs, int &num_edges);
 
-  static int PoseOptimization(Frame *pFrame);
-  static int PoseInertialOptimizationLastKeyFrame(Frame *pFrame,
-                                                  bool bRecInit = false);
-  static int PoseInertialOptimizationLastFrame(Frame *pFrame,
-                                               bool bRecInit = false);
+  static int PoseOptimization(const std::shared_ptr<Frame> &pFrame);
+  static int PoseInertialOptimizationLastKeyFrame(
+      const std::shared_ptr<Frame> &pFrame, bool bRecInit = false);
+  static int PoseInertialOptimizationLastFrame(
+      const std::shared_ptr<Frame> &pFrame, bool bRecInit = false);
 
   // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise
   // (mono)
   static void OptimizeEssentialGraph(
-      const std::shared_ptr<Map> &pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
+      const std::shared_ptr<Map> &pMap,
+      const std::shared_ptr<KeyFrame> &pLoopKF,
+      const std::shared_ptr<KeyFrame> &pCurKF,
       const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
       const LoopClosing::KeyFrameAndPose &CorrectedSim3,
-      const map<KeyFrame *, set<KeyFrame *> > &LoopConnections,
+      const map<std::shared_ptr<KeyFrame>, set<std::shared_ptr<KeyFrame>>>
+          &LoopConnections,
       const bool &bFixScale);
-  static void OptimizeEssentialGraph(KeyFrame *pCurKF,
-                                     vector<KeyFrame *> &vpFixedKFs,
-                                     vector<KeyFrame *> &vpFixedCorrectedKFs,
-                                     vector<KeyFrame *> &vpNonFixedKFs,
-                                     vector<MapPoint *> &vpNonCorrectedMPs);
+
+  static void OptimizeEssentialGraph(
+      const std::shared_ptr<KeyFrame> &pCurKF,
+      vector<std::shared_ptr<KeyFrame>> &vpFixedKFs,
+      vector<std::shared_ptr<KeyFrame>> &vpFixedCorrectedKFs,
+      vector<std::shared_ptr<KeyFrame>> &vpNonFixedKFs,
+      vector<MapPoint *> &vpNonCorrectedMPs);
 
   // For inertial loopclosing
   static void OptimizeEssentialGraph4DoF(
-      const std::shared_ptr<Map> &pMap, KeyFrame *pLoopKF, KeyFrame *pCurKF,
+      const std::shared_ptr<Map> &pMap,
+      const std::shared_ptr<KeyFrame> &pLoopKF,
+      const std::shared_ptr<KeyFrame> &pCurKF,
       const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
       const LoopClosing::KeyFrameAndPose &CorrectedSim3,
-      const map<KeyFrame *, set<KeyFrame *> > &LoopConnections);
+      const map<std::shared_ptr<KeyFrame>, set<std::shared_ptr<KeyFrame>>>
+          &LoopConnections);
 
   // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono)
   // (NEW)
-  static int OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2,
+  static int OptimizeSim3(const std::shared_ptr<KeyFrame> &pKF1,
+                          const std::shared_ptr<KeyFrame> &pKF2,
                           std::vector<MapPoint *> &vpMatches1,
                           g2o::Sim3 &g2oS12, const float th2,
                           const bool bFixScale,
@@ -108,20 +118,23 @@ class Optimizer {
 
   // For inertial systems
 
-  static void LocalInertialBA(KeyFrame *pKF, bool *pbStopFlag,
+  static void LocalInertialBA(const std::shared_ptr<KeyFrame> &pKF,
+                              bool *pbStopFlag,
                               const std::shared_ptr<Map> &pMap,
                               int &num_fixedKF, int &num_OptKF, int &num_MPs,
                               int &num_edges, bool bLarge = false,
                               bool bRecInit = false);
-  static void MergeInertialBA(KeyFrame *pCurrKF, KeyFrame *pMergeKF,
+
+  static void MergeInertialBA(const std::shared_ptr<KeyFrame> &pCurrKF,
+                              const std::shared_ptr<KeyFrame> &pMergeKF,
                               bool *pbStopFlag,
                               const std::shared_ptr<Map> &pMap,
                               LoopClosing::KeyFrameAndPose &corrPoses);
 
   // Local BA in welding area when two maps are merged
-  static void LocalBundleAdjustment(KeyFrame *pMainKF,
-                                    vector<KeyFrame *> vpAdjustKF,
-                                    vector<KeyFrame *> vpFixedKF,
+  static void LocalBundleAdjustment(const std::shared_ptr<KeyFrame> &pMainKF,
+                                    vector<shared_ptr<KeyFrame>> vpAdjustKF,
+                                    vector<shared_ptr<KeyFrame>> vpFixedKF,
                                     bool *pbStopFlag);
 
   // Marginalize block element (start:end,start:end). Perform Schur complement.
diff --git a/include/Sim3Solver.h b/include/Sim3Solver.h
index 6bb9aded649..7f5cfe42aa7 100644
--- a/include/Sim3Solver.h
+++ b/include/Sim3Solver.h
@@ -32,10 +32,12 @@ namespace ORB_SLAM3 {
 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(const std::shared_ptr<KeyFrame> &pKF1,
+             const std::shared_ptr<KeyFrame> &pKF2,
+             const std::vector<MapPoint *> &vpMatched12,
+             const bool bFixScale = true,
+             const vector<std::shared_ptr<KeyFrame>> vpKeyFrameMatchedMP =
+                 vector<std::shared_ptr<KeyFrame>>());
 
   void SetRansacParameters(double probability = 0.99, int minInliers = 6,
                            int maxIterations = 300);
@@ -70,8 +72,8 @@ class Sim3Solver {
 
  protected:
   // KeyFrames and matches
-  KeyFrame *mpKF1;
-  KeyFrame *mpKF2;
+  std::shared_ptr<KeyFrame> mpKF1;
+  std::shared_ptr<KeyFrame> mpKF2;
 
   std::vector<Eigen::Vector3f> mvX3Dc1;
   std::vector<Eigen::Vector3f> mvX3Dc2;
diff --git a/include/System.h b/include/System.h
index 14d8a733a94..251eacd9ce8 100644
--- a/include/System.h
+++ b/include/System.h
@@ -66,26 +66,18 @@ class SystemFactory {
                          const SensorType sensor);
 };
 
-class System {
+class System : public std::enable_shared_from_this<System> {
  public:
+  friend SystemFactory::Expected SystemFactory::create(
+      const std::shared_ptr<Settings> &settings);
+
   // File type
   enum FileType {
     TEXT_FILE = 0,
     BINARY_FILE = 1,
   };
 
- public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and
-  // Viewer threads.
-  // System(const string &strVocFile, const string &strSettingsFile,
-  //        const SensorType sensor, bool initFr = false,
-  //        const string &strSequence = std::string());
-
-  // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and
-  // Viewer threads.
-  System(const std::shared_ptr<Settings> &settings, bool initFr = false,
-         const string &strSequence = std::string());
 
   // Proccess the given stereo frame. Images must be synchronized and rectified.
   // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to
@@ -192,10 +184,24 @@ class System {
   void InsertTrackTime(double &time);
 #endif
 
- private:
+ protected:
+  // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and
+  // Viewer threads.
+  //
+  // All construction should go through the factory to ensure correct
+  // initialization
+  System(const std::shared_ptr<Settings> &settings, bool initFr = false,
+         const string &strSequence = std::string());
+
   void printBanner();
 
-  void initialize(bool initFr, const string &strSequence);
+  bool initialize(bool initFr = false,
+                  const string &strSequence = std::string());
+
+ private:
+  void processLocalizationModeChange(void);
+  void processReset(void);
+  void updateTrackingState();
 
   void SaveAtlas(int type);
   bool LoadAtlas(int type);
diff --git a/include/Tracking.h b/include/Tracking.h
index cc4b73bd31b..383574d0c02 100644
--- a/include/Tracking.h
+++ b/include/Tracking.h
@@ -55,10 +55,11 @@ class LoopClosing;
 class System;
 class Settings;
 
-class Tracking {
+class Tracking : public std::enable_shared_from_this<Tracking> {
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  Tracking(System *pSys, const std::shared_ptr<ORBVocabulary> &pVoc,
+  Tracking(const std::shared_ptr<System> &pSys,
+           const std::shared_ptr<ORBVocabulary> &pVoc,
            const std::shared_ptr<FrameDrawer> &pFrameDrawer,
            const std::shared_ptr<MapDrawer> &pMapDrawer,
            const std::shared_ptr<Atlas> &pAtlas,
@@ -103,8 +104,8 @@ class Tracking {
   void InformOnlyTracking(const bool &flag);
 
   void UpdateFrameIMU(const float s, const IMU::Bias &b,
-                      KeyFrame *pCurrentKeyFrame);
-  KeyFrame *GetLastKeyFrame() { return mpLastKeyFrame; }
+                      const std::shared_ptr<KeyFrame> &pCurrentKeyFrame);
+  std::shared_ptr<KeyFrame> GetLastKeyFrame() { return mpLastKeyFrame; }
 
   void CreateMapInAtlas();
   // std::mutex mMutexTracks;
@@ -148,8 +149,8 @@ class Tracking {
   SensorType mSensor;
 
   // Current Frame
-  Frame mCurrentFrame;
-  Frame mLastFrame;
+  std::shared_ptr<Frame> mCurrentFrame;
+  std::shared_ptr<Frame> mLastFrame;
 
   cv::Mat mImGray;
 
@@ -158,13 +159,13 @@ class Tracking {
   std::vector<int> mvIniMatches;
   std::vector<cv::Point2f> mvbPrevMatched;
   std::vector<cv::Point3f> mvIniP3D;
-  Frame mInitialFrame;
+  std::shared_ptr<Frame> mInitialFrame;
 
   // Lists used to recover the full camera trajectory at the end of the
   // execution. Basically we store the reference keyframe for each frame and its
   // relative transformation
   list<Sophus::SE3f> mlRelativeFramePoses;
-  list<KeyFrame *> mlpReferences;
+  list<std::shared_ptr<KeyFrame>> mlpReferences;
   list<double> mlFrameTimes;
   list<bool> mlbLost;
 
@@ -256,7 +257,7 @@ class Tracking {
   std::mutex mMutexImuQueue;
 
   // Imu calibration parameters
-  IMU::Calib *mpImuCalib;
+  IMU::Calib mImuCalib;
 
   // Last Bias Estimation (at keyframe creation)
   IMU::Bias mLastBias;
@@ -285,12 +286,12 @@ class Tracking {
   bool mbSetInit;
 
   // Local Map
-  KeyFrame *mpReferenceKF;
-  std::vector<KeyFrame *> mvpLocalKeyFrames;
+  std::shared_ptr<KeyFrame> mpReferenceKF;
+  std::vector<std::shared_ptr<KeyFrame>> mvpLocalKeyFrames;
   std::vector<MapPoint *> mvpLocalMapPoints;
 
   // System
-  System *mpSystem;
+  std::shared_ptr<System> mpSystem;
 
   // Drawers
   std::shared_ptr<Viewer> mpViewer;
@@ -333,7 +334,7 @@ class Tracking {
   int mnMatchesInliers;
 
   // Last Frame, KeyFrame and Relocalisation Info
-  KeyFrame *mpLastKeyFrame;
+  std::shared_ptr<KeyFrame> mpLastKeyFrame;
   unsigned int mnLastKeyFrameId;
   unsigned int mnLastRelocFrameId;
   double mTimeStampLost;
diff --git a/src/Atlas.cc b/src/Atlas.cc
index cefbde041d7..0dbd30dbdf4 100644
--- a/src/Atlas.cc
+++ b/src/Atlas.cc
@@ -62,19 +62,19 @@ Atlas::~Atlas() {
 
 void Atlas::CreateNewMap() {
   unique_lock<mutex> lock(mMutexAtlas);
-  cout << "Creation of new map with id: " << Map::nNextId << endl;
+  spdlog::info("Creation of new map with id: {}", Map::nNextId);
   if (mpCurrentMap) {
     if (!mspMaps.empty() && mnLastInitKFidMap < mpCurrentMap->GetMaxKFid())
       mnLastInitKFidMap = mpCurrentMap->GetMaxKFid() +
                           1;  // The init KF is the next of current maximum
 
     mpCurrentMap->SetStoredMap();
-    cout << "Stored map with ID: " << mpCurrentMap->GetId() << endl;
+    spdlog::info("Stored map with ID: {}", mpCurrentMap->GetId());
 
     // if(mpViewer)
     //     mpViewer->AddMapToCreateThumbnail(mpCurrentMap);
   }
-  cout << "Creation of new map with last KF id: " << mnLastInitKFidMap << endl;
+  spdlog::info("Creation of new map with last KF id: {}", mnLastInitKFidMap);
 
   mpCurrentMap = std::make_shared<Map>(mnLastInitKFidMap);
   mpCurrentMap->SetCurrentMap();
@@ -83,7 +83,7 @@ void Atlas::CreateNewMap() {
 
 void Atlas::ChangeMap(const std::shared_ptr<Map> &pMap) {
   unique_lock<mutex> lock(mMutexAtlas);
-  cout << "Change to map with id: " << pMap->GetId() << endl;
+  spdlog::info("Change to map with id: {}", pMap->GetId());
   if (mpCurrentMap) {
     mpCurrentMap->SetStoredMap();
   }
@@ -101,7 +101,7 @@ void Atlas::SetViewer(const std::shared_ptr<Viewer> &pViewer) {
   mpViewer = pViewer;
 }
 
-void Atlas::AddKeyFrame(KeyFrame *pKF) {
+void Atlas::AddKeyFrame(const std::shared_ptr<KeyFrame> &pKF) {
   std::shared_ptr<Map> pMapKF = pKF->GetMap();
   pMapKF->AddKeyFrame(pKF);
 }
@@ -121,6 +121,7 @@ std::shared_ptr<GeometricCamera> Atlas::AddCamera(
 
     if (!pCam) std::cout << "Not pCam" << std::endl;
     if (!pCam_i) std::cout << "Not pCam_i" << std::endl;
+
     if (pCam->GetType() != pCam_i->GetType()) continue;
 
     if (pCam->GetType() == GeometricCamera::CAM_PINHOLE) {
@@ -173,7 +174,7 @@ long unsigned Atlas::KeyFramesInMap() {
   return mpCurrentMap->KeyFramesInMap();
 }
 
-std::vector<KeyFrame *> Atlas::GetAllKeyFrames() {
+std::vector<std::shared_ptr<KeyFrame>> Atlas::GetAllKeyFrames() {
   unique_lock<mutex> lock(mMutexAtlas);
   return mpCurrentMap->GetAllKeyFrames();
 }
@@ -307,7 +308,7 @@ void Atlas::PostLoad() {
 
   mspMaps.clear();
   unsigned long int numKF = 0, numMP = 0;
-  for (std::shared_ptr<Map> pMi : mvpBackupMaps) {
+  for (auto pMi : mvpBackupMaps) {
     mspMaps.insert(pMi);
     pMi->PostLoad(mpKeyFrameDB, mpORBVocabulary, mpCams);
     numKF += pMi->GetAllKeyFrames().size();
@@ -336,7 +337,7 @@ std::shared_ptr<ORBVocabulary> Atlas::GetORBVocabulary() {
 long unsigned int Atlas::GetNumLivedKF() {
   unique_lock<mutex> lock(mMutexAtlas);
   long unsigned int num = 0;
-  for (auto pMap_i : mspMaps) {
+  for (auto const &pMap_i : mspMaps) {
     num += pMap_i->GetAllKeyFrames().size();
   }
 
@@ -346,19 +347,19 @@ long unsigned int Atlas::GetNumLivedKF() {
 long unsigned int Atlas::GetNumLivedMP() {
   unique_lock<mutex> lock(mMutexAtlas);
   long unsigned int num = 0;
-  for (auto pMap_i : mspMaps) {
+  for (auto const &pMap_i : mspMaps) {
     num += pMap_i->GetAllMapPoints().size();
   }
 
   return num;
 }
 
-map<long unsigned int, KeyFrame *> Atlas::GetAtlasKeyframes() {
-  map<long unsigned int, KeyFrame *> mpIdKFs;
-  for (auto pMap_i : mvpBackupMaps) {
-    vector<KeyFrame *> vpKFs_Mi = pMap_i->GetAllKeyFrames();
+map<long unsigned int, std::shared_ptr<KeyFrame>> Atlas::GetAtlasKeyframes() {
+  map<long unsigned int, std::shared_ptr<KeyFrame>> mpIdKFs;
+  for (auto const &pMap_i : mvpBackupMaps) {
+    vector<std::shared_ptr<KeyFrame>> vpKFs_Mi = pMap_i->GetAllKeyFrames();
 
-    for (KeyFrame *pKF_j_Mi : vpKFs_Mi) {
+    for (auto pKF_j_Mi : vpKFs_Mi) {
       mpIdKFs[pKF_j_Mi->mnId] = pKF_j_Mi;
     }
   }
diff --git a/src/Frame.cc b/src/Frame.cc
index 335f0de634d..44259678f39 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -68,7 +68,8 @@ Frame::Frame()
 
 // Copy Constructor
 Frame::Frame(const Frame &frame)
-    : mpcpi(frame.mpcpi),
+    : std::enable_shared_from_this<Frame>(),
+      mpcpi(frame.mpcpi),
       mpORBvocabulary(frame.mpORBvocabulary),
       mpORBextractorLeft(frame.mpORBextractorLeft),
       mpORBextractorRight(frame.mpORBextractorRight),
@@ -157,8 +158,8 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
              const std::shared_ptr<ORBextractor> &extractorRight,
              const std::shared_ptr<ORBVocabulary> &voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
-             const std::shared_ptr<GeometricCamera> &pCamera, Frame *pPrevF,
-             const IMU::Calib &ImuCalib)
+             const std::shared_ptr<GeometricCamera> &pCamera,
+             const std::shared_ptr<Frame> &pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractorLeft),
@@ -173,7 +174,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(nullptr),
-      mpReferenceKF(static_cast<KeyFrame *>(nullptr)),
+      mpReferenceKF(),
       mbIsSet(false),
       mbImuPreintegrated(false),
       mpCamera(pCamera),
@@ -283,8 +284,8 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
              const std::shared_ptr<ORBextractor> &extractor,
              const std::shared_ptr<ORBVocabulary> &voc, cv::Mat &K,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
-             const std::shared_ptr<GeometricCamera> &pCamera, Frame *pPrevF,
-             const IMU::Calib &ImuCalib)
+             const std::shared_ptr<GeometricCamera> &pCamera,
+             const std::shared_ptr<Frame> &pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractor),
@@ -299,7 +300,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
       mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(nullptr),
-      mpReferenceKF(static_cast<KeyFrame *>(nullptr)),
+      mpReferenceKF(),
       mbIsSet(false),
       mbImuPreintegrated(false),
       mpCamera(pCamera),
@@ -395,8 +396,8 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
              const std::shared_ptr<ORBextractor> &extractor,
              const std::shared_ptr<ORBVocabulary> &voc,
              const std::shared_ptr<GeometricCamera> &pCamera, cv::Mat &distCoef,
-             const float &bf, const float &thDepth, Frame *pPrevF,
-             const IMU::Calib &ImuCalib)
+             const float &bf, const float &thDepth,
+             const std::shared_ptr<Frame> &pPrevF, const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractor),
@@ -662,7 +663,7 @@ bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit) {
     if (viewCos < viewingCosLimit) return false;
 
     // Predict scale in the image
-    const int nPredictedLevel = pMP->PredictScale(dist, this);
+    const int nPredictedLevel = pMP->PredictScale(dist, shared_from_this());
 
     // Data used by the tracking
     pMP->mbTrackInView = true;
@@ -1119,7 +1120,8 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
              cv::Mat &distCoef, const float &bf, const float &thDepth,
              const std::shared_ptr<GeometricCamera> &pCamera,
              const std::shared_ptr<GeometricCamera> &pCamera2,
-             Sophus::SE3f &Tlr, Frame *pPrevF, const IMU::Calib &ImuCalib)
+             Sophus::SE3f &Tlr, const std::shared_ptr<Frame> &pPrevF,
+             const IMU::Calib &ImuCalib)
     : mpcpi(nullptr),
       mpORBvocabulary(voc),
       mpORBextractorLeft(extractorLeft),
@@ -1134,7 +1136,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
       mpImuPreintegrated(nullptr),
       mpPrevFrame(pPrevF),
       mpImuPreintegratedFrame(nullptr),
-      mpReferenceKF(static_cast<KeyFrame *>(nullptr)),
+      mpReferenceKF(),
       mbImuPreintegrated(false),
       mpCamera(pCamera),
       mpCamera2(pCamera2),
@@ -1349,7 +1351,7 @@ bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit,
   if (viewCos < viewingCosLimit) return false;
 
   // Predict scale in the image
-  const int nPredictedLevel = pMP->PredictScale(dist, this);
+  const int nPredictedLevel = pMP->PredictScale(dist, shared_from_this());
 
   if (bRight) {
     pMP->mTrackProjXR = uv(0);
diff --git a/src/FrameDrawer.cc b/src/FrameDrawer.cc
index 22a686307ad..fd49e30022b 100644
--- a/src/FrameDrawer.cc
+++ b/src/FrameDrawer.cc
@@ -52,7 +52,7 @@ cv::Mat FrameDrawer::DrawFrame(float imageScale) {
   vector<float> vCurrentDepth;
   float thDepth;
 
-  Frame currentFrame;
+  std::shared_ptr<Frame> currentFrame;
   vector<MapPoint *> vpLocalMap;
   vector<cv::KeyPoint> vMatchesKeys;
   vector<MapPoint *> vpMatchedMPs;
@@ -326,15 +326,15 @@ void FrameDrawer::DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText) {
               cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(255, 255, 255), 1, 8);
 }
 
-void FrameDrawer::Update(Tracking *pTracker) {
+void FrameDrawer::Update(const std::shared_ptr<Tracking> &pTracker) {
   unique_lock<mutex> lock(mMutex);
   pTracker->mImGray.copyTo(mIm);
-  mvCurrentKeys = pTracker->mCurrentFrame.mvKeys;
-  mThDepth = pTracker->mCurrentFrame.mThDepth;
-  mvCurrentDepth = pTracker->mCurrentFrame.mvDepth;
+  mvCurrentKeys = pTracker->mCurrentFrame->mvKeys;
+  mThDepth = pTracker->mCurrentFrame->mThDepth;
+  mvCurrentDepth = pTracker->mCurrentFrame->mvDepth;
 
   if (both) {
-    mvCurrentKeysRight = pTracker->mCurrentFrame.mvKeysRight;
+    mvCurrentKeysRight = pTracker->mCurrentFrame->mvKeysRight;
     pTracker->mImRight.copyTo(mImRight);
     N = mvCurrentKeys.size() + mvCurrentKeysRight.size();
   } else {
@@ -347,7 +347,7 @@ void FrameDrawer::Update(Tracking *pTracker) {
 
   // Variables for the new visualization
   mCurrentFrame = pTracker->mCurrentFrame;
-  mmProjectPoints = mCurrentFrame.mmProjectPoints;
+  mmProjectPoints = mCurrentFrame->mmProjectPoints;
   mmMatchedInImage.clear();
 
   mvpLocalMap = pTracker->GetLocalMapMPS();
@@ -361,13 +361,14 @@ void FrameDrawer::Update(Tracking *pTracker) {
   mvpOutlierMPs.reserve(N);
 
   if (pTracker->mLastProcessedState == Tracking::NOT_INITIALIZED) {
-    mvIniKeys = pTracker->mInitialFrame.mvKeys;
+    mvIniKeys = pTracker->mInitialFrame->mvKeys;
     mvIniMatches = pTracker->mvIniMatches;
+
   } else if (pTracker->mLastProcessedState == Tracking::OK) {
     for (int i = 0; i < N; i++) {
-      MapPoint *pMP = pTracker->mCurrentFrame.mvpMapPoints[i];
+      MapPoint *pMP = pTracker->mCurrentFrame->mvpMapPoints.at(i);
       if (pMP) {
-        if (!pTracker->mCurrentFrame.mvbOutlier[i]) {
+        if (!pTracker->mCurrentFrame->mvbOutlier[i]) {
           if (pMP->Observations() > 0)
             mvbMap[i] = true;
           else
diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc
index fd853207556..b298940344b 100644
--- a/src/G2oTypes.cc
+++ b/src/G2oTypes.cc
@@ -28,7 +28,7 @@
 #include "ImuTypes.h"
 namespace ORB_SLAM3 {
 
-ImuCamPose::ImuCamPose(KeyFrame* pKF) : its(0) {
+ImuCamPose::ImuCamPose(const std::shared_ptr<KeyFrame>& pKF) : its(0) {
   // Load IMU pose
   twb = pKF->GetImuPosition().cast<double>();
   Rwb = pKF->GetImuRotation().cast<double>();
@@ -74,7 +74,7 @@ ImuCamPose::ImuCamPose(KeyFrame* pKF) : its(0) {
   DR.setIdentity();
 }
 
-ImuCamPose::ImuCamPose(Frame* pF) : its(0) {
+ImuCamPose::ImuCamPose(const std::shared_ptr<Frame>& pF) : its(0) {
   // Load IMU pose
   twb = pF->GetImuPosition().cast<double>();
   Rwb = pF->GetImuRotation().cast<double>();
@@ -121,7 +121,7 @@ ImuCamPose::ImuCamPose(Frame* pF) : its(0) {
 }
 
 ImuCamPose::ImuCamPose(Eigen::Matrix3d& _Rwc, Eigen::Vector3d& _twc,
-                       KeyFrame* pKF)
+                       const std::shared_ptr<KeyFrame>& pKF)
     : its(0) {
   // This is only for posegrpah, we do not care about multicamera
   tcw.resize(1);
@@ -251,7 +251,7 @@ void ImuCamPose::UpdateW(const double* pu) {
 }
 
 InvDepthPoint::InvDepthPoint(double _rho, double _u, double _v,
-                             KeyFrame* pHostKF)
+                             const std::shared_ptr<KeyFrame>& pHostKF)
     : u(_u),
       v(_v),
       rho(_rho),
@@ -444,29 +444,29 @@ void EdgeStereoOnlyPose::linearizeOplus() {
   _jacobianOplusXi = proj_jac * Rcb * SE3deriv;
 }
 
-VertexVelocity::VertexVelocity(KeyFrame* pKF) {
+VertexVelocity::VertexVelocity(const std::shared_ptr<KeyFrame>& pKF) {
   setEstimate(pKF->GetVelocity().cast<double>());
 }
 
-VertexVelocity::VertexVelocity(Frame* pF) {
+VertexVelocity::VertexVelocity(const std::shared_ptr<Frame>& pF) {
   setEstimate(pF->GetVelocity().cast<double>());
 }
 
-VertexGyroBias::VertexGyroBias(KeyFrame* pKF) {
+VertexGyroBias::VertexGyroBias(const std::shared_ptr<KeyFrame>& pKF) {
   setEstimate(pKF->GetGyroBias().cast<double>());
 }
 
-VertexGyroBias::VertexGyroBias(Frame* pF) {
+VertexGyroBias::VertexGyroBias(const std::shared_ptr<Frame>& pF) {
   Eigen::Vector3d bg;
   bg << pF->mImuBias.bwx, pF->mImuBias.bwy, pF->mImuBias.bwz;
   setEstimate(bg);
 }
 
-VertexAccBias::VertexAccBias(KeyFrame* pKF) {
+VertexAccBias::VertexAccBias(const std::shared_ptr<KeyFrame>& pKF) {
   setEstimate(pKF->GetAccBias().cast<double>());
 }
 
-VertexAccBias::VertexAccBias(Frame* pF) {
+VertexAccBias::VertexAccBias(const std::shared_ptr<Frame>& pF) {
   Eigen::Vector3d ba;
   ba << pF->mImuBias.bax, pF->mImuBias.bay, pF->mImuBias.baz;
   setEstimate(ba);
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index 4e264f7925d..4e685d22c5d 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -39,77 +39,78 @@ namespace ORB_SLAM3 {
 
 unsigned long int KeyFrame::nNextId = 0;
 
-KeyFrame::KeyFrame()
-    : mnFrameId(0),
-      mTimeStamp(0),
-      mnGridCols(FRAME_GRID_COLS),
-      mnGridRows(FRAME_GRID_ROWS),
-      mfGridElementWidthInv(0),
-      mfGridElementHeightInv(0),
-      mnTrackReferenceForFrame(0),
-      mnFuseTargetForKF(0),
-      mnBALocalForKF(0),
-      mnBAFixedForKF(0),
-      mnBALocalForMerge(0),
-      mnLoopQuery(0),
-      mnLoopWords(0),
-      mnRelocQuery(0),
-      mnRelocWords(0),
-      mnMergeQuery(0),
-      mnMergeWords(0),
-      mnBAGlobalForKF(0),
-      fx(0),
-      fy(0),
-      cx(0),
-      cy(0),
-      invfx(0),
-      invfy(0),
-      mnPlaceRecognitionQuery(0),
-      mnPlaceRecognitionWords(0),
-      mPlaceRecognitionScore(0),
-      mbf(0),
-      mb(0),
-      mThDepth(0),
-      N(0),
-      mvKeys(),
-      mvKeysUn(),
-      mvuRight(),
-      mvDepth(),
-      mnScaleLevels(0),
-      mfScaleFactor(0),
-      mfLogScaleFactor(0),
-      mvScaleFactors(0),
-      mvLevelSigma2(0),
-      mvInvLevelSigma2(0),
-      mnMinX(0),
-      mnMinY(0),
-      mnMaxX(0),
-      mnMaxY(0),
-      mPrevKF(nullptr),
-      mNextKF(nullptr),
-      mbFirstConnection(true),
-      mpBackupImuPreintegrated(std::make_shared<IMU::Preintegrated>()),
-      mpParent(NULL),
-      mbNotErase(false),
-      mbToBeErased(false),
-      mbBad(false),
-      mHalfBaseline(0),
-      mbCurrentPlaceRecognition(false),
-      mnMergeCorrectedForKF(0),
-      NLeft(0),
-      NRight(0),
-      mnNumberOfOpt(0),
-      mbHasVelocity(false) {}
-
-KeyFrame::KeyFrame(Frame &F, const std::shared_ptr<Map> &pMap,
+// KeyFrame::KeyFrame()
+//     : mnFrameId(0),
+//       mTimeStamp(0),
+//       mnGridCols(FRAME_GRID_COLS),
+//       mnGridRows(FRAME_GRID_ROWS),
+//       mfGridElementWidthInv(0),
+//       mfGridElementHeightInv(0),
+//       mnTrackReferenceForFrame(0),
+//       mnFuseTargetForKF(0),
+//       mnBALocalForKF(0),
+//       mnBAFixedForKF(0),
+//       mnBALocalForMerge(0),
+//       mnLoopQuery(0),
+//       mnLoopWords(0),
+//       mnRelocQuery(0),
+//       mnRelocWords(0),
+//       mnMergeQuery(0),
+//       mnMergeWords(0),
+//       mnBAGlobalForKF(0),
+//       fx(0),
+//       fy(0),
+//       cx(0),
+//       cy(0),
+//       invfx(0),
+//       invfy(0),
+//       mnPlaceRecognitionQuery(0),
+//       mnPlaceRecognitionWords(0),
+//       mPlaceRecognitionScore(0),
+//       mbf(0),
+//       mb(0),
+//       mThDepth(0),
+//       N(0),
+//       mvKeys(),
+//       mvKeysUn(),
+//       mvuRight(),
+//       mvDepth(),
+//       mnScaleLevels(0),
+//       mfScaleFactor(0),
+//       mfLogScaleFactor(0),
+//       mvScaleFactors(0),
+//       mvLevelSigma2(0),
+//       mvInvLevelSigma2(0),
+//       mnMinX(0),
+//       mnMinY(0),
+//       mnMaxX(0),
+//       mnMaxY(0),
+//       mPrevKF(nullptr),
+//       mNextKF(nullptr),
+//       mbFirstConnection(true),
+//       mpBackupImuPreintegrated(std::make_shared<IMU::Preintegrated>()),
+//       mpParent(NULL),
+//       mbNotErase(false),
+//       mbToBeErased(false),
+//       mbBad(false),
+//       mHalfBaseline(0),
+//       mbCurrentPlaceRecognition(false),
+//       mnMergeCorrectedForKF(0),
+//       NLeft(0),
+//       NRight(0),
+//       mnNumberOfOpt(0),
+//       mbHasVelocity(false) {}
+
+KeyFrame::KeyFrame(const std::shared_ptr<Frame> &F,
+                   const std::shared_ptr<Map> &pMap,
                    const std::shared_ptr<KeyFrameDatabase> &pKFDB)
     : bImu(pMap->isImuInitialized()),
-      mnFrameId(F.mnId),
-      mTimeStamp(F.mTimeStamp),
+      mnFrameId(F->mnId),
+      mTimeStamp(F->mTimeStamp),
       mnGridCols(FRAME_GRID_COLS),
       mnGridRows(FRAME_GRID_ROWS),
-      mfGridElementWidthInv(F.mfGridElementWidthInv),
-      mfGridElementHeightInv(F.mfGridElementHeightInv),
+      mfGridElementWidthInv(F->mfGridElementWidthInv),
+      mfGridElementHeightInv(F->mfGridElementHeightInv),
       mnTrackReferenceForFrame(0),
       mnFuseTargetForKF(0),
       mnBALocalForKF(0),
@@ -123,90 +124,90 @@ KeyFrame::KeyFrame(Frame &F, const std::shared_ptr<Map> &pMap,
       mnPlaceRecognitionQuery(0),
       mnPlaceRecognitionWords(0),
       mPlaceRecognitionScore(0),
-      fx(F.fx),
-      fy(F.fy),
-      cx(F.cx),
-      cy(F.cy),
-      invfx(F.invfx),
-      invfy(F.invfy),
-      mbf(F.mbf),
-      mb(F.mb),
-      mThDepth(F.mThDepth),
-      N(F.N),
-      mvKeys(F.mvKeys),
-      mvKeysUn(F.mvKeysUn),
-      mvuRight(F.mvuRight),
-      mvDepth(F.mvDepth),
-      mDescriptors(F.mDescriptors.clone()),
-      mBowVec(F.mBowVec),
-      mFeatVec(F.mFeatVec),
-      mnScaleLevels(F.mnScaleLevels),
-      mfScaleFactor(F.mfScaleFactor),
-      mfLogScaleFactor(F.mfLogScaleFactor),
-      mvScaleFactors(F.mvScaleFactors),
-      mvLevelSigma2(F.mvLevelSigma2),
-      mvInvLevelSigma2(F.mvInvLevelSigma2),
-      mnMinX(F.mnMinX),
-      mnMinY(F.mnMinY),
-      mnMaxX(F.mnMaxX),
-      mnMaxY(F.mnMaxY),
-      mK_(F.mK_),
+      fx(F->fx),
+      fy(F->fy),
+      cx(F->cx),
+      cy(F->cy),
+      invfx(F->invfx),
+      invfy(F->invfy),
+      mbf(F->mbf),
+      mb(F->mb),
+      mThDepth(F->mThDepth),
+      N(F->N),
+      mvKeys(F->mvKeys),
+      mvKeysUn(F->mvKeysUn),
+      mvuRight(F->mvuRight),
+      mvDepth(F->mvDepth),
+      mDescriptors(F->mDescriptors.clone()),
+      mBowVec(F->mBowVec),
+      mFeatVec(F->mFeatVec),
+      mnScaleLevels(F->mnScaleLevels),
+      mfScaleFactor(F->mfScaleFactor),
+      mfLogScaleFactor(F->mfLogScaleFactor),
+      mvScaleFactors(F->mvScaleFactors),
+      mvLevelSigma2(F->mvLevelSigma2),
+      mvInvLevelSigma2(F->mvInvLevelSigma2),
+      mnMinX(F->mnMinX),
+      mnMinY(F->mnMinY),
+      mnMaxX(F->mnMaxX),
+      mnMaxY(F->mnMaxY),
+      mK_(F->mK_),
       mPrevKF(NULL),
       mNextKF(NULL),
-      mpImuPreintegrated(F.mpImuPreintegrated),
+      mpImuPreintegrated(F->mpImuPreintegrated),
       mpBackupImuPreintegrated(std::make_shared<IMU::Preintegrated>()),
-      mImuCalib(F.mImuCalib),
-      mvpMapPoints(F.mvpMapPoints),
+      mImuCalib(F->mImuCalib),
+      mvpMapPoints(F->mvpMapPoints),
       mpKeyFrameDB(pKFDB),
-      mpORBvocabulary(F.mpORBvocabulary),
+      mpORBvocabulary(F->mpORBvocabulary),
       mbFirstConnection(true),
       mpParent(NULL),
-      mDistCoef(F.mDistCoef),
+      mDistCoef(F->mDistCoef),
       mbNotErase(false),
-      mnDataset(F.mnDataset),
+      mnDataset(F->mnDataset),
       mbToBeErased(false),
       mbBad(false),
-      mHalfBaseline(F.mb / 2),
+      mHalfBaseline(F->mb / 2),
       mpMap(pMap),
       mbCurrentPlaceRecognition(false),
-      mNameFile(F.mNameFile),
+      mNameFile(F->mNameFile),
       mnMergeCorrectedForKF(0),
-      mpCamera(F.mpCamera),
-      mpCamera2(F.mpCamera2),
-      mvLeftToRightMatch(F.mvLeftToRightMatch),
-      mvRightToLeftMatch(F.mvRightToLeftMatch),
-      mTlr(F.GetRelativePoseTlr()),
-      mvKeysRight(F.mvKeysRight),
-      NLeft(F.Nleft),
-      NRight(F.Nright),
-      mTrl(F.GetRelativePoseTrl()),
+      mpCamera(F->mpCamera),
+      mpCamera2(F->mpCamera2),
+      mvLeftToRightMatch(F->mvLeftToRightMatch),
+      mvRightToLeftMatch(F->mvRightToLeftMatch),
+      mTlr(F->GetRelativePoseTlr()),
+      mvKeysRight(F->mvKeysRight),
+      NLeft(F->Nleft),
+      NRight(F->Nright),
+      mTrl(F->GetRelativePoseTrl()),
       mnNumberOfOpt(0),
       mbHasVelocity(false) {
   mnId = nNextId++;
 
   mGrid.resize(mnGridCols);
-  if (F.Nleft != -1) mGridRight.resize(mnGridCols);
+  if (F->Nleft != -1) mGridRight.resize(mnGridCols);
   for (int i = 0; i < mnGridCols; i++) {
     mGrid[i].resize(mnGridRows);
-    if (F.Nleft != -1) mGridRight[i].resize(mnGridRows);
+    if (F->Nleft != -1) mGridRight[i].resize(mnGridRows);
     for (int j = 0; j < mnGridRows; j++) {
-      mGrid[i][j] = F.mGrid[i][j];
-      if (F.Nleft != -1) {
-        mGridRight[i][j] = F.mGridRight[i][j];
+      mGrid[i][j] = F->mGrid[i][j];
+      if (F->Nleft != -1) {
+        mGridRight[i][j] = F->mGridRight[i][j];
       }
     }
   }
 
-  if (!F.HasVelocity()) {
+  if (!F->HasVelocity()) {
     mVw.setZero();
     mbHasVelocity = false;
   } else {
-    mVw = F.GetVelocity();
+    mVw = F->GetVelocity();
     mbHasVelocity = true;
   }
 
-  mImuBias = F.mImuBias;
-  SetPose(F.GetPose());
+  mImuBias = F->mImuBias;
+  SetPose(F->GetPose());
 
   mnOriginMapId = pMap->GetId();
 }
@@ -291,7 +292,8 @@ bool KeyFrame::isVelocitySet() {
   return mbHasVelocity;
 }
 
-void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) {
+void KeyFrame::AddConnection(const std::shared_ptr<KeyFrame> &pKF,
+                             const int &weight) {
   {
     unique_lock<mutex> lock(mMutexConnections);
     if (!mConnectedKeyFrameWeights.count(pKF))
@@ -307,15 +309,16 @@ void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight) {
 
 void KeyFrame::UpdateBestCovisibles() {
   unique_lock<mutex> lock(mMutexConnections);
-  vector<pair<int, KeyFrame *>> vPairs;
+  vector<pair<int, std::shared_ptr<KeyFrame>>> vPairs;
   vPairs.reserve(mConnectedKeyFrameWeights.size());
-  for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin(),
-                                      mend = mConnectedKeyFrameWeights.end();
+  for (map<std::shared_ptr<KeyFrame>, int>::iterator
+           mit = mConnectedKeyFrameWeights.begin(),
+           mend = mConnectedKeyFrameWeights.end();
        mit != mend; mit++)
     vPairs.push_back(make_pair(mit->second, mit->first));
 
   sort(vPairs.begin(), vPairs.end());
-  list<KeyFrame *> lKFs;
+  list<std::shared_ptr<KeyFrame>> lKFs;
   list<int> lWs;
   for (size_t i = 0, iend = vPairs.size(); i < iend; i++) {
     if (!vPairs[i].second->isBad()) {
@@ -324,38 +327,42 @@ void KeyFrame::UpdateBestCovisibles() {
     }
   }
 
-  mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());
+  mvpOrderedConnectedKeyFrames =
+      vector<std::shared_ptr<KeyFrame>>(lKFs.begin(), lKFs.end());
   mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
 }
 
-set<KeyFrame *> KeyFrame::GetConnectedKeyFrames() {
+set<std::shared_ptr<KeyFrame>> KeyFrame::GetConnectedKeyFrames() {
   unique_lock<mutex> lock(mMutexConnections);
-  set<KeyFrame *> s;
-  for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin();
+  set<std::shared_ptr<KeyFrame>> s;
+  for (map<std::shared_ptr<KeyFrame>, int>::iterator mit =
+           mConnectedKeyFrameWeights.begin();
        mit != mConnectedKeyFrameWeights.end(); mit++)
     s.insert(mit->first);
   return s;
 }
 
-vector<KeyFrame *> KeyFrame::GetVectorCovisibleKeyFrames() {
+vector<std::shared_ptr<KeyFrame>> KeyFrame::GetVectorCovisibleKeyFrames() {
   unique_lock<mutex> lock(mMutexConnections);
   return mvpOrderedConnectedKeyFrames;
 }
 
-vector<KeyFrame *> KeyFrame::GetBestCovisibilityKeyFrames(const int &N) {
+vector<std::shared_ptr<KeyFrame>> KeyFrame::GetBestCovisibilityKeyFrames(
+    int N) {
   unique_lock<mutex> lock(mMutexConnections);
-  if (static_cast<int>(mvpOrderedConnectedKeyFrames.size() < N))
+  if (mvpOrderedConnectedKeyFrames.size() < N)
     return mvpOrderedConnectedKeyFrames;
   else
-    return vector<KeyFrame *>(mvpOrderedConnectedKeyFrames.begin(),
-                              mvpOrderedConnectedKeyFrames.begin() + N);
+    return vector<std::shared_ptr<KeyFrame>>(
+        mvpOrderedConnectedKeyFrames.begin(),
+        mvpOrderedConnectedKeyFrames.begin() + N);
 }
 
-vector<KeyFrame *> KeyFrame::GetCovisiblesByWeight(const int &w) {
+vector<std::shared_ptr<KeyFrame>> KeyFrame::GetCovisiblesByWeight(int w) {
   unique_lock<mutex> lock(mMutexConnections);
 
   if (mvpOrderedConnectedKeyFrames.empty()) {
-    return vector<KeyFrame *>();
+    return vector<std::shared_ptr<KeyFrame>>();
   }
 
   vector<int>::iterator it =
@@ -363,15 +370,16 @@ vector<KeyFrame *> KeyFrame::GetCovisiblesByWeight(const int &w) {
                   KeyFrame::weightComp);
 
   if (it == mvOrderedWeights.end() && mvOrderedWeights.back() < w) {
-    return vector<KeyFrame *>();
+    return vector<std::shared_ptr<KeyFrame>>();
   } else {
     int n = it - mvOrderedWeights.begin();
-    return vector<KeyFrame *>(mvpOrderedConnectedKeyFrames.begin(),
-                              mvpOrderedConnectedKeyFrames.begin() + n);
+    return vector<std::shared_ptr<KeyFrame>>(
+        mvpOrderedConnectedKeyFrames.begin(),
+        mvpOrderedConnectedKeyFrames.begin() + n);
   }
 }
 
-int KeyFrame::GetWeight(KeyFrame *pKF) {
+int KeyFrame::GetWeight(const std::shared_ptr<KeyFrame> &pKF) {
   unique_lock<mutex> lock(mMutexConnections);
   if (mConnectedKeyFrameWeights.count(pKF))
     return mConnectedKeyFrameWeights[pKF];
@@ -400,7 +408,7 @@ void KeyFrame::EraseMapPointMatch(const int &idx) {
 }
 
 void KeyFrame::EraseMapPointMatch(MapPoint *pMP) {
-  tuple<size_t, size_t> indexes = pMP->GetIndexInKeyFrame(this);
+  tuple<size_t, size_t> indexes = pMP->GetIndexInKeyFrame(shared_from_this());
   size_t leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
   if (leftIndex != -1) mvpMapPoints[leftIndex] = static_cast<MapPoint *>(NULL);
   if (rightIndex != -1)
@@ -454,7 +462,7 @@ MapPoint *KeyFrame::GetMapPoint(const size_t &idx) {
 }
 
 void KeyFrame::UpdateConnections(bool upParent) {
-  map<KeyFrame *, int> KFcounter;
+  map<std::shared_ptr<KeyFrame>, int> KFcounter;
 
   vector<MapPoint *> vpMP;
 
@@ -473,10 +481,12 @@ void KeyFrame::UpdateConnections(bool upParent) {
 
     if (pMP->isBad()) continue;
 
-    map<KeyFrame *, tuple<int, int>> observations = pMP->GetObservations();
+    map<std::shared_ptr<KeyFrame>, tuple<int, int>> observations =
+        pMP->GetObservations();
 
-    for (map<KeyFrame *, tuple<int, int>>::iterator mit = observations.begin(),
-                                                    mend = observations.end();
+    for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::iterator
+             mit = observations.begin(),
+             mend = observations.end();
          mit != mend; mit++) {
       if (mit->first->mnId == mnId || mit->first->isBad() ||
           mit->first->GetMap() != mpMap)
@@ -492,14 +502,14 @@ void KeyFrame::UpdateConnections(bool upParent) {
   // In case no keyframe counter is over threshold add the one with maximum
   // counter
   int nmax = 0;
-  KeyFrame *pKFmax = NULL;
+  std::shared_ptr<KeyFrame> pKFmax = NULL;
   int th = 15;
 
-  vector<pair<int, KeyFrame *>> vPairs;
+  vector<pair<int, std::shared_ptr<KeyFrame>>> vPairs;
   vPairs.reserve(KFcounter.size());
   if (!upParent) cout << "UPDATE_CONN: current KF " << mnId << endl;
-  for (map<KeyFrame *, int>::iterator mit = KFcounter.begin(),
-                                      mend = KFcounter.end();
+  for (map<std::shared_ptr<KeyFrame>, int>::iterator mit = KFcounter.begin(),
+                                                     mend = KFcounter.end();
        mit != mend; mit++) {
     if (!upParent)
       cout << "  UPDATE_CONN: KF " << mit->first->mnId
@@ -510,17 +520,17 @@ void KeyFrame::UpdateConnections(bool upParent) {
     }
     if (mit->second >= th) {
       vPairs.push_back(make_pair(mit->second, mit->first));
-      (mit->first)->AddConnection(this, mit->second);
+      (mit->first)->AddConnection(shared_from_this(), mit->second);
     }
   }
 
   if (vPairs.empty()) {
     vPairs.push_back(make_pair(nmax, pKFmax));
-    pKFmax->AddConnection(this, nmax);
+    pKFmax->AddConnection(shared_from_this(), nmax);
   }
 
   sort(vPairs.begin(), vPairs.end());
-  list<KeyFrame *> lKFs;
+  list<std::shared_ptr<KeyFrame>> lKFs;
   list<int> lWs;
   for (size_t i = 0; i < vPairs.size(); i++) {
     lKFs.push_front(vPairs[i].second);
@@ -531,50 +541,51 @@ void KeyFrame::UpdateConnections(bool upParent) {
     unique_lock<mutex> lockCon(mMutexConnections);
 
     mConnectedKeyFrameWeights = KFcounter;
-    mvpOrderedConnectedKeyFrames = vector<KeyFrame *>(lKFs.begin(), lKFs.end());
+    mvpOrderedConnectedKeyFrames =
+        vector<std::shared_ptr<KeyFrame>>(lKFs.begin(), lKFs.end());
     mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
 
     if (mbFirstConnection && mnId != mpMap->GetInitKFid()) {
       mpParent = mvpOrderedConnectedKeyFrames.front();
-      mpParent->AddChild(this);
+      mpParent->AddChild(shared_from_this());
       mbFirstConnection = false;
     }
   }
 }
 
-void KeyFrame::AddChild(KeyFrame *pKF) {
+void KeyFrame::AddChild(const std::shared_ptr<KeyFrame> &pKF) {
   unique_lock<mutex> lockCon(mMutexConnections);
   mspChildrens.insert(pKF);
 }
 
-void KeyFrame::EraseChild(KeyFrame *pKF) {
+void KeyFrame::EraseChild(const std::shared_ptr<KeyFrame> &pKF) {
   unique_lock<mutex> lockCon(mMutexConnections);
   mspChildrens.erase(pKF);
 }
 
-void KeyFrame::ChangeParent(KeyFrame *pKF) {
+void KeyFrame::ChangeParent(const std::shared_ptr<KeyFrame> &pKF) {
   unique_lock<mutex> lockCon(mMutexConnections);
-  if (pKF == this) {
+  if (pKF == shared_from_this()) {
     cout << "ERROR: Change parent KF, the parent and child are the same KF"
          << endl;
     throw std::invalid_argument("The parent and child can not be the same");
   }
 
   mpParent = pKF;
-  pKF->AddChild(this);
+  pKF->AddChild(shared_from_this());
 }
 
-set<KeyFrame *> KeyFrame::GetChilds() {
+set<std::shared_ptr<KeyFrame>> KeyFrame::GetChilds() {
   unique_lock<mutex> lockCon(mMutexConnections);
   return mspChildrens;
 }
 
-KeyFrame *KeyFrame::GetParent() {
+std::shared_ptr<KeyFrame> KeyFrame::GetParent() {
   unique_lock<mutex> lockCon(mMutexConnections);
   return mpParent;
 }
 
-bool KeyFrame::hasChild(KeyFrame *pKF) {
+bool KeyFrame::hasChild(const std::shared_ptr<KeyFrame> &pKF) {
   unique_lock<mutex> lockCon(mMutexConnections);
   return mspChildrens.count(pKF);
 }
@@ -584,24 +595,24 @@ void KeyFrame::SetFirstConnection(bool bFirst) {
   mbFirstConnection = bFirst;
 }
 
-void KeyFrame::AddLoopEdge(KeyFrame *pKF) {
+void KeyFrame::AddLoopEdge(const std::shared_ptr<KeyFrame> &pKF) {
   unique_lock<mutex> lockCon(mMutexConnections);
   mbNotErase = true;
   mspLoopEdges.insert(pKF);
 }
 
-set<KeyFrame *> KeyFrame::GetLoopEdges() {
+set<std::shared_ptr<KeyFrame>> KeyFrame::GetLoopEdges() {
   unique_lock<mutex> lockCon(mMutexConnections);
   return mspLoopEdges;
 }
 
-void KeyFrame::AddMergeEdge(KeyFrame *pKF) {
+void KeyFrame::AddMergeEdge(const std::shared_ptr<KeyFrame> &pKF) {
   unique_lock<mutex> lockCon(mMutexConnections);
   mbNotErase = true;
   mspMergeEdges.insert(pKF);
 }
 
-set<KeyFrame *> KeyFrame::GetMergeEdges() {
+set<std::shared_ptr<KeyFrame>> KeyFrame::GetMergeEdges() {
   unique_lock<mutex> lockCon(mMutexConnections);
   return mspMergeEdges;
 }
@@ -635,15 +646,16 @@ void KeyFrame::SetBadFlag() {
     }
   }
 
-  for (map<KeyFrame *, int>::iterator mit = mConnectedKeyFrameWeights.begin(),
-                                      mend = mConnectedKeyFrameWeights.end();
+  for (map<std::shared_ptr<KeyFrame>, int>::iterator
+           mit = mConnectedKeyFrameWeights.begin(),
+           mend = mConnectedKeyFrameWeights.end();
        mit != mend; mit++) {
-    mit->first->EraseConnection(this);
+    mit->first->EraseConnection(shared_from_this());
   }
 
   for (size_t i = 0; i < mvpMapPoints.size(); i++) {
     if (mvpMapPoints[i]) {
-      mvpMapPoints[i]->EraseObservation(this);
+      mvpMapPoints[i]->EraseObservation(shared_from_this());
     }
   }
 
@@ -655,7 +667,7 @@ void KeyFrame::SetBadFlag() {
     mvpOrderedConnectedKeyFrames.clear();
 
     // Update Spanning Tree
-    set<KeyFrame *> sParentCandidates;
+    set<std::shared_ptr<KeyFrame>> sParentCandidates;
     if (mpParent) sParentCandidates.insert(mpParent);
 
     // Assign at each iteration one children with a parent (the pair with
@@ -665,20 +677,22 @@ void KeyFrame::SetBadFlag() {
       bool bContinue = false;
 
       int max = -1;
-      KeyFrame *pC;
-      KeyFrame *pP;
+      std::shared_ptr<KeyFrame> pC;
+      std::shared_ptr<KeyFrame> pP;
 
-      for (set<KeyFrame *>::iterator sit = mspChildrens.begin(),
-                                     send = mspChildrens.end();
+      for (set<std::shared_ptr<KeyFrame>>::iterator sit = mspChildrens.begin(),
+                                                    send = mspChildrens.end();
            sit != send; sit++) {
-        KeyFrame *pKF = *sit;
+        std::shared_ptr<KeyFrame> pKF = *sit;
         if (pKF->isBad()) continue;
 
         // Check if a parent candidate is connected to the keyframe
-        vector<KeyFrame *> vpConnected = pKF->GetVectorCovisibleKeyFrames();
+        vector<std::shared_ptr<KeyFrame>> vpConnected =
+            pKF->GetVectorCovisibleKeyFrames();
         for (size_t i = 0, iend = vpConnected.size(); i < iend; i++) {
-          for (set<KeyFrame *>::iterator spcit = sParentCandidates.begin(),
-                                         spcend = sParentCandidates.end();
+          for (set<std::shared_ptr<KeyFrame>>::iterator
+                   spcit = sParentCandidates.begin(),
+                   spcend = sParentCandidates.end();
                spcit != spcend; spcit++) {
             if (vpConnected[i]->mnId == (*spcit)->mnId) {
               int w = pKF->GetWeight(vpConnected[i]);
@@ -705,21 +719,21 @@ void KeyFrame::SetBadFlag() {
     // If a children has no covisibility links with any parent candidate, assign
     // to the original parent of this KF
     if (!mspChildrens.empty()) {
-      for (set<KeyFrame *>::iterator sit = mspChildrens.begin();
+      for (set<std::shared_ptr<KeyFrame>>::iterator sit = mspChildrens.begin();
            sit != mspChildrens.end(); sit++) {
         (*sit)->ChangeParent(mpParent);
       }
     }
 
     if (mpParent) {
-      mpParent->EraseChild(this);
+      mpParent->EraseChild(shared_from_this());
       mTcp = mTcw * mpParent->GetPoseInverse();
     }
     mbBad = true;
   }
 
-  mpMap->EraseKeyFrame(this);
-  mpKeyFrameDB->erase(this);
+  mpMap->EraseKeyFrame(shared_from_this());
+  mpKeyFrameDB->erase(shared_from_this());
 }
 
 bool KeyFrame::isBad() {
@@ -727,7 +741,7 @@ bool KeyFrame::isBad() {
   return mbBad;
 }
 
-void KeyFrame::EraseConnection(KeyFrame *pKF) {
+void KeyFrame::EraseConnection(const std::shared_ptr<KeyFrame> &pKF) {
   bool bUpdate = false;
   {
     unique_lock<mutex> lock(mMutexConnections);
@@ -870,7 +884,8 @@ void KeyFrame::UpdateMap(const std::shared_ptr<Map> &pMap) {
   mpMap = pMap;
 }
 
-void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP,
+void KeyFrame::PreSave(set<std::shared_ptr<KeyFrame>> &spKF,
+                       set<MapPoint *> &spMP,
                        set<std::shared_ptr<GeometricCamera>> &spCam) {
   // Save the id of each MapPoint in this KF, there can be null pointer in the
   // vector
@@ -886,7 +901,7 @@ void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP,
   }
   // Save the id of each connected KF with it weight
   mBackupConnectedKeyFrameIdWeights.clear();
-  for (std::map<KeyFrame *, int>::const_iterator
+  for (std::map<std::shared_ptr<KeyFrame>, int>::const_iterator
            it = mConnectedKeyFrameWeights.begin(),
            end = mConnectedKeyFrameWeights.end();
        it != end; ++it) {
@@ -902,7 +917,7 @@ void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP,
   // Save the id of the childrens KF
   mvBackupChildrensId.clear();
   mvBackupChildrensId.reserve(mspChildrens.size());
-  for (KeyFrame *pKFi : mspChildrens) {
+  for (auto pKFi : mspChildrens) {
     if (spKF.find(pKFi) != spKF.end())
       mvBackupChildrensId.push_back(pKFi->mnId);
   }
@@ -910,7 +925,7 @@ void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP,
   // Save the id of the loop edge KF
   mvBackupLoopEdgesId.clear();
   mvBackupLoopEdgesId.reserve(mspLoopEdges.size());
-  for (KeyFrame *pKFi : mspLoopEdges) {
+  for (auto pKFi : mspLoopEdges) {
     if (spKF.find(pKFi) != spKF.end())
       mvBackupLoopEdgesId.push_back(pKFi->mnId);
   }
@@ -918,7 +933,7 @@ void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP,
   // Save the id of the merge edge KF
   mvBackupMergeEdgesId.clear();
   mvBackupMergeEdgesId.reserve(mspMergeEdges.size());
-  for (KeyFrame *pKFi : mspMergeEdges) {
+  for (auto pKFi : mspMergeEdges) {
     if (spKF.find(pKFi) != spKF.end())
       mvBackupMergeEdgesId.push_back(pKFi->mnId);
   }
@@ -946,7 +961,7 @@ void KeyFrame::PreSave(set<KeyFrame *> &spKF, set<MapPoint *> &spMP,
 }
 
 void KeyFrame::PostLoad(
-    map<unsigned long int, KeyFrame *> &mpKFid,
+    map<unsigned long int, std::shared_ptr<KeyFrame>> &mpKFid,
     map<unsigned long int, MapPoint *> &mpMPid,
     map<unsigned int, std::shared_ptr<GeometricCamera>> &mpCamId) {
   // Rebuild the empty variables
@@ -973,7 +988,7 @@ void KeyFrame::PostLoad(
            it = mBackupConnectedKeyFrameIdWeights.begin(),
            end = mBackupConnectedKeyFrameIdWeights.end();
        it != end; ++it) {
-    KeyFrame *pKFi = mpKFid[it->first];
+    std::shared_ptr<KeyFrame> pKFi = mpKFid[it->first];
     mConnectedKeyFrameWeights[pKFi] = it->second;
   }
 
diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc
index b56d18ad6a9..8197b198cf1 100644
--- a/src/KeyFrameDatabase.cc
+++ b/src/KeyFrameDatabase.cc
@@ -38,27 +38,21 @@ KeyFrameDatabase::KeyFrameDatabase(const std::shared_ptr<ORBVocabulary>& voc)
   mvInvertedFile.resize(voc->size());
 }
 
-void KeyFrameDatabase::add(KeyFrame* pKF) {
+void KeyFrameDatabase::add(const std::shared_ptr<KeyFrame>& pKF) {
   unique_lock<mutex> lock(mMutex);
 
-  for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(),
-                                        vend = pKF->mBowVec.end();
-       vit != vend; vit++)
-    mvInvertedFile[vit->first].push_back(pKF);
+  for (auto const& vit : pKF->mBowVec) mvInvertedFile[vit.first].push_back(pKF);
 }
 
-void KeyFrameDatabase::erase(KeyFrame* pKF) {
+void KeyFrameDatabase::erase(const std::shared_ptr<KeyFrame>& pKF) {
   unique_lock<mutex> lock(mMutex);
 
   // Erase elements in the Inverse File for the entry
-  for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(),
-                                        vend = pKF->mBowVec.end();
-       vit != vend; vit++) {
+  for (auto vit : pKF->mBowVec) {
     // List of keyframes that share the word
-    list<KeyFrame*>& lKFs = mvInvertedFile[vit->first];
+    auto& lKFs = mvInvertedFile[vit.first];
 
-    for (list<KeyFrame*>::iterator lit = lKFs.begin(), lend = lKFs.end();
-         lit != lend; lit++) {
+    for (auto lit = lKFs.begin(), lend = lKFs.end(); lit != lend; lit++) {
       if (pKF == *lit) {
         lKFs.erase(lit);
         break;
@@ -76,16 +70,10 @@ void KeyFrameDatabase::clearMap(const std::shared_ptr<Map>& pMap) {
   unique_lock<mutex> lock(mMutex);
 
   // Erase elements in the Inverse File for the entry
-  for (std::vector<list<KeyFrame*> >::iterator vit = mvInvertedFile.begin(),
-                                               vend = mvInvertedFile.end();
-       vit != vend; vit++) {
+  for (auto lKFs : mvInvertedFile) {
     // List of keyframes that share the word
-    list<KeyFrame*>& lKFs = *vit;
-
-    for (list<KeyFrame*>::iterator lit = lKFs.begin(), lend = lKFs.end();
-         lit != lend;) {
-      KeyFrame* pKFi = *lit;
-      if (pMap == pKFi->GetMap()) {
+    for (auto lit = lKFs.begin(), lend = lKFs.end(); lit != lend;) {
+      if (pMap == (*lit)->GetMap()) {
         lit = lKFs.erase(lit);
         // Dont delete the KF because the class Map clean all the KF when it is
         // destroyed
@@ -96,25 +84,21 @@ void KeyFrameDatabase::clearMap(const std::shared_ptr<Map>& pMap) {
   }
 }
 
-vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF,
-                                                         float minScore) {
-  set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
-  list<KeyFrame*> lKFsSharingWords;
+vector<std::shared_ptr<KeyFrame>> KeyFrameDatabase::DetectLoopCandidates(
+    const std::shared_ptr<KeyFrame>& pKF, float minScore) {
+  set<std::shared_ptr<KeyFrame>> spConnectedKeyFrames =
+      pKF->GetConnectedKeyFrames();
+  list<std::shared_ptr<KeyFrame>> lKFsSharingWords;
 
   // Search all keyframes that share a word with current keyframes
   // Discard keyframes connected to the query keyframe
   {
     unique_lock<mutex> lock(mMutex);
 
-    for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(),
-                                          vend = pKF->mBowVec.end();
-         vit != vend; vit++) {
-      list<KeyFrame*>& lKFs = mvInvertedFile[vit->first];
-
-      for (list<KeyFrame*>::iterator lit = lKFs.begin(), lend = lKFs.end();
-           lit != lend; lit++) {
-        KeyFrame* pKFi = *lit;
+    for (auto const vit : pKF->mBowVec) {
+      auto const& lKFs = mvInvertedFile[vit.first];
 
+      for (auto pKFi : lKFs) {
         // For consider a loop candidate it a candidate it must be in the same
         // map
         if (pKFi->GetMap() == pKF->GetMap()) {
@@ -131,14 +115,14 @@ vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF,
     }
   }
 
-  if (lKFsSharingWords.empty()) return vector<KeyFrame*>();
+  if (lKFsSharingWords.empty()) return vector<std::shared_ptr<KeyFrame>>();
 
-  list<pair<float, KeyFrame*> > lScoreAndMatch;
+  list<pair<float, std::shared_ptr<KeyFrame>>> lScoreAndMatch;
 
   // Only compare against those keyframes that share enough words
   int maxCommonWords = 0;
-  for (list<KeyFrame*>::iterator lit = lKFsSharingWords.begin(),
-                                 lend = lKFsSharingWords.end();
+  for (list<std::shared_ptr<KeyFrame>>::iterator lit = lKFsSharingWords.begin(),
+                                                 lend = lKFsSharingWords.end();
        lit != lend; lit++) {
     if ((*lit)->mnLoopWords > maxCommonWords)
       maxCommonWords = (*lit)->mnLoopWords;
@@ -150,10 +134,10 @@ vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF,
 
   // Compute similarity score. Retain the matches whose score is higher than
   // minScore
-  for (list<KeyFrame*>::iterator lit = lKFsSharingWords.begin(),
-                                 lend = lKFsSharingWords.end();
+  for (list<std::shared_ptr<KeyFrame>>::iterator lit = lKFsSharingWords.begin(),
+                                                 lend = lKFsSharingWords.end();
        lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
+    std::shared_ptr<KeyFrame> pKFi = *lit;
 
     if (pKFi->mnLoopWords > minCommonWords) {
       nscores++;
@@ -165,25 +149,27 @@ vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF,
     }
   }
 
-  if (lScoreAndMatch.empty()) return vector<KeyFrame*>();
+  if (lScoreAndMatch.empty()) return vector<std::shared_ptr<KeyFrame>>();
 
-  list<pair<float, KeyFrame*> > lAccScoreAndMatch;
+  list<pair<float, std::shared_ptr<KeyFrame>>> lAccScoreAndMatch;
   float bestAccScore = minScore;
 
   // Lets now accumulate score by covisibility
-  for (list<pair<float, KeyFrame*> >::iterator it = lScoreAndMatch.begin(),
-                                               itend = lScoreAndMatch.end();
+  for (list<pair<float, std::shared_ptr<KeyFrame>>>::iterator
+           it = lScoreAndMatch.begin(),
+           itend = lScoreAndMatch.end();
        it != itend; it++) {
-    KeyFrame* pKFi = it->second;
-    vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+    std::shared_ptr<KeyFrame> pKFi = it->second;
+    vector<std::shared_ptr<KeyFrame>> vpNeighs =
+        pKFi->GetBestCovisibilityKeyFrames(10);
 
     float bestScore = it->first;
     float accScore = it->first;
-    KeyFrame* pBestKF = pKFi;
-    for (vector<KeyFrame*>::iterator vit = vpNeighs.begin(),
-                                     vend = vpNeighs.end();
+    std::shared_ptr<KeyFrame> pBestKF = pKFi;
+    for (vector<std::shared_ptr<KeyFrame>>::iterator vit = vpNeighs.begin(),
+                                                     vend = vpNeighs.end();
          vit != vend; vit++) {
-      KeyFrame* pKF2 = *vit;
+      std::shared_ptr<KeyFrame> pKF2 = *vit;
       if (pKF2->mnLoopQuery == pKF->mnId &&
           pKF2->mnLoopWords > minCommonWords) {
         accScore += pKF2->mLoopScore;
@@ -201,15 +187,16 @@ vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF,
   // Return all those keyframes with a score higher than 0.75*bestScore
   float minScoreToRetain = 0.75f * bestAccScore;
 
-  set<KeyFrame*> spAlreadyAddedKF;
-  vector<KeyFrame*> vpLoopCandidates;
+  set<std::shared_ptr<KeyFrame>> spAlreadyAddedKF;
+  vector<std::shared_ptr<KeyFrame>> vpLoopCandidates;
   vpLoopCandidates.reserve(lAccScoreAndMatch.size());
 
-  for (list<pair<float, KeyFrame*> >::iterator it = lAccScoreAndMatch.begin(),
-                                               itend = lAccScoreAndMatch.end();
+  for (list<pair<float, std::shared_ptr<KeyFrame>>>::iterator
+           it = lAccScoreAndMatch.begin(),
+           itend = lAccScoreAndMatch.end();
        it != itend; it++) {
     if (it->first > minScoreToRetain) {
-      KeyFrame* pKFi = it->second;
+      std::shared_ptr<KeyFrame> pKFi = it->second;
       if (!spAlreadyAddedKF.count(pKFi)) {
         vpLoopCandidates.push_back(pKFi);
         spAlreadyAddedKF.insert(pKFi);
@@ -220,11 +207,12 @@ vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF,
   return vpLoopCandidates;
 }
 
-void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
-                                        vector<KeyFrame*>& vpLoopCand,
-                                        vector<KeyFrame*>& vpMergeCand) {
-  set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
-  list<KeyFrame*> lKFsSharingWordsLoop, lKFsSharingWordsMerge;
+void KeyFrameDatabase::DetectCandidates(
+    const std::shared_ptr<KeyFrame>& pKF, float minScore,
+    vector<std::shared_ptr<KeyFrame>>& vpLoopCand,
+    vector<std::shared_ptr<KeyFrame>>& vpMergeCand) {
+  auto spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
+  list<std::shared_ptr<KeyFrame>> lKFsSharingWordsLoop, lKFsSharingWordsMerge;
 
   // Search all keyframes that share a word with current keyframes
   // Discard keyframes connected to the query keyframe
@@ -234,12 +222,9 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
     for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(),
                                           vend = pKF->mBowVec.end();
          vit != vend; vit++) {
-      list<KeyFrame*>& lKFs = mvInvertedFile[vit->first];
-
-      for (list<KeyFrame*>::iterator lit = lKFs.begin(), lend = lKFs.end();
-           lit != lend; lit++) {
-        KeyFrame* pKFi = *lit;
+      list<std::shared_ptr<KeyFrame>>& lKFs = mvInvertedFile[vit->first];
 
+      for (auto pKFi : lKFs) {
         // For consider a loop candidate it a candidate it must be in the same
         // map
         if (pKFi->GetMap() == pKF->GetMap()) {
@@ -268,12 +253,12 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
   if (lKFsSharingWordsLoop.empty() && lKFsSharingWordsMerge.empty()) return;
 
   if (!lKFsSharingWordsLoop.empty()) {
-    list<pair<float, KeyFrame*> > lScoreAndMatch;
+    list<pair<float, std::shared_ptr<KeyFrame>>> lScoreAndMatch;
 
     // Only compare against those keyframes that share enough words
     int maxCommonWords = 0;
-    for (list<KeyFrame*>::iterator lit = lKFsSharingWordsLoop.begin(),
-                                   lend = lKFsSharingWordsLoop.end();
+    for (auto lit = lKFsSharingWordsLoop.begin(),
+              lend = lKFsSharingWordsLoop.end();
          lit != lend; lit++) {
       if ((*lit)->mnLoopWords > maxCommonWords)
         maxCommonWords = (*lit)->mnLoopWords;
@@ -285,10 +270,10 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
 
     // Compute similarity score. Retain the matches whose score is higher than
     // minScore
-    for (list<KeyFrame*>::iterator lit = lKFsSharingWordsLoop.begin(),
-                                   lend = lKFsSharingWordsLoop.end();
+    for (auto lit = lKFsSharingWordsLoop.begin(),
+              lend = lKFsSharingWordsLoop.end();
          lit != lend; lit++) {
-      KeyFrame* pKFi = *lit;
+      std::shared_ptr<KeyFrame> pKFi = *lit;
 
       if (pKFi->mnLoopWords > minCommonWords) {
         nscores++;
@@ -301,23 +286,25 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
     }
 
     if (!lScoreAndMatch.empty()) {
-      list<pair<float, KeyFrame*> > lAccScoreAndMatch;
+      list<pair<float, std::shared_ptr<KeyFrame>>> lAccScoreAndMatch;
       float bestAccScore = minScore;
 
       // Lets now accumulate score by covisibility
-      for (list<pair<float, KeyFrame*> >::iterator it = lScoreAndMatch.begin(),
-                                                   itend = lScoreAndMatch.end();
+      for (list<pair<float, std::shared_ptr<KeyFrame>>>::iterator
+               it = lScoreAndMatch.begin(),
+               itend = lScoreAndMatch.end();
            it != itend; it++) {
-        KeyFrame* pKFi = it->second;
-        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+        std::shared_ptr<KeyFrame> pKFi = it->second;
+        vector<std::shared_ptr<KeyFrame>> vpNeighs =
+            pKFi->GetBestCovisibilityKeyFrames(10);
 
         float bestScore = it->first;
         float accScore = it->first;
-        KeyFrame* pBestKF = pKFi;
-        for (vector<KeyFrame*>::iterator vit = vpNeighs.begin(),
-                                         vend = vpNeighs.end();
+        std::shared_ptr<KeyFrame> pBestKF = pKFi;
+        for (vector<std::shared_ptr<KeyFrame>>::iterator vit = vpNeighs.begin(),
+                                                         vend = vpNeighs.end();
              vit != vend; vit++) {
-          KeyFrame* pKF2 = *vit;
+          std::shared_ptr<KeyFrame> pKF2 = *vit;
           if (pKF2->mnLoopQuery == pKF->mnId &&
               pKF2->mnLoopWords > minCommonWords) {
             accScore += pKF2->mLoopScore;
@@ -335,15 +322,13 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
       // Return all those keyframes with a score higher than 0.75*bestScore
       float minScoreToRetain = 0.75f * bestAccScore;
 
-      set<KeyFrame*> spAlreadyAddedKF;
+      set<std::shared_ptr<KeyFrame>> spAlreadyAddedKF;
       vpLoopCand.reserve(lAccScoreAndMatch.size());
 
-      for (list<pair<float, KeyFrame*> >::iterator
-               it = lAccScoreAndMatch.begin(),
-               itend = lAccScoreAndMatch.end();
+      for (auto it = lAccScoreAndMatch.begin(), itend = lAccScoreAndMatch.end();
            it != itend; it++) {
         if (it->first > minScoreToRetain) {
-          KeyFrame* pKFi = it->second;
+          std::shared_ptr<KeyFrame> pKFi = it->second;
           if (!spAlreadyAddedKF.count(pKFi)) {
             vpLoopCand.push_back(pKFi);
             spAlreadyAddedKF.insert(pKFi);
@@ -354,12 +339,13 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
   }
 
   if (!lKFsSharingWordsMerge.empty()) {
-    list<pair<float, KeyFrame*> > lScoreAndMatch;
+    list<pair<float, std::shared_ptr<KeyFrame>>> lScoreAndMatch;
 
     // Only compare against those keyframes that share enough words
     int maxCommonWords = 0;
-    for (list<KeyFrame*>::iterator lit = lKFsSharingWordsMerge.begin(),
-                                   lend = lKFsSharingWordsMerge.end();
+    for (list<std::shared_ptr<KeyFrame>>::iterator
+             lit = lKFsSharingWordsMerge.begin(),
+             lend = lKFsSharingWordsMerge.end();
          lit != lend; lit++) {
       if ((*lit)->mnMergeWords > maxCommonWords)
         maxCommonWords = (*lit)->mnMergeWords;
@@ -371,10 +357,11 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
 
     // Compute similarity score. Retain the matches whose score is higher than
     // minScore
-    for (list<KeyFrame*>::iterator lit = lKFsSharingWordsMerge.begin(),
-                                   lend = lKFsSharingWordsMerge.end();
+    for (list<std::shared_ptr<KeyFrame>>::iterator
+             lit = lKFsSharingWordsMerge.begin(),
+             lend = lKFsSharingWordsMerge.end();
          lit != lend; lit++) {
-      KeyFrame* pKFi = *lit;
+      std::shared_ptr<KeyFrame> pKFi = *lit;
 
       if (pKFi->mnMergeWords > minCommonWords) {
         nscores++;
@@ -387,23 +374,23 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
     }
 
     if (!lScoreAndMatch.empty()) {
-      list<pair<float, KeyFrame*> > lAccScoreAndMatch;
+      list<pair<float, std::shared_ptr<KeyFrame>>> lAccScoreAndMatch;
       float bestAccScore = minScore;
 
       // Lets now accumulate score by covisibility
-      for (list<pair<float, KeyFrame*> >::iterator it = lScoreAndMatch.begin(),
-                                                   itend = lScoreAndMatch.end();
+      for (auto it = lScoreAndMatch.begin(), itend = lScoreAndMatch.end();
            it != itend; it++) {
-        KeyFrame* pKFi = it->second;
-        vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+        std::shared_ptr<KeyFrame> pKFi = it->second;
+        vector<std::shared_ptr<KeyFrame>> vpNeighs =
+            pKFi->GetBestCovisibilityKeyFrames(10);
 
         float bestScore = it->first;
         float accScore = it->first;
-        KeyFrame* pBestKF = pKFi;
-        for (vector<KeyFrame*>::iterator vit = vpNeighs.begin(),
-                                         vend = vpNeighs.end();
+        std::shared_ptr<KeyFrame> pBestKF = pKFi;
+        for (vector<std::shared_ptr<KeyFrame>>::iterator vit = vpNeighs.begin(),
+                                                         vend = vpNeighs.end();
              vit != vend; vit++) {
-          KeyFrame* pKF2 = *vit;
+          std::shared_ptr<KeyFrame> pKF2 = *vit;
           if (pKF2->mnMergeQuery == pKF->mnId &&
               pKF2->mnMergeWords > minCommonWords) {
             accScore += pKF2->mMergeScore;
@@ -421,15 +408,15 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
       // Return all those keyframes with a score higher than 0.75*bestScore
       float minScoreToRetain = 0.75f * bestAccScore;
 
-      set<KeyFrame*> spAlreadyAddedKF;
+      set<std::shared_ptr<KeyFrame>> spAlreadyAddedKF;
       vpMergeCand.reserve(lAccScoreAndMatch.size());
 
-      for (list<pair<float, KeyFrame*> >::iterator
+      for (list<pair<float, std::shared_ptr<KeyFrame>>>::iterator
                it = lAccScoreAndMatch.begin(),
                itend = lAccScoreAndMatch.end();
            it != itend; it++) {
         if (it->first > minScoreToRetain) {
-          KeyFrame* pKFi = it->second;
+          std::shared_ptr<KeyFrame> pKFi = it->second;
           if (!spAlreadyAddedKF.count(pKFi)) {
             vpMergeCand.push_back(pKFi);
             spAlreadyAddedKF.insert(pKFi);
@@ -442,23 +429,21 @@ void KeyFrameDatabase::DetectCandidates(KeyFrame* pKF, float minScore,
   for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(),
                                         vend = pKF->mBowVec.end();
        vit != vend; vit++) {
-    list<KeyFrame*>& lKFs = mvInvertedFile[vit->first];
+    list<std::shared_ptr<KeyFrame>>& lKFs = mvInvertedFile[vit->first];
 
-    for (list<KeyFrame*>::iterator lit = lKFs.begin(), lend = lKFs.end();
-         lit != lend; lit++) {
-      KeyFrame* pKFi = *lit;
+    for (auto pKFi : lKFs) {
       pKFi->mnLoopQuery = -1;
       pKFi->mnMergeQuery = -1;
     }
   }
 }
 
-void KeyFrameDatabase::DetectBestCandidates(KeyFrame* pKF,
-                                            vector<KeyFrame*>& vpLoopCand,
-                                            vector<KeyFrame*>& vpMergeCand,
-                                            int nMinWords) {
-  list<KeyFrame*> lKFsSharingWords;
-  set<KeyFrame*> spConnectedKF;
+void KeyFrameDatabase::DetectBestCandidates(
+    const std::shared_ptr<KeyFrame>& pKF,
+    vector<std::shared_ptr<KeyFrame>>& vpLoopCand,
+    vector<std::shared_ptr<KeyFrame>>& vpMergeCand, int nMinWords) {
+  list<std::shared_ptr<KeyFrame>> lKFsSharingWords;
+  set<std::shared_ptr<KeyFrame>> spConnectedKF;
 
   // Search all keyframes that share a word with current frame
   {
@@ -469,11 +454,12 @@ void KeyFrameDatabase::DetectBestCandidates(KeyFrame* pKF,
     for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(),
                                           vend = pKF->mBowVec.end();
          vit != vend; vit++) {
-      list<KeyFrame*>& lKFs = mvInvertedFile[vit->first];
+      list<std::shared_ptr<KeyFrame>>& lKFs = mvInvertedFile[vit->first];
 
-      for (list<KeyFrame*>::iterator lit = lKFs.begin(), lend = lKFs.end();
+      for (list<std::shared_ptr<KeyFrame>>::iterator lit = lKFs.begin(),
+                                                     lend = lKFs.end();
            lit != lend; lit++) {
-        KeyFrame* pKFi = *lit;
+        std::shared_ptr<KeyFrame> pKFi = *lit;
         if (spConnectedKF.find(pKFi) != spConnectedKF.end()) {
           continue;
         }
@@ -490,8 +476,8 @@ void KeyFrameDatabase::DetectBestCandidates(KeyFrame* pKF,
 
   // Only compare against those keyframes that share enough words
   int maxCommonWords = 0;
-  for (list<KeyFrame*>::iterator lit = lKFsSharingWords.begin(),
-                                 lend = lKFsSharingWords.end();
+  for (list<std::shared_ptr<KeyFrame>>::iterator lit = lKFsSharingWords.begin(),
+                                                 lend = lKFsSharingWords.end();
        lit != lend; lit++) {
     if ((*lit)->mnPlaceRecognitionWords > maxCommonWords)
       maxCommonWords = (*lit)->mnPlaceRecognitionWords;
@@ -503,15 +489,15 @@ void KeyFrameDatabase::DetectBestCandidates(KeyFrame* pKF,
     minCommonWords = nMinWords;
   }
 
-  list<pair<float, KeyFrame*> > lScoreAndMatch;
+  list<pair<float, std::shared_ptr<KeyFrame>>> lScoreAndMatch;
 
   int nscores = 0;
 
   // Compute similarity score.
-  for (list<KeyFrame*>::iterator lit = lKFsSharingWords.begin(),
-                                 lend = lKFsSharingWords.end();
+  for (list<std::shared_ptr<KeyFrame>>::iterator lit = lKFsSharingWords.begin(),
+                                                 lend = lKFsSharingWords.end();
        lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
+    std::shared_ptr<KeyFrame> pKFi = *lit;
 
     if (pKFi->mnPlaceRecognitionWords > minCommonWords) {
       nscores++;
@@ -523,23 +509,24 @@ void KeyFrameDatabase::DetectBestCandidates(KeyFrame* pKF,
 
   if (lScoreAndMatch.empty()) return;
 
-  list<pair<float, KeyFrame*> > lAccScoreAndMatch;
+  list<pair<float, std::shared_ptr<KeyFrame>>> lAccScoreAndMatch;
   float bestAccScore = 0;
 
   // Lets now accumulate score by covisibility
-  for (list<pair<float, KeyFrame*> >::iterator it = lScoreAndMatch.begin(),
-                                               itend = lScoreAndMatch.end();
+  for (list<pair<float, std::shared_ptr<KeyFrame>>>::iterator
+           it = lScoreAndMatch.begin(),
+           itend = lScoreAndMatch.end();
        it != itend; it++) {
-    KeyFrame* pKFi = it->second;
-    vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+    std::shared_ptr<KeyFrame> pKFi = it->second;
+    auto vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
 
     float bestScore = it->first;
     float accScore = bestScore;
-    KeyFrame* pBestKF = pKFi;
-    for (vector<KeyFrame*>::iterator vit = vpNeighs.begin(),
-                                     vend = vpNeighs.end();
+    auto pBestKF = pKFi;
+    for (vector<std::shared_ptr<KeyFrame>>::iterator vit = vpNeighs.begin(),
+                                                     vend = vpNeighs.end();
          vit != vend; vit++) {
-      KeyFrame* pKF2 = *vit;
+      auto pKF2 = *vit;
       if (pKF2->mnPlaceRecognitionQuery != pKF->mnId) continue;
 
       accScore += pKF2->mPlaceRecognitionScore;
@@ -554,15 +541,16 @@ void KeyFrameDatabase::DetectBestCandidates(KeyFrame* pKF,
 
   // Return all those keyframes with a score higher than 0.75*bestScore
   float minScoreToRetain = 0.75f * bestAccScore;
-  set<KeyFrame*> spAlreadyAddedKF;
+  set<std::shared_ptr<KeyFrame>> spAlreadyAddedKF;
   vpLoopCand.reserve(lAccScoreAndMatch.size());
   vpMergeCand.reserve(lAccScoreAndMatch.size());
-  for (list<pair<float, KeyFrame*> >::iterator it = lAccScoreAndMatch.begin(),
-                                               itend = lAccScoreAndMatch.end();
+  for (list<pair<float, std::shared_ptr<KeyFrame>>>::iterator
+           it = lAccScoreAndMatch.begin(),
+           itend = lAccScoreAndMatch.end();
        it != itend; it++) {
     const float& si = it->first;
     if (si > minScoreToRetain) {
-      KeyFrame* pKFi = it->second;
+      std::shared_ptr<KeyFrame> pKFi = it->second;
       if (!spAlreadyAddedKF.count(pKFi)) {
         if (pKF->GetMap() == pKFi->GetMap()) {
           vpLoopCand.push_back(pKFi);
@@ -575,17 +563,17 @@ void KeyFrameDatabase::DetectBestCandidates(KeyFrame* pKF,
   }
 }
 
-bool compFirst(const pair<float, KeyFrame*>& a,
-               const pair<float, KeyFrame*>& b) {
+bool compFirst(const pair<float, std::shared_ptr<KeyFrame>>& a,
+               const pair<float, std::shared_ptr<KeyFrame>>& b) {
   return a.first > b.first;
 }
 
-void KeyFrameDatabase::DetectNBestCandidates(KeyFrame* pKF,
-                                             vector<KeyFrame*>& vpLoopCand,
-                                             vector<KeyFrame*>& vpMergeCand,
-                                             int nNumCandidates) {
-  list<KeyFrame*> lKFsSharingWords;
-  set<KeyFrame*> spConnectedKF;
+void KeyFrameDatabase::DetectNBestCandidates(
+    const std::shared_ptr<KeyFrame>& pKF,
+    vector<std::shared_ptr<KeyFrame>>& vpLoopCand,
+    vector<std::shared_ptr<KeyFrame>>& vpMergeCand, int nNumCandidates) {
+  list<std::shared_ptr<KeyFrame>> lKFsSharingWords;
+  set<std::shared_ptr<KeyFrame>> spConnectedKF;
 
   // Search all keyframes that share a word with current frame
   {
@@ -596,11 +584,12 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame* pKF,
     for (DBoW2::BowVector::const_iterator vit = pKF->mBowVec.begin(),
                                           vend = pKF->mBowVec.end();
          vit != vend; vit++) {
-      list<KeyFrame*>& lKFs = mvInvertedFile[vit->first];
+      auto& lKFs = mvInvertedFile[vit->first];
 
-      for (list<KeyFrame*>::iterator lit = lKFs.begin(), lend = lKFs.end();
+      for (list<std::shared_ptr<KeyFrame>>::iterator lit = lKFs.begin(),
+                                                     lend = lKFs.end();
            lit != lend; lit++) {
-        KeyFrame* pKFi = *lit;
+        std::shared_ptr<KeyFrame> pKFi = *lit;
 
         if (pKFi->mnPlaceRecognitionQuery != pKF->mnId) {
           pKFi->mnPlaceRecognitionWords = 0;
@@ -617,24 +606,22 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame* pKF,
 
   // Only compare against those keyframes that share enough words
   int maxCommonWords = 0;
-  for (list<KeyFrame*>::iterator lit = lKFsSharingWords.begin(),
-                                 lend = lKFsSharingWords.end();
-       lit != lend; lit++) {
-    if ((*lit)->mnPlaceRecognitionWords > maxCommonWords)
-      maxCommonWords = (*lit)->mnPlaceRecognitionWords;
+  for (auto pKFi : lKFsSharingWords) {
+    if (pKFi->mnPlaceRecognitionWords > maxCommonWords)
+      maxCommonWords = pKFi->mnPlaceRecognitionWords;
   }
 
   int minCommonWords = maxCommonWords * 0.8f;
 
-  list<pair<float, KeyFrame*> > lScoreAndMatch;
+  list<pair<float, std::shared_ptr<KeyFrame>>> lScoreAndMatch;
 
   int nscores = 0;
 
   // Compute similarity score.
-  for (list<KeyFrame*>::iterator lit = lKFsSharingWords.begin(),
-                                 lend = lKFsSharingWords.end();
+  for (list<std::shared_ptr<KeyFrame>>::iterator lit = lKFsSharingWords.begin(),
+                                                 lend = lKFsSharingWords.end();
        lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
+    std::shared_ptr<KeyFrame> pKFi = *lit;
 
     if (pKFi->mnPlaceRecognitionWords > minCommonWords) {
       nscores++;
@@ -646,23 +633,21 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame* pKF,
 
   if (lScoreAndMatch.empty()) return;
 
-  list<pair<float, KeyFrame*> > lAccScoreAndMatch;
+  list<pair<float, std::shared_ptr<KeyFrame>>> lAccScoreAndMatch;
   float bestAccScore = 0;
 
   // Lets now accumulate score by covisibility
-  for (list<pair<float, KeyFrame*> >::iterator it = lScoreAndMatch.begin(),
-                                               itend = lScoreAndMatch.end();
+  for (list<pair<float, std::shared_ptr<KeyFrame>>>::iterator
+           it = lScoreAndMatch.begin(),
+           itend = lScoreAndMatch.end();
        it != itend; it++) {
-    KeyFrame* pKFi = it->second;
-    vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+    std::shared_ptr<KeyFrame> pKFi = it->second;
+    auto vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
 
     float bestScore = it->first;
     float accScore = bestScore;
-    KeyFrame* pBestKF = pKFi;
-    for (vector<KeyFrame*>::iterator vit = vpNeighs.begin(),
-                                     vend = vpNeighs.end();
-         vit != vend; vit++) {
-      KeyFrame* pKF2 = *vit;
+    std::shared_ptr<KeyFrame> pBestKF = pKFi;
+    for (auto pKF2 : vpNeighs) {
       if (pKF2->mnPlaceRecognitionQuery != pKF->mnId) continue;
 
       accScore += pKF2->mPlaceRecognitionScore;
@@ -679,13 +664,14 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame* pKF,
 
   vpLoopCand.reserve(nNumCandidates);
   vpMergeCand.reserve(nNumCandidates);
-  set<KeyFrame*> spAlreadyAddedKF;
+  set<std::shared_ptr<KeyFrame>> spAlreadyAddedKF;
   int i = 0;
-  list<pair<float, KeyFrame*> >::iterator it = lAccScoreAndMatch.begin();
+  list<pair<float, std::shared_ptr<KeyFrame>>>::iterator it =
+      lAccScoreAndMatch.begin();
   while (i < lAccScoreAndMatch.size() &&
          (vpLoopCand.size() < nNumCandidates ||
           vpMergeCand.size() < nNumCandidates)) {
-    KeyFrame* pKFi = it->second;
+    std::shared_ptr<KeyFrame> pKFi = it->second;
     if (pKFi->isBad()) continue;
 
     if (!spAlreadyAddedKF.count(pKFi)) {
@@ -704,9 +690,10 @@ void KeyFrameDatabase::DetectNBestCandidates(KeyFrame* pKF,
   }
 }
 
-vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(
-    Frame* F, const std::shared_ptr<Map>& pMap) {
-  list<KeyFrame*> lKFsSharingWords;
+vector<std::shared_ptr<KeyFrame>>
+KeyFrameDatabase::DetectRelocalizationCandidates(
+    const std::shared_ptr<Frame>& F, const std::shared_ptr<Map>& pMap) {
+  list<std::shared_ptr<KeyFrame>> lKFsSharingWords;
 
   // Search all keyframes that share a word with current frame
   {
@@ -715,11 +702,12 @@ vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(
     for (DBoW2::BowVector::const_iterator vit = F->mBowVec.begin(),
                                           vend = F->mBowVec.end();
          vit != vend; vit++) {
-      list<KeyFrame*>& lKFs = mvInvertedFile[vit->first];
+      list<std::shared_ptr<KeyFrame>>& lKFs = mvInvertedFile[vit->first];
 
-      for (list<KeyFrame*>::iterator lit = lKFs.begin(), lend = lKFs.end();
+      for (list<std::shared_ptr<KeyFrame>>::iterator lit = lKFs.begin(),
+                                                     lend = lKFs.end();
            lit != lend; lit++) {
-        KeyFrame* pKFi = *lit;
+        std::shared_ptr<KeyFrame> pKFi = *lit;
         if (pKFi->mnRelocQuery != F->mnId) {
           pKFi->mnRelocWords = 0;
           pKFi->mnRelocQuery = F->mnId;
@@ -729,12 +717,12 @@ vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(
       }
     }
   }
-  if (lKFsSharingWords.empty()) return vector<KeyFrame*>();
+  if (lKFsSharingWords.empty()) return vector<std::shared_ptr<KeyFrame>>();
 
   // Only compare against those keyframes that share enough words
   int maxCommonWords = 0;
-  for (list<KeyFrame*>::iterator lit = lKFsSharingWords.begin(),
-                                 lend = lKFsSharingWords.end();
+  for (list<std::shared_ptr<KeyFrame>>::iterator lit = lKFsSharingWords.begin(),
+                                                 lend = lKFsSharingWords.end();
        lit != lend; lit++) {
     if ((*lit)->mnRelocWords > maxCommonWords)
       maxCommonWords = (*lit)->mnRelocWords;
@@ -742,16 +730,12 @@ vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(
 
   int minCommonWords = maxCommonWords * 0.8f;
 
-  list<pair<float, KeyFrame*> > lScoreAndMatch;
+  list<pair<float, std::shared_ptr<KeyFrame>>> lScoreAndMatch;
 
   int nscores = 0;
 
   // Compute similarity score.
-  for (list<KeyFrame*>::iterator lit = lKFsSharingWords.begin(),
-                                 lend = lKFsSharingWords.end();
-       lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
-
+  for (auto pKFi : lKFsSharingWords) {
     if (pKFi->mnRelocWords > minCommonWords) {
       nscores++;
       float si = mpVoc->score(F->mBowVec, pKFi->mBowVec);
@@ -760,25 +744,24 @@ vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(
     }
   }
 
-  if (lScoreAndMatch.empty()) return vector<KeyFrame*>();
+  if (lScoreAndMatch.empty()) return vector<std::shared_ptr<KeyFrame>>();
 
-  list<pair<float, KeyFrame*> > lAccScoreAndMatch;
+  list<pair<float, std::shared_ptr<KeyFrame>>> lAccScoreAndMatch;
   float bestAccScore = 0;
 
   // Lets now accumulate score by covisibility
-  for (list<pair<float, KeyFrame*> >::iterator it = lScoreAndMatch.begin(),
-                                               itend = lScoreAndMatch.end();
+  for (list<pair<float, std::shared_ptr<KeyFrame>>>::iterator
+           it = lScoreAndMatch.begin(),
+           itend = lScoreAndMatch.end();
        it != itend; it++) {
-    KeyFrame* pKFi = it->second;
-    vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+    std::shared_ptr<KeyFrame> pKFi = it->second;
+    vector<std::shared_ptr<KeyFrame>> vpNeighs =
+        pKFi->GetBestCovisibilityKeyFrames(10);
 
     float bestScore = it->first;
     float accScore = bestScore;
-    KeyFrame* pBestKF = pKFi;
-    for (vector<KeyFrame*>::iterator vit = vpNeighs.begin(),
-                                     vend = vpNeighs.end();
-         vit != vend; vit++) {
-      KeyFrame* pKF2 = *vit;
+    std::shared_ptr<KeyFrame> pBestKF = pKFi;
+    for (auto pKF2 : vpNeighs) {
       if (pKF2->mnRelocQuery != F->mnId) continue;
 
       accScore += pKF2->mRelocScore;
@@ -793,15 +776,16 @@ vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(
 
   // Return all those keyframes with a score higher than 0.75*bestScore
   float minScoreToRetain = 0.75f * bestAccScore;
-  set<KeyFrame*> spAlreadyAddedKF;
-  vector<KeyFrame*> vpRelocCandidates;
+  set<std::shared_ptr<KeyFrame>> spAlreadyAddedKF;
+  vector<std::shared_ptr<KeyFrame>> vpRelocCandidates;
   vpRelocCandidates.reserve(lAccScoreAndMatch.size());
-  for (list<pair<float, KeyFrame*> >::iterator it = lAccScoreAndMatch.begin(),
-                                               itend = lAccScoreAndMatch.end();
+  for (list<pair<float, std::shared_ptr<KeyFrame>>>::iterator
+           it = lAccScoreAndMatch.begin(),
+           itend = lAccScoreAndMatch.end();
        it != itend; it++) {
     const float& si = it->first;
     if (si > minScoreToRetain) {
-      KeyFrame* pKFi = it->second;
+      std::shared_ptr<KeyFrame> pKFi = it->second;
       if (pKFi->GetMap() != pMap) continue;
       if (!spAlreadyAddedKF.count(pKFi)) {
         vpRelocCandidates.push_back(pKFi);
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index d9cf4d58024..e8457474974 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -41,7 +41,8 @@
 
 namespace ORB_SLAM3 {
 
-LocalMapping::LocalMapping(System *pSys, const std::shared_ptr<Atlas> &pAtlas,
+LocalMapping::LocalMapping(const std::shared_ptr<System> &pSys,
+                           const std::shared_ptr<Atlas> &pAtlas,
                            const float bMonocular, bool bInertial,
                            const string &_strSeqName)
     : mpSystem(pSys),
@@ -325,7 +326,7 @@ void LocalMapping::Run() {
   SetFinish();
 }
 
-void LocalMapping::InsertKeyFrame(KeyFrame *pKF) {
+void LocalMapping::InsertKeyFrame(const std::shared_ptr<KeyFrame> &pKF) {
   unique_lock<mutex> lock(mMutexNewKFs);
   mlNewKeyFrames.push_back(pKF);
   mbAbortBA = true;
@@ -420,15 +421,13 @@ void LocalMapping::CreateNewMapPoints() {
   int nn = 10;
   // For stereo inertial case
   if (mbMonocular) nn = 30;
-  vector<KeyFrame *> vpNeighKFs =
-      mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
+  auto vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
 
   if (mbInertial) {
-    KeyFrame *pKF = mpCurrentKeyFrame;
+    auto pKF = mpCurrentKeyFrame;
     int count = 0;
     while ((vpNeighKFs.size() <= nn) && (pKF->mPrevKF) && (count++ < nn)) {
-      vector<KeyFrame *>::iterator it =
-          std::find(vpNeighKFs.begin(), vpNeighKFs.end(), pKF->mPrevKF);
+      auto it = std::find(vpNeighKFs.begin(), vpNeighKFs.end(), pKF->mPrevKF);
       if (it == vpNeighKFs.end()) vpNeighKFs.push_back(pKF->mPrevKF);
       pKF = pKF->mPrevKF;
     }
@@ -461,7 +460,7 @@ void LocalMapping::CreateNewMapPoints() {
   for (size_t i = 0; i < vpNeighKFs.size(); i++) {
     if (i > 0 && CheckNewKeyFrames()) return;
 
-    KeyFrame *pKF2 = vpNeighKFs[i];
+    auto pKF2 = vpNeighKFs[i];
 
     std::shared_ptr<GeometricCamera> pCamera1 = mpCurrentKeyFrame->mpCamera,
                                      pCamera2 = pKF2->mpCamera;
@@ -726,28 +725,29 @@ void LocalMapping::SearchInNeighbors() {
   // Retrieve neighbor keyframes
   int nn = 10;
   if (mbMonocular) nn = 30;
-  const vector<KeyFrame *> vpNeighKFs =
-      mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
-  vector<KeyFrame *> vpTargetKFs;
-  for (vector<KeyFrame *>::const_iterator vit = vpNeighKFs.begin(),
-                                          vend = vpNeighKFs.end();
-       vit != vend; vit++) {
-    KeyFrame *pKFi = *vit;
+  const auto vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
+
+  list<std::shared_ptr<KeyFrame>> vpTargetKFs;
+
+  for (auto const &pKFi : vpNeighKFs) {
     if (pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)
       continue;
+
     vpTargetKFs.push_back(pKFi);
     pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;
   }
 
   // Add some covisible of covisible
   // Extend to some second neighbors if abort is not requested
-  for (int i = 0, imax = vpTargetKFs.size(); i < imax; i++) {
-    const vector<KeyFrame *> vpSecondNeighKFs =
-        vpTargetKFs[i]->GetBestCovisibilityKeyFrames(20);
-    for (vector<KeyFrame *>::const_iterator vit2 = vpSecondNeighKFs.begin(),
-                                            vend2 = vpSecondNeighKFs.end();
-         vit2 != vend2; vit2++) {
-      KeyFrame *pKFi2 = *vit2;
+  //
+  // Note this code extends the list while iterating through it...
+  // Ensure we are only going over the initial set
+  list<std::shared_ptr<KeyFrame>> vpInitialTargetKFs(vpTargetKFs.begin(),
+                                                     vpTargetKFs.end());
+  for (auto const &pTargetKF : vpInitialTargetKFs) {
+    const auto vpSecondNeighKFs = pTargetKF->GetBestCovisibilityKeyFrames(20);
+
+    for (auto const &pKFi2 : vpSecondNeighKFs) {
       if (pKFi2->isBad() ||
           pKFi2->mnFuseTargetForKF == mpCurrentKeyFrame->mnId ||
           pKFi2->mnId == mpCurrentKeyFrame->mnId)
@@ -755,12 +755,13 @@ void LocalMapping::SearchInNeighbors() {
       vpTargetKFs.push_back(pKFi2);
       pKFi2->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;
     }
+
     if (mbAbortBA) break;
   }
 
   // Extend to temporal neighbors
   if (mbInertial) {
-    KeyFrame *pKFi = mpCurrentKeyFrame->mPrevKF;
+    auto pKFi = mpCurrentKeyFrame->mPrevKF;
     while (vpTargetKFs.size() < 20 && pKFi) {
       if (pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId) {
         pKFi = pKFi->mPrevKF;
@@ -776,11 +777,8 @@ void LocalMapping::SearchInNeighbors() {
   ORBmatcher matcher;
   vector<MapPoint *> vpMapPointMatches =
       mpCurrentKeyFrame->GetMapPointMatches();
-  for (vector<KeyFrame *>::iterator vit = vpTargetKFs.begin(),
-                                    vend = vpTargetKFs.end();
-       vit != vend; vit++) {
-    KeyFrame *pKFi = *vit;
 
+  for (auto pKFi : vpTargetKFs) {
     matcher.Fuse(pKFi, vpMapPointMatches);
     if (pKFi->NLeft != -1) matcher.Fuse(pKFi, vpMapPointMatches, true);
   }
@@ -791,17 +789,9 @@ void LocalMapping::SearchInNeighbors() {
   vector<MapPoint *> vpFuseCandidates;
   vpFuseCandidates.reserve(vpTargetKFs.size() * vpMapPointMatches.size());
 
-  for (vector<KeyFrame *>::iterator vitKF = vpTargetKFs.begin(),
-                                    vendKF = vpTargetKFs.end();
-       vitKF != vendKF; vitKF++) {
-    KeyFrame *pKFi = *vitKF;
-
-    vector<MapPoint *> vpMapPointsKFi = pKFi->GetMapPointMatches();
-
-    for (vector<MapPoint *>::iterator vitMP = vpMapPointsKFi.begin(),
-                                      vendMP = vpMapPointsKFi.end();
-         vitMP != vendMP; vitMP++) {
-      MapPoint *pMP = *vitMP;
+  for (auto pKFi : vpTargetKFs) {
+    auto vpMapPointsKFi = pKFi->GetMapPointMatches();
+    for (auto pMP : vpMapPointsKFi) {
       if (!pMP) continue;
       if (pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
         continue;
@@ -816,8 +806,7 @@ void LocalMapping::SearchInNeighbors() {
 
   // Update points
   vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
-  for (size_t i = 0, iend = vpMapPointMatches.size(); i < iend; i++) {
-    MapPoint *pMP = vpMapPointMatches[i];
+  for (auto pMP : vpMapPointMatches) {
     if (pMP) {
       if (!pMP->isBad()) {
         pMP->ComputeDistinctiveDescriptors();
@@ -864,10 +853,12 @@ void LocalMapping::Release() {
   if (mbFinished) return;
   mbStopped = false;
   mbStopRequested = false;
-  for (list<KeyFrame *>::iterator lit = mlNewKeyFrames.begin(),
-                                  lend = mlNewKeyFrames.end();
-       lit != lend; lit++)
-    delete *lit;
+
+  // for (list<KeyFrame *>::iterator lit = mlNewKeyFrames.begin(),
+  //                                 lend = mlNewKeyFrames.end();
+  //      lit != lend; lit++)
+  //   delete *lit;
+
   mlNewKeyFrames.clear();
 
   cout << "Local Mapping RELEASE" << endl;
@@ -902,8 +893,7 @@ void LocalMapping::KeyFrameCulling() {
   // consider close stereo points
   const int Nd = 21;
   mpCurrentKeyFrame->UpdateBestCovisibles();
-  vector<KeyFrame *> vpLocalKeyFrames =
-      mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
+  auto vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
 
   float redundant_th;
   if (!mbInertial)
@@ -920,7 +910,7 @@ void LocalMapping::KeyFrameCulling() {
   unsigned int last_ID;
   if (mbInertial) {
     int count = 0;
-    KeyFrame *aux_KF = mpCurrentKeyFrame;
+    auto aux_KF = mpCurrentKeyFrame;
     while (count < Nd && aux_KF->mPrevKF) {
       aux_KF = aux_KF->mPrevKF;
       count++;
@@ -928,11 +918,8 @@ void LocalMapping::KeyFrameCulling() {
     last_ID = aux_KF->mnId;
   }
 
-  for (vector<KeyFrame *>::iterator vit = vpLocalKeyFrames.begin(),
-                                    vend = vpLocalKeyFrames.end();
-       vit != vend; vit++) {
+  for (auto pKF : vpLocalKeyFrames) {
     count++;
-    KeyFrame *pKF = *vit;
 
     if ((pKF->mnId == pKF->GetMap()->GetInitKFid()) || pKF->isBad()) continue;
     const vector<MapPoint *> vpMapPoints = pKF->GetMapPointMatches();
@@ -956,16 +943,14 @@ void LocalMapping::KeyFrameCulling() {
                                     : (i < pKF->NLeft)
                                         ? pKF->mvKeys[i].octave
                                         : pKF->mvKeysRight[i].octave;
-            const map<KeyFrame *, tuple<int, int>> observations =
-                pMP->GetObservations();
+            const auto observations = pMP->GetObservations();
             int nObs = 0;
-            for (map<KeyFrame *, tuple<int, int>>::const_iterator
-                     mit = observations.begin(),
-                     mend = observations.end();
-                 mit != mend; mit++) {
-              KeyFrame *pKFi = mit->first;
+
+            for (auto const &mit : observations) {
+              std::shared_ptr<KeyFrame> pKFi = mit.first;
               if (pKFi == pKF) continue;
-              tuple<int, int> indexes = mit->second;
+
+              tuple<int, int> indexes = mit.second;
               int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
               int scaleLeveli = -1;
               if (pKFi->NLeft == -1) {
@@ -988,9 +973,8 @@ void LocalMapping::KeyFrameCulling() {
                 if (nObs > thObs) break;
               }
             }
-            if (nObs > thObs) {
-              nRedundantObservations++;
-            }
+
+            if (nObs > thObs) nRedundantObservations++;
           }
         }
       }
@@ -1155,14 +1139,14 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
   if (mpAtlas->KeyFramesInMap() < nMinKF) return;
 
   // Retrieve all keyframe in temporal order
-  list<KeyFrame *> lpKF;
-  KeyFrame *pKF = mpCurrentKeyFrame;
+  list<std::shared_ptr<KeyFrame>> lpKF;
+  auto pKF = mpCurrentKeyFrame;
   while (pKF->mPrevKF) {
     lpKF.push_front(pKF);
     pKF = pKF->mPrevKF;
   }
   lpKF.push_front(pKF);
-  vector<KeyFrame *> vpKF(lpKF.begin(), lpKF.end());
+  vector<std::shared_ptr<KeyFrame>> vpKF(lpKF.begin(), lpKF.end());
 
   if (vpKF.size() < nMinKF) return;
 
@@ -1185,18 +1169,18 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
     Eigen::Matrix3f Rwg;
     Eigen::Vector3f dirG;
     dirG.setZero();
-    for (vector<KeyFrame *>::iterator itKF = vpKF.begin(); itKF != vpKF.end();
-         itKF++) {
-      if (!(*itKF)->mpImuPreintegrated) continue;
-      if (!(*itKF)->mPrevKF) continue;
 
-      dirG -= (*itKF)->mPrevKF->GetImuRotation() *
-              (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity();
+    for (auto pKFi : vpKF) {
+      if (!pKFi->mpImuPreintegrated) continue;
+      if (!pKFi->mPrevKF) continue;
+
+      dirG -= pKFi->mPrevKF->GetImuRotation() *
+              pKFi->mpImuPreintegrated->GetUpdatedDeltaVelocity();
       Eigen::Vector3f _vel =
-          ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition()) /
-          (*itKF)->mpImuPreintegrated->dT;
-      (*itKF)->SetVelocity(_vel);
-      (*itKF)->mPrevKF->SetVelocity(_vel);
+          (pKFi->GetImuPosition() - pKFi->mPrevKF->GetImuPosition()) /
+          pKFi->mpImuPreintegrated->dT;
+      pKFi->SetVelocity(_vel);
+      pKFi->mPrevKF->SetVelocity(_vel);
     }
 
     dirG = dirG / dirG.norm();
@@ -1217,7 +1201,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
 
   mScale = 1.0;
 
-  mInitTime = mpTracker->mLastFrame.mTimeStamp - vpKF.front()->mTimeStamp;
+  mInitTime = mpTracker->mLastFrame->mTimeStamp - vpKF.front()->mTimeStamp;
 
   std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
   Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg,
@@ -1245,7 +1229,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
     // Check if initialization OK
     if (!mpAtlas->isImuInitialized()) {
       for (int i = 0; i < N; i++) {
-        KeyFrame *pKF2 = vpKF[i];
+        std::shared_ptr<KeyFrame> pKF2(vpKF[i]);
         pKF2->bImu = true;
       }
     }
@@ -1254,7 +1238,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
   mpTracker->UpdateFrameIMU(1.0, vpKF[0]->GetImuBias(), mpCurrentKeyFrame);
   if (!mpAtlas->isImuInitialized()) {
     mpAtlas->SetImuInitialized();
-    mpTracker->t0IMU = mpTracker->mCurrentFrame.mTimeStamp;
+    mpTracker->t0IMU = mpTracker->mCurrentFrame->mTimeStamp;
     mpCurrentKeyFrame->bImu = true;
   }
 
@@ -1287,17 +1271,16 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
   }
 
   // Correct keyframes starting at map first keyframe
-  list<KeyFrame *> lpKFtoCheck(
+  list<std::shared_ptr<KeyFrame>> lpKFtoCheck(
       mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.begin(),
       mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.end());
 
   while (!lpKFtoCheck.empty()) {
-    KeyFrame *pKF = lpKFtoCheck.front();
-    const set<KeyFrame *> sChilds = pKF->GetChilds();
+    auto pKF = lpKFtoCheck.front();
+    const auto sChilds = pKF->GetChilds();
     Sophus::SE3f Twc = pKF->GetPoseInverse();
-    for (set<KeyFrame *>::const_iterator sit = sChilds.begin();
-         sit != sChilds.end(); sit++) {
-      KeyFrame *pChild = *sit;
+
+    for (auto pChild : sChilds) {
       if (!pChild || pChild->isBad()) continue;
 
       if (pChild->mnBAGlobalForKF != GBAid) {
@@ -1346,7 +1329,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
       pMP->SetWorldPos(pMP->mPosGBA);
     } else {
       // Update according to the correction of its reference keyframe
-      KeyFrame *pRefKF = pMP->GetReferenceKeyFrame();
+      auto pRefKF = pMP->GetReferenceKeyFrame();
 
       if (pRefKF->mnBAGlobalForKF != GBAid) continue;
 
@@ -1363,11 +1346,9 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
   mnKFs = vpKF.size();
   mIdxInit++;
 
-  for (list<KeyFrame *>::iterator lit = mlNewKeyFrames.begin(),
-                                  lend = mlNewKeyFrames.end();
-       lit != lend; lit++) {
-    (*lit)->SetBadFlag();
-    delete *lit;
+  for (auto pKFi : mlNewKeyFrames) {
+    pKFi->SetBadFlag();
+    // delete *lit;
   }
   mlNewKeyFrames.clear();
 
@@ -1387,14 +1368,14 @@ void LocalMapping::ScaleRefinement() {
   if (mbResetRequested) return;
 
   // Retrieve all keyframes in temporal order
-  list<KeyFrame *> lpKF;
-  KeyFrame *pKF = mpCurrentKeyFrame;
+  list<std::shared_ptr<KeyFrame>> lpKF;
+  auto pKF = mpCurrentKeyFrame;
   while (pKF->mPrevKF) {
     lpKF.push_front(pKF);
     pKF = pKF->mPrevKF;
   }
   lpKF.push_front(pKF);
-  vector<KeyFrame *> vpKF(lpKF.begin(), lpKF.end());
+  vector<std::shared_ptr<KeyFrame>> vpKF(lpKF.begin(), lpKF.end());
 
   while (CheckNewKeyFrames()) {
     ProcessNewKeyFrame();
@@ -1429,11 +1410,9 @@ void LocalMapping::ScaleRefinement() {
   }
   std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
 
-  for (list<KeyFrame *>::iterator lit = mlNewKeyFrames.begin(),
-                                  lend = mlNewKeyFrames.end();
-       lit != lend; lit++) {
-    (*lit)->SetBadFlag();
-    delete *lit;
+  for (auto pKFi : mlNewKeyFrames) {
+    pKFi->SetBadFlag();
+    // delete *lit;
   }
   mlNewKeyFrames.clear();
 
@@ -1457,6 +1436,8 @@ double LocalMapping::GetCurrKFTime() {
   }
 }
 
-KeyFrame *LocalMapping::GetCurrKF() { return mpCurrentKeyFrame; }
+std::shared_ptr<KeyFrame> LocalMapping::GetCurrKF() {
+  return mpCurrentKeyFrame;
+}
 
 }  // namespace ORB_SLAM3
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index 4c37a8a0b3e..e6870ba8762 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -65,9 +65,9 @@ LoopClosing::LoopClosing(const std::shared_ptr<Atlas>& pAtlas,
       mbMergeDetected(false),
       mnLoopNumNotFound(0),
       mnMergeNumNotFound(0),
-      mbActiveLC(bActiveLC) {
+      mbActiveLC(bActiveLC),
+      mpLastCurrentKF() {
   mnCovisibilityConsistencyTh = 3;
-  mpLastCurrentKF = static_cast<KeyFrame*>(NULL);
 
 #ifdef REGISTER_TIMES
 
@@ -335,7 +335,7 @@ void LoopClosing::Run() {
   SetFinish();
 }
 
-void LoopClosing::InsertKeyFrame(KeyFrame* pKF) {
+void LoopClosing::InsertKeyFrame(const std::shared_ptr<KeyFrame>& pKF) {
   unique_lock<mutex> lock(mMutexLoopQueue);
   if (pKF->mnId != 0) {
     mlpLoopKeyFrameQueue.push_back(pKF);
@@ -510,11 +510,11 @@ bool LoopClosing::NewDetectCommonRegions() {
 
   // TODO: This is only necessary if we use a minimun score for pick the best
   // candidates
-  const vector<KeyFrame*> vpConnectedKeyFrames =
+  const vector<std::shared_ptr<KeyFrame>> vpConnectedKeyFrames =
       mpCurrentKF->GetVectorCovisibleKeyFrames();
 
   // Extract candidates from the bag of words
-  vector<KeyFrame*> vpMergeBowCand, vpLoopBowCand;
+  vector<std::shared_ptr<KeyFrame>> vpMergeBowCand, vpLoopBowCand;
   if (!bMergeDetectedInKF || !bLoopDetectedInKF) {
     // Search in BoW
 #ifdef REGISTER_TIMES
@@ -577,7 +577,8 @@ bool LoopClosing::NewDetectCommonRegions() {
 }
 
 bool LoopClosing::DetectAndReffineSim3FromLastKF(
-    KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3& gScw,
+    const std::shared_ptr<KeyFrame>& pCurrentKF,
+    std::shared_ptr<KeyFrame>& pMatchedKF, g2o::Sim3& gScw,
     int& nNumProjMatches, std::vector<MapPoint*>& vpMPs,
     std::vector<MapPoint*>& vpMatchedMPs) {
   set<MapPoint*> spAlreadyMatchedMPs;
@@ -629,16 +630,19 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(
 }
 
 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::vector<std::shared_ptr<KeyFrame>>& vpBowCand,
+    std::shared_ptr<KeyFrame>& pMatchedKF2,
+    std::shared_ptr<KeyFrame>& pLastCurrentKF, g2o::Sim3& g2oScw,
+    int& nNumCoincidences, std::vector<MapPoint*>& vpMPs,
+    std::vector<MapPoint*>& vpMatchedMPs) {
   int nBoWMatches = 20;
   int nBoWInliers = 15;
   int nSim3Inliers = 20;
   int nProjMatches = 50;
   int nProjOptMatches = 80;
 
-  set<KeyFrame*> spConnectedKeyFrames = mpCurrentKF->GetConnectedKeyFrames();
+  set<std::shared_ptr<KeyFrame>> spConnectedKeyFrames =
+      mpCurrentKF->GetConnectedKeyFrames();
 
   int nNumCovisibles = 10;
 
@@ -646,7 +650,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
   ORBmatcher matcher(0.75, true);
 
   // Varibles to select the best numbe
-  KeyFrame* pBestMatchedKF;
+  std::shared_ptr<KeyFrame> pBestMatchedKF;
   int nBestMatchesReproj = 0;
   int nBestNumCoindicendes = 0;
   g2o::Sim3 g2oBestScw;
@@ -661,12 +665,12 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
   // Verbose::PrintMess("BoW candidates: There are " +
   // to_string(vpBowCand.size()) + " possible candidates ",
   // Verbose::VERBOSITY_DEBUG);
-  for (KeyFrame* pKFi : vpBowCand) {
+  for (auto pKFi : vpBowCand) {
     if (!pKFi || pKFi->isBad()) continue;
 
     // std::cout << "KF candidate: " << pKFi->mnId << std::endl;
     // Current KF against KF with covisibles version
-    std::vector<KeyFrame*> vpCovKFi =
+    std::vector<std::shared_ptr<KeyFrame>> vpCovKFi =
         pKFi->GetBestCovisibilityKeyFrames(nNumCovisibles);
     if (vpCovKFi.empty()) {
       std::cout << "Covisible list empty" << std::endl;
@@ -697,13 +701,14 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
     std::set<MapPoint*> spMatchedMPi;
     int numBoWMatches = 0;
 
-    KeyFrame* pMostBoWMatchesKF = pKFi;
+    std::shared_ptr<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));
+    std::vector<std::shared_ptr<KeyFrame>> vpKeyFrameMatchedMP =
+        std::vector<std::shared_ptr<KeyFrame>>(
+            mpCurrentKF->GetMapPointMatches().size(), nullptr);
 
     int nIndexMostBoWMatchesKF = 0;
     for (int j = 0; j < vpCovKFi.size(); ++j) {
@@ -771,15 +776,16 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
         vpCovKFi =
             pMostBoWMatchesKF->GetBestCovisibilityKeyFrames(nNumCovisibles);
         vpCovKFi.push_back(pMostBoWMatchesKF);
-        set<KeyFrame*> spCheckKFs(vpCovKFi.begin(), vpCovKFi.end());
+        set<std::shared_ptr<KeyFrame>> spCheckKFs(vpCovKFi.begin(),
+                                                  vpCovKFi.end());
 
         // std::cout << "There are " << vpCovKFi.size() <<" near KFs" <<
         // std::endl;
 
         set<MapPoint*> spMapPoints;
         vector<MapPoint*> vpMapPoints;
-        vector<KeyFrame*> vpKeyFrames;
-        for (KeyFrame* pCovKFi : vpCovKFi) {
+        vector<std::shared_ptr<KeyFrame>> vpKeyFrames;
+        for (auto pCovKFi : vpCovKFi) {
           for (MapPoint* pCovMPij : pCovKFi->GetMapPointMatches()) {
             if (!pCovMPij || pCovMPij->isBad()) continue;
 
@@ -805,11 +811,9 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
         Sophus::Sim3f mScw = Converter::toSophus(gScw);
 
         vector<MapPoint*> vpMatchedMP;
-        vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(),
-                           static_cast<MapPoint*>(NULL));
-        vector<KeyFrame*> vpMatchedKF;
-        vpMatchedKF.resize(mpCurrentKF->GetMapPointMatches().size(),
-                           static_cast<KeyFrame*>(NULL));
+        vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size());
+        vector<std::shared_ptr<KeyFrame>> vpMatchedKF;
+        vpMatchedKF.resize(mpCurrentKF->GetMapPointMatches().size());
         int numProjMatches = matcher.SearchByProjection(
             mpCurrentKF, mScw, vpMapPoints, vpKeyFrames, vpMatchedMP,
             vpMatchedKF, 8, 1.5);
@@ -877,12 +881,12 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
               // vpMPs = vpMapPoints;
               //  Check the Sim3 transformation with the current KeyFrame
               //  covisibles
-              vector<KeyFrame*> vpCurrentCovKFs =
+              vector<std::shared_ptr<KeyFrame>> vpCurrentCovKFs =
                   mpCurrentKF->GetBestCovisibilityKeyFrames(nNumCovisibles);
 
               int j = 0;
               while (nNumKFs < 3 && j < vpCurrentCovKFs.size()) {
-                KeyFrame* pKFj = vpCurrentCovKFs[j];
+                std::shared_ptr<KeyFrame> pKFj = vpCurrentCovKFs[j];
                 Sophus::SE3d mTjc =
                     (pKFj->GetPose() * mpCurrentKF->GetPoseInverse())
                         .cast<double>();
@@ -954,7 +958,8 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
 }
 
 bool LoopClosing::DetectCommonRegionsFromLastKF(
-    KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3& gScw,
+    const std::shared_ptr<KeyFrame>& pCurrentKF,
+    const std::shared_ptr<KeyFrame>& pMatchedKF, g2o::Sim3& gScw,
     int& nNumProjMatches, std::vector<MapPoint*>& vpMPs,
     std::vector<MapPoint*>& vpMatchedMPs) {
   set<MapPoint*> spAlreadyMatchedMPs(vpMatchedMPs.begin(), vpMatchedMPs.end());
@@ -970,19 +975,21 @@ bool LoopClosing::DetectCommonRegionsFromLastKF(
 }
 
 int LoopClosing::FindMatchesByProjection(
-    KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3& g2oScw,
+    const std::shared_ptr<KeyFrame>& pCurrentKF,
+    const std::shared_ptr<KeyFrame>& pMatchedKFw, g2o::Sim3& g2oScw,
     set<MapPoint*>& spMatchedMPinOrigin, vector<MapPoint*>& vpMapPoints,
     vector<MapPoint*>& vpMatchedMapPoints) {
   int nNumCovisibles = 10;
-  vector<KeyFrame*> vpCovKFm =
+  vector<std::shared_ptr<KeyFrame>> vpCovKFm =
       pMatchedKFw->GetBestCovisibilityKeyFrames(nNumCovisibles);
   int nInitialCov = vpCovKFm.size();
   vpCovKFm.push_back(pMatchedKFw);
-  set<KeyFrame*> spCheckKFs(vpCovKFm.begin(), vpCovKFm.end());
-  set<KeyFrame*> spCurrentCovisbles = pCurrentKF->GetConnectedKeyFrames();
+  set<std::shared_ptr<KeyFrame>> spCheckKFs(vpCovKFm.begin(), vpCovKFm.end());
+  set<std::shared_ptr<KeyFrame>> spCurrentCovisbles =
+      pCurrentKF->GetConnectedKeyFrames();
   if (nInitialCov < nNumCovisibles) {
     for (int i = 0; i < nInitialCov; ++i) {
-      vector<KeyFrame*> vpKFs =
+      vector<std::shared_ptr<KeyFrame>> vpKFs =
           vpCovKFm[i]->GetBestCovisibilityKeyFrames(nNumCovisibles);
       int nInserted = 0;
       int j = 0;
@@ -1000,7 +1007,7 @@ int LoopClosing::FindMatchesByProjection(
   set<MapPoint*> spMapPoints;
   vpMapPoints.clear();
   vpMatchedMapPoints.clear();
-  for (KeyFrame* pKFi : vpCovKFm) {
+  for (auto pKFi : vpCovKFm) {
     for (MapPoint* pMPij : pKFi->GetMapPointMatches()) {
       if (!pMPij || pMPij->isBad()) continue;
 
@@ -1100,11 +1107,7 @@ void LoopClosing::CorrectLoop() {
 
     const bool bImuInit = pLoopMap->isImuInitialized();
 
-    for (vector<KeyFrame*>::iterator vit = mvpCurrentConnectedKFs.begin(),
-                                     vend = mvpCurrentConnectedKFs.end();
-         vit != vend; vit++) {
-      KeyFrame* pKFi = *vit;
-
+    for (auto pKFi : mvpCurrentConnectedKFs) {
       if (pKFi != mpCurrentKF) {
         Sophus::SE3f Tiw = pKFi->GetPose();
         Sophus::SE3d Tic = (Tiw * Twc).cast<double>();
@@ -1129,11 +1132,9 @@ void LoopClosing::CorrectLoop() {
 
     // Correct all MapPoints obsrved by current keyframe and neighbors, so that
     // they align with the other side of the loop
-    for (KeyFrameAndPose::iterator mit = CorrectedSim3.begin(),
-                                   mend = CorrectedSim3.end();
-         mit != mend; mit++) {
-      KeyFrame* pKFi = mit->first;
-      g2o::Sim3 g2oCorrectedSiw = mit->second;
+    for (auto mit : CorrectedSim3) {
+      auto pKFi = mit.first;
+      g2o::Sim3 g2oCorrectedSiw = mit.second;
       g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
 
       g2o::Sim3 g2oSiw = NonCorrectedSim3[pKFi];
@@ -1201,26 +1202,20 @@ void LoopClosing::CorrectLoop() {
 
   // After the MapPoint fusion, new links in the covisibility graph will appear
   // attaching both sides of the loop
-  map<KeyFrame*, set<KeyFrame*>> LoopConnections;
+  map<std::shared_ptr<KeyFrame>, set<std::shared_ptr<KeyFrame>>>
+      LoopConnections;
 
-  for (vector<KeyFrame*>::iterator vit = mvpCurrentConnectedKFs.begin(),
-                                   vend = mvpCurrentConnectedKFs.end();
-       vit != vend; vit++) {
-    KeyFrame* pKFi = *vit;
-    vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
+  for (auto pKFi : mvpCurrentConnectedKFs) {
+    auto vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
 
     // Update connections. Detect new links.
     pKFi->UpdateConnections();
     LoopConnections[pKFi] = pKFi->GetConnectedKeyFrames();
-    for (vector<KeyFrame*>::iterator vit_prev = vpPreviousNeighbors.begin(),
-                                     vend_prev = vpPreviousNeighbors.end();
-         vit_prev != vend_prev; vit_prev++) {
-      LoopConnections[pKFi].erase(*vit_prev);
+    for (auto vit_prev : vpPreviousNeighbors) {
+      LoopConnections[pKFi].erase(vit_prev);
     }
-    for (vector<KeyFrame*>::iterator vit2 = mvpCurrentConnectedKFs.begin(),
-                                     vend2 = mvpCurrentConnectedKFs.end();
-         vit2 != vend2; vit2++) {
-      LoopConnections[pKFi].erase(*vit2);
+    for (auto vit2 : mvpCurrentConnectedKFs) {
+      LoopConnections[pKFi].erase(vit2);
     }
   }
 
@@ -1296,11 +1291,11 @@ void LoopClosing::MergeLocal() {
 
   // Relationship to rebuild the essential graph, it is used two times, first in
   // the local window and later in the rest of the map
-  KeyFrame* pNewChild;
-  KeyFrame* pNewParent;
+  std::shared_ptr<KeyFrame> pNewChild;
+  std::shared_ptr<KeyFrame> pNewParent;
 
-  vector<KeyFrame*> vpLocalCurrentWindowKFs;
-  vector<KeyFrame*> vpMergeConnectedKFs;
+  vector<std::shared_ptr<KeyFrame>> vpLocalCurrentWindowKFs;
+  vector<std::shared_ptr<KeyFrame>> vpMergeConnectedKFs;
 
   // Flag that is true only when we stopped a running BA, in this case we need
   // relaunch at the end of the merge
@@ -1354,13 +1349,13 @@ void LoopClosing::MergeLocal() {
 
   // Get the current KF and its neighbors(visual->covisibles;
   // inertial->temporal+covisibles)
-  set<KeyFrame*> spLocalWindowKFs;
+  set<std::shared_ptr<KeyFrame>> spLocalWindowKFs;
   // Get MPs in the welding area from the current map
   set<MapPoint*> spLocalWindowMPs;
 
   // TODO Check the correct initialization
   if (pCurrentMap->IsInertial() && pMergeMap->IsInertial()) {
-    KeyFrame* pKFi = mpCurrentKF;
+    std::shared_ptr<KeyFrame> pKFi = mpCurrentKF;
     int nInserted = 0;
     while (pKFi && nInserted < numTemporalKFs) {
       spLocalWindowKFs.insert(pKFi);
@@ -1384,19 +1379,19 @@ void LoopClosing::MergeLocal() {
     spLocalWindowKFs.insert(mpCurrentKF);
   }
 
-  vector<KeyFrame*> vpCovisibleKFs =
+  vector<std::shared_ptr<KeyFrame>> vpCovisibleKFs =
       mpCurrentKF->GetBestCovisibilityKeyFrames(numTemporalKFs);
   spLocalWindowKFs.insert(vpCovisibleKFs.begin(), vpCovisibleKFs.end());
   spLocalWindowKFs.insert(mpCurrentKF);
   const int nMaxTries = 5;
   int nNumTries = 0;
   while (spLocalWindowKFs.size() < numTemporalKFs && nNumTries < nMaxTries) {
-    vector<KeyFrame*> vpNewCovKFs;
+    vector<std::shared_ptr<KeyFrame>> vpNewCovKFs;
     vpNewCovKFs.empty();
-    for (KeyFrame* pKFi : spLocalWindowKFs) {
-      vector<KeyFrame*> vpKFiCov =
+    for (auto pKFi : spLocalWindowKFs) {
+      vector<std::shared_ptr<KeyFrame>> vpKFiCov =
           pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs / 2);
-      for (KeyFrame* pKFcov : vpKFiCov) {
+      for (auto pKFcov : vpKFiCov) {
         if (pKFcov && !pKFcov->isBad() &&
             spLocalWindowKFs.find(pKFcov) == spLocalWindowKFs.end()) {
           vpNewCovKFs.push_back(pKFcov);
@@ -1408,7 +1403,7 @@ void LoopClosing::MergeLocal() {
     nNumTries++;
   }
 
-  for (KeyFrame* pKFi : spLocalWindowKFs) {
+  for (auto pKFi : spLocalWindowKFs) {
     if (!pKFi || pKFi->isBad()) continue;
 
     set<MapPoint*> spMPs = pKFi->GetMapPoints();
@@ -1419,10 +1414,10 @@ void LoopClosing::MergeLocal() {
   // = " << to_string(spLocalWindowKFs.size()) << "; #MPs = " <<
   // to_string(spLocalWindowMPs.size()) << std::endl;
 
-  set<KeyFrame*> spMergeConnectedKFs;
+  set<std::shared_ptr<KeyFrame>> spMergeConnectedKFs;
   // TODO Check the correct initialization
   if (pCurrentMap->IsInertial() && pMergeMap->IsInertial()) {
-    KeyFrame* pKFi = mpMergeMatchedKF;
+    auto pKFi = mpMergeMatchedKF;
     int nInserted = 0;
     while (pKFi && nInserted < numTemporalKFs / 2) {
       spMergeConnectedKFs.insert(pKFi);
@@ -1444,11 +1439,11 @@ void LoopClosing::MergeLocal() {
   spMergeConnectedKFs.insert(mpMergeMatchedKF);
   nNumTries = 0;
   while (spMergeConnectedKFs.size() < numTemporalKFs && nNumTries < nMaxTries) {
-    vector<KeyFrame*> vpNewCovKFs;
-    for (KeyFrame* pKFi : spMergeConnectedKFs) {
-      vector<KeyFrame*> vpKFiCov =
+    vector<std::shared_ptr<KeyFrame>> vpNewCovKFs;
+    for (auto pKFi : spMergeConnectedKFs) {
+      vector<std::shared_ptr<KeyFrame>> vpKFiCov =
           pKFi->GetBestCovisibilityKeyFrames(numTemporalKFs / 2);
-      for (KeyFrame* pKFcov : vpKFiCov) {
+      for (auto pKFcov : vpKFiCov) {
         if (pKFcov && !pKFcov->isBad() &&
             spMergeConnectedKFs.find(pKFcov) == spMergeConnectedKFs.end()) {
           vpNewCovKFs.push_back(pKFcov);
@@ -1461,7 +1456,7 @@ void LoopClosing::MergeLocal() {
   }
 
   set<MapPoint*> spMapPointMerge;
-  for (KeyFrame* pKFi : spMergeConnectedKFs) {
+  for (auto pKFi : spMergeConnectedKFs) {
     set<MapPoint*> vpMPs = pKFi->GetMapPoints();
     spMapPointMerge.insert(vpMPs.begin(), vpMPs.end());
   }
@@ -1489,7 +1484,7 @@ void LoopClosing::MergeLocal() {
   vnMergeKFs.push_back(spLocalWindowKFs.size() + spMergeConnectedKFs.size());
   vnMergeMPs.push_back(spLocalWindowMPs.size() + spMapPointMerge.size());
 #endif
-  for (KeyFrame* pKFi : spLocalWindowKFs) {
+  for (auto pKFi : spLocalWindowKFs) {
     if (!pKFi || pKFi->isBad()) {
       Verbose::PrintMess("Bad KF in correction", Verbose::VERBOSITY_DEBUG);
       continue;
@@ -1546,7 +1541,7 @@ void LoopClosing::MergeLocal() {
       continue;
     }
 
-    KeyFrame* pKFref = pMPi->GetReferenceKeyFrame();
+    auto pKFref = pMPi->GetReferenceKeyFrame();
     if (vCorrectedSim3.find(pKFref) == vCorrectedSim3.end()) {
       itMP = spLocalWindowMPs.erase(itMP);
       numPointsWithCorrection++;
@@ -1585,7 +1580,7 @@ void LoopClosing::MergeLocal() {
 
     // std::cout << "Merge local window: " << spLocalWindowKFs.size() <<
     // std::endl; std::cout << "[Merge]: init merging maps " << std::endl;
-    for (KeyFrame* pKFi : spLocalWindowKFs) {
+    for (auto pKFi : spLocalWindowKFs) {
       if (!pKFi || pKFi->isBad()) {
         // std::cout << "Bad KF in correction" << std::endl;
         continue;
@@ -1639,7 +1634,7 @@ void LoopClosing::MergeLocal() {
   while (pNewChild) {
     pNewChild->EraseChild(pNewParent);  // We remove the relation between the
                                         // old parent and the new for avoid loop
-    KeyFrame* pOldParent = pNewChild->GetParent();
+    auto pOldParent = pNewChild->GetParent();
 
     pNewChild->ChangeParent(pNewParent);
 
@@ -1664,12 +1659,12 @@ void LoopClosing::MergeLocal() {
   // std::cout << "[Merge]: fuse points finished" << std::endl;
 
   // Update connectivity
-  for (KeyFrame* pKFi : spLocalWindowKFs) {
+  for (auto pKFi : spLocalWindowKFs) {
     if (!pKFi || pKFi->isBad()) continue;
 
     pKFi->UpdateConnections();
   }
-  for (KeyFrame* pKFi : spMergeConnectedKFs) {
+  for (auto pKFi : spMergeConnectedKFs) {
     if (!pKFi || pKFi->isBad()) continue;
 
     pKFi->UpdateConnections();
@@ -1719,8 +1714,8 @@ void LoopClosing::MergeLocal() {
   mpLocalMapper->Release();
 
   // Update the non critical area from the current map to the merged map
-  vector<KeyFrame*> vpCurrentMapKFs = pCurrentMap->GetAllKeyFrames();
-  vector<MapPoint*> vpCurrentMapMPs = pCurrentMap->GetAllMapPoints();
+  auto vpCurrentMapKFs = pCurrentMap->GetAllKeyFrames();
+  auto vpCurrentMapMPs = pCurrentMap->GetAllMapPoints();
 
   if (vpCurrentMapKFs.size() == 0) {
   } else {
@@ -1729,7 +1724,7 @@ void LoopClosing::MergeLocal() {
           pCurrentMap->mMutexMapUpdate);  // We update the current map with the
                                           // Merge information
 
-      for (KeyFrame* pKFi : vpCurrentMapKFs) {
+      for (auto pKFi : vpCurrentMapKFs) {
         if (!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap) {
           continue;
         }
@@ -1771,7 +1766,7 @@ void LoopClosing::MergeLocal() {
       for (MapPoint* pMPi : vpCurrentMapMPs) {
         if (!pMPi || pMPi->isBad() || pMPi->GetMap() != pCurrentMap) continue;
 
-        KeyFrame* pKFref = pMPi->GetReferenceKeyFrame();
+        auto pKFref = pMPi->GetReferenceKeyFrame();
         g2o::Sim3 g2oCorrectedSwi = vCorrectedSim3[pKFref].inverse();
         g2o::Sim3 g2oNonCorrectedSiw = vNonCorrectedSim3[pKFref];
 
@@ -1810,7 +1805,7 @@ void LoopClosing::MergeLocal() {
 
       // std::cout << "Merge outside KFs: " << vpCurrentMapKFs.size() <<
       // std::endl;
-      for (KeyFrame* pKFi : vpCurrentMapKFs) {
+      for (auto pKFi : vpCurrentMapKFs) {
         if (!pKFi || pKFi->isBad() || pKFi->GetMap() != pCurrentMap) {
           continue;
         }
@@ -1873,11 +1868,11 @@ void LoopClosing::MergeLocal2() {
 
   // Relationship to rebuild the essential graph, it is used two times, first in
   // the local window and later in the rest of the map
-  KeyFrame* pNewChild;
-  KeyFrame* pNewParent;
+  std::shared_ptr<KeyFrame> pNewChild;
+  std::shared_ptr<KeyFrame> pNewParent;
 
-  vector<KeyFrame*> vpLocalCurrentWindowKFs;
-  vector<KeyFrame*> vpMergeConnectedKFs;
+  vector<std::shared_ptr<KeyFrame>> vpLocalCurrentWindowKFs;
+  vector<std::shared_ptr<KeyFrame>> vpMergeConnectedKFs;
 
   KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
   // NonCorrectedSim3[mpCurrentKF]=mg2oLoopScw;
@@ -1971,10 +1966,11 @@ void LoopClosing::MergeLocal2() {
         pMergeMap->mMutexMapUpdate);  // We remove the Kfs and MPs in the merged
                                       // area from the old map
 
-    vector<KeyFrame*> vpMergeMapKFs = pMergeMap->GetAllKeyFrames();
+    vector<std::shared_ptr<KeyFrame>> vpMergeMapKFs =
+        pMergeMap->GetAllKeyFrames();
     vector<MapPoint*> vpMergeMapMPs = pMergeMap->GetAllMapPoints();
 
-    for (KeyFrame* pKFi : vpMergeMapKFs) {
+    for (auto pKFi : vpMergeMapKFs) {
       if (!pKFi || pKFi->isBad() || pKFi->GetMap() != pMergeMap) {
         continue;
       }
@@ -1994,8 +1990,8 @@ void LoopClosing::MergeLocal2() {
     }
 
     // Save non corrected poses (already merged maps)
-    vector<KeyFrame*> vpKFs = pCurrentMap->GetAllKeyFrames();
-    for (KeyFrame* pKFi : vpKFs) {
+    auto vpKFs = pCurrentMap->GetAllKeyFrames();
+    for (auto pKFi : vpKFs) {
       Sophus::SE3d Tiw = (pKFi->GetPose()).cast<double>();
       g2o::Sim3 g2oSiw(Tiw.unit_quaternion(), Tiw.translation(), 1.0);
       NonCorrectedSim3[pKFi] = g2oSiw;
@@ -2025,7 +2021,7 @@ void LoopClosing::MergeLocal2() {
   while (pNewChild) {
     pNewChild->EraseChild(pNewParent);  // We remove the relation between the
                                         // old parent and the new for avoid loop
-    KeyFrame* pOldParent = pNewChild->GetParent();
+    auto pOldParent = pNewChild->GetParent();
     pNewChild->ChangeParent(pNewParent);
     pNewParent = pNewChild;
     pNewChild = pOldParent;
@@ -2044,10 +2040,10 @@ void LoopClosing::MergeLocal2() {
   vector<MapPoint*>
       vpCheckFuseMapPoint;  // MapPoint vector from current map to allow to fuse
                             // duplicated points with the old map (merge)
-  vector<KeyFrame*> vpCurrentConnectedKFs;
+  vector<std::shared_ptr<KeyFrame>> vpCurrentConnectedKFs;
 
   mvpMergeConnectedKFs.push_back(mpMergeMatchedKF);
-  vector<KeyFrame*> aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();
+  auto aux = mpMergeMatchedKF->GetVectorCovisibleKeyFrames();
   mvpMergeConnectedKFs.insert(mvpMergeConnectedKFs.end(), aux.begin(),
                               aux.end());
   if (mvpMergeConnectedKFs.size() > 6)
@@ -2068,7 +2064,7 @@ void LoopClosing::MergeLocal2() {
                                 vpCurrentConnectedKFs.end());
 
   set<MapPoint*> spMapPointMerge;
-  for (KeyFrame* pKFi : mvpMergeConnectedKFs) {
+  for (auto pKFi : mvpMergeConnectedKFs) {
     set<MapPoint*> vpMPs = pKFi->GetMapPoints();
     spMapPointMerge.insert(vpMPs.begin(), vpMPs.end());
     if (spMapPointMerge.size() > 1000) break;
@@ -2103,12 +2099,13 @@ void LoopClosing::MergeLocal2() {
 
   cout << "Init to update connections" << endl;*/
 
-  for (KeyFrame* pKFi : vpCurrentConnectedKFs) {
+  for (auto pKFi : vpCurrentConnectedKFs) {
     if (!pKFi || pKFi->isBad()) continue;
 
     pKFi->UpdateConnections();
   }
-  for (KeyFrame* pKFi : mvpMergeConnectedKFs) {
+
+  for (auto pKFi : mvpMergeConnectedKFs) {
     if (!pKFi || pKFi->isBad()) continue;
 
     pKFi->UpdateConnections();
@@ -2135,7 +2132,7 @@ void LoopClosing::MergeLocal2() {
 
   // Perform BA
   bool bStopFlag = false;
-  KeyFrame* pCurrKF = mpTracker->GetLastKeyFrame();
+  auto pCurrKF = mpTracker->GetLastKeyFrame();
   // cout << "start MergeInertialBA" << endl;
   Optimizer::MergeInertialBA(pCurrKF, mpMergeMatchedKF, &bStopFlag, pCurrentMap,
                              CorrectedSim3);
@@ -2151,11 +2148,11 @@ void LoopClosing::MergeLocal2() {
   return;
 }
 
-void LoopClosing::CheckObservations(set<KeyFrame*>& spKFsMap1,
-                                    set<KeyFrame*>& spKFsMap2) {
+void LoopClosing::CheckObservations(set<std::shared_ptr<KeyFrame>>& spKFsMap1,
+                                    set<std::shared_ptr<KeyFrame>>& spKFsMap2) {
   cout << "----------------------" << endl;
-  for (KeyFrame* pKFi1 : spKFsMap1) {
-    map<KeyFrame*, int> mMatchedMP;
+  for (auto pKFi1 : spKFsMap1) {
+    map<std::shared_ptr<KeyFrame>, int> mMatchedMP;
     set<MapPoint*> spMPs = pKFi1->GetMapPoints();
 
     for (MapPoint* pMPij : spMPs) {
@@ -2163,8 +2160,9 @@ void LoopClosing::CheckObservations(set<KeyFrame*>& spKFsMap1,
         continue;
       }
 
-      map<KeyFrame*, tuple<int, int>> mMPijObs = pMPij->GetObservations();
-      for (KeyFrame* pKFi2 : spKFsMap2) {
+      map<std::shared_ptr<KeyFrame>, tuple<int, int>> mMPijObs =
+          pMPij->GetObservations();
+      for (auto pKFi2 : spKFsMap2) {
         if (mMPijObs.find(pKFi2) != mMPijObs.end()) {
           if (mMatchedMP.find(pKFi2) != mMatchedMP.end()) {
             mMatchedMP[pKFi2] = mMatchedMP[pKFi2] + 1;
@@ -2181,7 +2179,7 @@ void LoopClosing::CheckObservations(set<KeyFrame*>& spKFsMap1,
     } else {
       cout << "CHECK-OBS: KF " << pKFi1->mnId << " has matched MP with "
            << mMatchedMP.size() << " KF from the other map" << endl;
-      for (pair<KeyFrame*, int> matchedKF : mMatchedMP) {
+      for (pair<std::shared_ptr<KeyFrame>, int> matchedKF : mMatchedMP) {
         cout << "   -KF: " << matchedKF.first->mnId
              << ", Number of matches: " << matchedKF.second << endl;
       }
@@ -2203,7 +2201,7 @@ void LoopClosing::SearchAndFuse(const KeyFrameAndPose& CorrectedPosesMap,
                                        mend = CorrectedPosesMap.end();
        mit != mend; mit++) {
     int num_replaces = 0;
-    KeyFrame* pKFi = mit->first;
+    std::shared_ptr<KeyFrame> pKFi = mit->first;
     std::shared_ptr<Map> pMap = pKFi->GetMap();
 
     g2o::Sim3 g2oScw = mit->second;
@@ -2229,8 +2227,9 @@ void LoopClosing::SearchAndFuse(const KeyFrameAndPose& CorrectedPosesMap,
   // cout << "[FUSE]: " << total_replaces << " MPs had been fused" << endl;
 }
 
-void LoopClosing::SearchAndFuse(const vector<KeyFrame*>& vConectedKFs,
-                                vector<MapPoint*>& vpMapPoints) {
+void LoopClosing::SearchAndFuse(
+    const vector<std::shared_ptr<KeyFrame>>& vConectedKFs,
+    vector<MapPoint*>& vpMapPoints) {
   ORBmatcher matcher(0.8);
 
   int total_replaces = 0;
@@ -2241,7 +2240,7 @@ void LoopClosing::SearchAndFuse(const vector<KeyFrame*>& vConectedKFs,
   for (auto mit = vConectedKFs.begin(), mend = vConectedKFs.end(); mit != mend;
        mit++) {
     int num_replaces = 0;
-    KeyFrame* pKF = (*mit);
+    std::shared_ptr<KeyFrame> pKF = (*mit);
     std::shared_ptr<Map> pMap = pKF->GetMap();
     Sophus::SE3f Tcw = pKF->GetPose();
     Sophus::Sim3f Scw(Tcw.unit_quaternion(), Tcw.translation());
@@ -2310,9 +2309,10 @@ void LoopClosing::ResetIfRequested() {
     mbResetRequested = false;
     mbResetActiveMapRequested = false;
   } else if (mbResetActiveMapRequested) {
-    for (list<KeyFrame*>::const_iterator it = mlpLoopKeyFrameQueue.begin();
+    for (list<std::shared_ptr<KeyFrame>>::const_iterator it =
+             mlpLoopKeyFrameQueue.begin();
          it != mlpLoopKeyFrameQueue.end();) {
-      KeyFrame* pKFi = *it;
+      std::shared_ptr<KeyFrame> pKFi = *it;
       if (pKFi->GetMap() == mpMapToReset) {
         it = mlpLoopKeyFrameQueue.erase(it);
       } else {
@@ -2396,21 +2396,20 @@ void LoopClosing::RunGlobalBundleAdjustment(
 
       // pActiveMap->PrintEssentialGraph();
       //  Correct keyframes starting at map first keyframe
-      list<KeyFrame*> lpKFtoCheck(pActiveMap->mvpKeyFrameOrigins.begin(),
-                                  pActiveMap->mvpKeyFrameOrigins.end());
+      list<std::shared_ptr<KeyFrame>> lpKFtoCheck(
+          pActiveMap->mvpKeyFrameOrigins.begin(),
+          pActiveMap->mvpKeyFrameOrigins.end());
 
       while (!lpKFtoCheck.empty()) {
-        KeyFrame* pKF = lpKFtoCheck.front();
-        const set<KeyFrame*> sChilds = pKF->GetChilds();
+        std::shared_ptr<KeyFrame> pKF = lpKFtoCheck.front();
+        const set<std::shared_ptr<KeyFrame>> sChilds = pKF->GetChilds();
         // cout << "---Updating KF " << pKF->mnId << " with " << sChilds.size()
         // << " childs" << endl; cout << " KF mnBAGlobalForKF: " <<
         // pKF->mnBAGlobalForKF << endl;
         Sophus::SE3f Twc = pKF->GetPoseInverse();
         // cout << "Twc: " << Twc << endl;
         // cout << "GBA: Correct KeyFrames" << endl;
-        for (set<KeyFrame*>::const_iterator sit = sChilds.begin();
-             sit != sChilds.end(); sit++) {
-          KeyFrame* pChild = *sit;
+        for (auto pChild : sChilds) {
           if (!pChild || pChild->isBad()) continue;
 
           if (pChild->mnBAGlobalForKF != nLoopKF) {
@@ -2530,7 +2529,7 @@ void LoopClosing::RunGlobalBundleAdjustment(
           pMP->SetWorldPos(pMP->mPosGBA);
         } else {
           // Update according to the correction of its reference keyframe
-          KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
+          auto pRefKF = pMP->GetReferenceKeyFrame();
 
           if (pRefKF->mnBAGlobalForKF != nLoopKF) continue;
 
diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp
index 31d4c2e3d8a..14c9c305c1b 100644
--- a/src/MLPnPsolver.cpp
+++ b/src/MLPnPsolver.cpp
@@ -54,23 +54,24 @@
 #include <Eigen/Sparse>
 #include <algorithm>
 #include <limits>
+#include <memory>
 #include <vector>
 
 namespace ORB_SLAM3 {
-MLPnPsolver::MLPnPsolver(const Frame &F,
+MLPnPsolver::MLPnPsolver(const std::shared_ptr<Frame> &F,
                          const vector<MapPoint *> &vpMapPointMatches)
     : mnInliersi(0),
       mnIterations(0),
       mnBestInliers(0),
       N(0),
-      mpCamera(F.mpCamera) {
+      mpCamera(F->mpCamera) {
   mvpMapPointMatches = vpMapPointMatches;
-  mvBearingVecs.reserve(F.mvpMapPoints.size());
-  mvP2D.reserve(F.mvpMapPoints.size());
-  mvSigma2.reserve(F.mvpMapPoints.size());
-  mvP3Dw.reserve(F.mvpMapPoints.size());
-  mvKeyPointIndices.reserve(F.mvpMapPoints.size());
-  mvAllIndices.reserve(F.mvpMapPoints.size());
+  mvBearingVecs.reserve(F->mvpMapPoints.size());
+  mvP2D.reserve(F->mvpMapPoints.size());
+  mvSigma2.reserve(F->mvpMapPoints.size());
+  mvP3Dw.reserve(F->mvpMapPoints.size());
+  mvKeyPointIndices.reserve(F->mvpMapPoints.size());
+  mvAllIndices.reserve(F->mvpMapPoints.size());
 
   int idx = 0;
   for (size_t i = 0, iend = mvpMapPointMatches.size(); i < iend; i++) {
@@ -78,11 +79,11 @@ MLPnPsolver::MLPnPsolver(const Frame &F,
 
     if (pMP) {
       if (!pMP->isBad()) {
-        if (i >= F.mvKeysUn.size()) continue;
-        const cv::KeyPoint &kp = F.mvKeysUn[i];
+        if (i >= F->mvKeysUn.size()) continue;
+        const cv::KeyPoint &kp = F->mvKeysUn[i];
 
         mvP2D.push_back(kp.pt);
-        mvSigma2.push_back(F.mvLevelSigma2[kp.octave]);
+        mvSigma2.push_back(F->mvLevelSigma2[kp.octave]);
 
         // Bearing vector should be normalized
         cv::Point3f cv_br = mpCamera->unproject(kp.pt);
diff --git a/src/Map.cc b/src/Map.cc
index 58784c2a1ff..e62835b9c98 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -84,7 +84,7 @@ Map::~Map() {
   mvpKeyFrameOrigins.clear();
 }
 
-void Map::AddKeyFrame(KeyFrame* pKF) {
+void Map::AddKeyFrame(const std::shared_ptr<KeyFrame>& pKF) {
   unique_lock<mutex> lock(mMutexMap);
   if (mspKeyFrames.empty()) {
     cout << "First KF:" << pKF->mnId << "; Map init KF:" << mnInitKFid << endl;
@@ -124,13 +124,14 @@ void Map::EraseMapPoint(MapPoint* pMP) {
   // Delete the MapPoint
 }
 
-void Map::EraseKeyFrame(KeyFrame* pKF) {
+void Map::EraseKeyFrame(const std::shared_ptr<KeyFrame>& pKF) {
   unique_lock<mutex> lock(mMutexMap);
   mspKeyFrames.erase(pKF);
   if (mspKeyFrames.size() > 0) {
     if (pKF->mnId == mpKFlowerID->mnId) {
-      vector<KeyFrame*> vpKFs =
-          vector<KeyFrame*>(mspKeyFrames.begin(), mspKeyFrames.end());
+      vector<std::shared_ptr<KeyFrame>> vpKFs =
+          vector<std::shared_ptr<KeyFrame>>(mspKeyFrames.begin(),
+                                            mspKeyFrames.end());
       sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
       mpKFlowerID = vpKFs[0];
     }
@@ -157,9 +158,10 @@ int Map::GetLastBigChangeIdx() {
   return mnBigChangeIdx;
 }
 
-vector<KeyFrame*> Map::GetAllKeyFrames() {
+vector<std::shared_ptr<KeyFrame>> Map::GetAllKeyFrames() {
   unique_lock<mutex> lock(mMutexMap);
-  return vector<KeyFrame*>(mspKeyFrames.begin(), mspKeyFrames.end());
+  return vector<std::shared_ptr<KeyFrame>>(mspKeyFrames.begin(),
+                                           mspKeyFrames.end());
 }
 
 vector<MapPoint*> Map::GetAllMapPoints() {
@@ -198,7 +200,7 @@ unsigned long int Map::GetMaxKFid() {
   return mnMaxKFid;
 }
 
-KeyFrame* Map::GetOriginKF() { return mpKFinitial; }
+std::shared_ptr<KeyFrame> Map::GetOriginKF() { return mpKFinitial; }
 
 void Map::SetCurrentMap() { mIsInUse = true; }
 
@@ -209,10 +211,10 @@ void Map::clear() {
   //    send=mspMapPoints.end(); sit!=send; sit++)
   //        delete *sit;
 
-  for (set<KeyFrame*>::iterator sit = mspKeyFrames.begin(),
-                                send = mspKeyFrames.end();
+  for (set<std::shared_ptr<KeyFrame>>::iterator sit = mspKeyFrames.begin(),
+                                                send = mspKeyFrames.end();
        sit != send; sit++) {
-    KeyFrame* pKF = *sit;
+    std::shared_ptr<KeyFrame> pKF = *sit;
     pKF->UpdateMap(nullptr);
     //        delete *sit;
   }
@@ -242,9 +244,9 @@ void Map::ApplyScaledRotation(const Sophus::SE3f& T, const float s,
   Eigen::Matrix3f Ryw = Tyw.rotationMatrix();
   Eigen::Vector3f tyw = Tyw.translation();
 
-  for (set<KeyFrame*>::iterator sit = mspKeyFrames.begin();
+  for (set<std::shared_ptr<KeyFrame>>::iterator sit = mspKeyFrames.begin();
        sit != mspKeyFrames.end(); sit++) {
-    KeyFrame* pKF = *sit;
+    std::shared_ptr<KeyFrame> pKF = *sit;
     Sophus::SE3f Twc = pKF->GetPoseInverse();
     Twc.translation() *= s;
     Sophus::SE3f Tyc = Tyw * Twc;
@@ -333,9 +335,11 @@ void Map::PreSave(std::set<std::shared_ptr<GeometricCamera>>& spCams) {
     if (pMPi->GetObservations().size() == 0) {
       nMPWithoutObs++;
     }
-    map<KeyFrame*, std::tuple<int, int>> mpObs = pMPi->GetObservations();
-    for (map<KeyFrame*, std::tuple<int, int>>::iterator it = mpObs.begin(),
-                                                        end = mpObs.end();
+    map<std::shared_ptr<KeyFrame>, std::tuple<int, int>> mpObs =
+        pMPi->GetObservations();
+    for (map<std::shared_ptr<KeyFrame>, std::tuple<int, int>>::iterator
+             it = mpObs.begin(),
+             end = mpObs.end();
          it != end; ++it) {
       if (it->first->GetMap().get() != this || it->first->isBad()) {
         pMPi->EraseObservation(it->first);
@@ -361,7 +365,7 @@ void Map::PreSave(std::set<std::shared_ptr<GeometricCamera>>& spCams) {
 
   // Backup of KeyFrames
   mvpBackupKeyFrames.clear();
-  for (KeyFrame* pKFi : mspKeyFrames) {
+  for (auto pKFi : mspKeyFrames) {
     if (!pKFi || pKFi->isBad()) continue;
 
     mvpBackupKeyFrames.push_back(pKFi);
@@ -397,8 +401,8 @@ void Map::PostLoad(
     mpMapPointId[pMPi->mnId] = pMPi;
   }
 
-  map<unsigned long int, KeyFrame*> mpKeyFrameId;
-  for (KeyFrame* pKFi : mspKeyFrames) {
+  map<unsigned long int, std::shared_ptr<KeyFrame>> mpKeyFrameId;
+  for (auto pKFi : mspKeyFrames) {
     if (!pKFi || pKFi->isBad()) continue;
 
     pKFi->UpdateMap(shared_from_this());
@@ -414,7 +418,7 @@ void Map::PostLoad(
     pMPi->PostLoad(mpKeyFrameId, mpMapPointId);
   }
 
-  for (KeyFrame* pKFi : mspKeyFrames) {
+  for (auto pKFi : mspKeyFrames) {
     if (!pKFi || pKFi->isBad()) continue;
 
     pKFi->PostLoad(mpKeyFrameId, mpMapPointId, mpCams);
diff --git a/src/MapDrawer.cc b/src/MapDrawer.cc
index dc77175ffa8..68d6ed70eaa 100644
--- a/src/MapDrawer.cc
+++ b/src/MapDrawer.cc
@@ -98,11 +98,10 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph,
 
   if (!pActiveMap) return;
 
-  const vector<KeyFrame *> vpKFs = pActiveMap->GetAllKeyFrames();
+  auto const vpKFs = pActiveMap->GetAllKeyFrames();
 
   if (bDrawKF) {
-    for (size_t i = 0; i < vpKFs.size(); i++) {
-      KeyFrame *pKF = vpKFs[i];
+    for (auto const &pKF : vpKFs) {
       Eigen::Matrix4f Twc = pKF->GetPoseInverse().matrix();
       unsigned int index_color = pKF->mnOriginMapId;
 
@@ -166,23 +165,22 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph,
     glBegin(GL_LINES);
 
     // cout << "-----------------Draw graph-----------------" << endl;
-    for (size_t i = 0; i < vpKFs.size(); i++) {
+
+    for (auto const &pKFi : vpKFs) {
       // Covisibility Graph
-      const vector<KeyFrame *> vCovKFs = vpKFs[i]->GetCovisiblesByWeight(100);
-      Eigen::Vector3f Ow = vpKFs[i]->GetCameraCenter();
+      auto const vCovKFs = pKFi->GetCovisiblesByWeight(100);
+      Eigen::Vector3f Ow = pKFi->GetCameraCenter();
       if (!vCovKFs.empty()) {
-        for (vector<KeyFrame *>::const_iterator vit = vCovKFs.begin(),
-                                                vend = vCovKFs.end();
-             vit != vend; vit++) {
-          if ((*vit)->mnId < vpKFs[i]->mnId) continue;
-          Eigen::Vector3f Ow2 = (*vit)->GetCameraCenter();
+        for (auto const &pKF2 : vCovKFs) {
+          if (pKF2->mnId < pKFi->mnId) continue;
+          Eigen::Vector3f Ow2 = pKF2->GetCameraCenter();
           glVertex3f(Ow(0), Ow(1), Ow(2));
           glVertex3f(Ow2(0), Ow2(1), Ow2(2));
         }
       }
 
       // Spanning tree
-      KeyFrame *pParent = vpKFs[i]->GetParent();
+      auto pParent = pKFi->GetParent();
       if (pParent) {
         Eigen::Vector3f Owp = pParent->GetCameraCenter();
         glVertex3f(Ow(0), Ow(1), Ow(2));
@@ -190,12 +188,10 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph,
       }
 
       // Loops
-      set<KeyFrame *> sLoopKFs = vpKFs[i]->GetLoopEdges();
-      for (set<KeyFrame *>::iterator sit = sLoopKFs.begin(),
-                                     send = sLoopKFs.end();
-           sit != send; sit++) {
-        if ((*sit)->mnId < vpKFs[i]->mnId) continue;
-        Eigen::Vector3f Owl = (*sit)->GetCameraCenter();
+      auto const sLoopKFs = pKFi->GetLoopEdges();
+      for (auto pKF2 : sLoopKFs) {
+        if (pKF2->mnId < pKFi->mnId) continue;
+        Eigen::Vector3f Owl = pKF2->GetCameraCenter();
         glVertex3f(Ow(0), Ow(1), Ow(2));
         glVertex3f(Owl(0), Owl(1), Owl(2));
       }
@@ -210,10 +206,9 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph,
     glBegin(GL_LINES);
 
     // Draw inertial links
-    for (size_t i = 0; i < vpKFs.size(); i++) {
-      KeyFrame *pKFi = vpKFs[i];
+    for (auto const &pKFi : vpKFs) {
       Eigen::Vector3f Ow = pKFi->GetCameraCenter();
-      KeyFrame *pNext = pKFi->mNextKF;
+      auto const &pNext = pKFi->mNextKF;
       if (pNext) {
         Eigen::Vector3f Owp = pNext->GetCameraCenter();
         glVertex3f(Ow(0), Ow(1), Ow(2));
@@ -230,10 +225,9 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph,
     for (auto pMap : vpMaps) {
       if (pMap == pActiveMap) continue;
 
-      vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames();
+      auto const vpKFs = pMap->GetAllKeyFrames();
 
-      for (size_t i = 0; i < vpKFs.size(); i++) {
-        KeyFrame *pKF = vpKFs[i];
+      for (auto const &pKF : vpKFs) {
         Eigen::Matrix4f Twc = pKF->GetPoseInverse().matrix();
         unsigned int index_color = pKF->mnOriginMapId;
 
@@ -241,7 +235,7 @@ void MapDrawer::DrawKeyFrames(const bool bDrawKF, const bool bDrawGraph,
 
         glMultMatrixf(static_cast<GLfloat *>(Twc.data()));
 
-        if (!vpKFs[i]->GetParent()) {
+        if (!pKF->GetParent()) {
           // It is the first KF in the map
           glLineWidth(mKeyFrameLineWidth * 5);
           glColor3f(1.0f, 0.0f, 0.0f);
diff --git a/src/MapPoint.cc b/src/MapPoint.cc
index 85db88fed8e..e288d8f0490 100644
--- a/src/MapPoint.cc
+++ b/src/MapPoint.cc
@@ -56,7 +56,8 @@ MapPoint::MapPoint()
   mpReplaced = static_cast<MapPoint*>(NULL);
 }
 
-MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF,
+MapPoint::MapPoint(const Eigen::Vector3f& Pos,
+                   const std::shared_ptr<KeyFrame>& pRefKF,
                    const std::shared_ptr<Map>& pMap)
     : mnFirstKFid(pRefKF->mnId),
       mnFirstFrame(pRefKF->mnFrameId),
@@ -73,7 +74,7 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF,
       mnVisible(1),
       mnFound(1),
       mbBad(false),
-      mpReplaced(static_cast<MapPoint*>(NULL)),
+      mpReplaced(nullptr),
       mfMinDistance(0),
       mfMaxDistance(0),
       mpMap(pMap),
@@ -91,8 +92,10 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, KeyFrame* pRefKF,
   mnId = nNextId++;
 }
 
-MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF,
-                   KeyFrame* pHostKF, const std::shared_ptr<Map>& pMap)
+MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init,
+                   const std::shared_ptr<KeyFrame>& pRefKF,
+                   const std::shared_ptr<KeyFrame>& pHostKF,
+                   const std::shared_ptr<Map>& pMap)
     : mnFirstKFid(pRefKF->mnId),
       mnFirstFrame(pRefKF->mnFrameId),
       nObs(0),
@@ -108,7 +111,7 @@ MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF,
       mnVisible(1),
       mnFound(1),
       mbBad(false),
-      mpReplaced(static_cast<MapPoint*>(NULL)),
+      mpReplaced(nullptr),
       mfMinDistance(0),
       mfMaxDistance(0),
       mpMap(pMap),
@@ -128,7 +131,7 @@ MapPoint::MapPoint(const double invDepth, cv::Point2f uv_init, KeyFrame* pRefKF,
 }
 
 MapPoint::MapPoint(const Eigen::Vector3f& Pos, const std::shared_ptr<Map>& pMap,
-                   Frame* pFrame, const int& idxF)
+                   const std::shared_ptr<Frame>& pFrame, const int& idxF)
     : mnFirstKFid(-1),
       mnFirstFrame(pFrame->mnId),
       nObs(0),
@@ -140,7 +143,7 @@ MapPoint::MapPoint(const Eigen::Vector3f& Pos, const std::shared_ptr<Map>& pMap,
       mnCorrectedByKF(0),
       mnCorrectedReference(0),
       mnBAGlobalForKF(0),
-      mpRefKF(static_cast<KeyFrame*>(NULL)),
+      mpRefKF(nullptr),
       mnVisible(1),
       mnFound(1),
       mbBad(false),
@@ -197,12 +200,12 @@ Eigen::Vector3f MapPoint::GetNormal() {
   return mNormalVector;
 }
 
-KeyFrame* MapPoint::GetReferenceKeyFrame() {
+std::shared_ptr<KeyFrame> MapPoint::GetReferenceKeyFrame() {
   unique_lock<mutex> lock(mMutexFeatures);
   return mpRefKF;
 }
 
-void MapPoint::AddObservation(KeyFrame* pKF, int idx) {
+void MapPoint::AddObservation(const std::shared_ptr<KeyFrame>& pKF, int idx) {
   unique_lock<mutex> lock(mMutexFeatures);
   tuple<int, int> indexes;
 
@@ -226,7 +229,7 @@ void MapPoint::AddObservation(KeyFrame* pKF, int idx) {
     nObs++;
 }
 
-void MapPoint::EraseObservation(KeyFrame* pKF) {
+void MapPoint::EraseObservation(const std::shared_ptr<KeyFrame>& pKF) {
   bool bBad = false;
   {
     unique_lock<mutex> lock(mMutexFeatures);
@@ -256,7 +259,8 @@ void MapPoint::EraseObservation(KeyFrame* pKF) {
   if (bBad) SetBadFlag();
 }
 
-std::map<KeyFrame*, std::tuple<int, int>> MapPoint::GetObservations() {
+std::map<std::shared_ptr<KeyFrame>, std::tuple<int, int>>
+MapPoint::GetObservations() {
   unique_lock<mutex> lock(mMutexFeatures);
   return mObservations;
 }
@@ -267,7 +271,7 @@ int MapPoint::Observations() {
 }
 
 void MapPoint::SetBadFlag() {
-  map<KeyFrame*, tuple<int, int>> obs;
+  map<std::shared_ptr<KeyFrame>, tuple<int, int>> obs;
   {
     unique_lock<mutex> lock1(mMutexFeatures);
     unique_lock<mutex> lock2(mMutexPos);
@@ -275,10 +279,11 @@ void MapPoint::SetBadFlag() {
     obs = mObservations;
     mObservations.clear();
   }
-  for (map<KeyFrame*, tuple<int, int>>::iterator mit = obs.begin(),
-                                                 mend = obs.end();
+  for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::iterator
+           mit = obs.begin(),
+           mend = obs.end();
        mit != mend; mit++) {
-    KeyFrame* pKF = mit->first;
+    std::shared_ptr<KeyFrame> pKF = mit->first;
     int leftIndex = get<0>(mit->second), rightIndex = get<1>(mit->second);
     if (leftIndex != -1) {
       pKF->EraseMapPointMatch(leftIndex);
@@ -301,7 +306,7 @@ void MapPoint::Replace(MapPoint* pMP) {
   if (pMP->mnId == this->mnId) return;
 
   int nvisible, nfound;
-  map<KeyFrame*, tuple<int, int>> obs;
+  map<std::shared_ptr<KeyFrame>, tuple<int, int>> obs;
   {
     unique_lock<mutex> lock1(mMutexFeatures);
     unique_lock<mutex> lock2(mMutexPos);
@@ -313,11 +318,12 @@ void MapPoint::Replace(MapPoint* pMP) {
     mpReplaced = pMP;
   }
 
-  for (map<KeyFrame*, tuple<int, int>>::iterator mit = obs.begin(),
-                                                 mend = obs.end();
+  for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::iterator
+           mit = obs.begin(),
+           mend = obs.end();
        mit != mend; mit++) {
     // Replace measurement in keyframe
-    KeyFrame* pKF = mit->first;
+    std::shared_ptr<KeyFrame> pKF = mit->first;
 
     tuple<int, int> indexes = mit->second;
     int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
@@ -374,7 +380,7 @@ void MapPoint::ComputeDistinctiveDescriptors() {
   // Retrieve all observed descriptors
   vector<cv::Mat> vDescriptors;
 
-  map<KeyFrame*, tuple<int, int>> observations;
+  map<std::shared_ptr<KeyFrame>, tuple<int, int>> observations;
 
   {
     unique_lock<mutex> lock1(mMutexFeatures);
@@ -386,10 +392,11 @@ void MapPoint::ComputeDistinctiveDescriptors() {
 
   vDescriptors.reserve(observations.size());
 
-  for (map<KeyFrame*, tuple<int, int>>::iterator mit = observations.begin(),
-                                                 mend = observations.end();
+  for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::iterator
+           mit = observations.begin(),
+           mend = observations.end();
        mit != mend; mit++) {
-    KeyFrame* pKF = mit->first;
+    std::shared_ptr<KeyFrame> pKF = mit->first;
 
     if (!pKF->isBad()) {
       tuple<int, int> indexes = mit->second;
@@ -445,7 +452,8 @@ cv::Mat MapPoint::GetDescriptor() {
   return mDescriptor.clone();
 }
 
-tuple<int, int> MapPoint::GetIndexInKeyFrame(KeyFrame* pKF) {
+tuple<int, int> MapPoint::GetIndexInKeyFrame(
+    const std::shared_ptr<KeyFrame>& pKF) {
   unique_lock<mutex> lock(mMutexFeatures);
   if (mObservations.count(pKF))
     return mObservations[pKF];
@@ -453,14 +461,14 @@ tuple<int, int> MapPoint::GetIndexInKeyFrame(KeyFrame* pKF) {
     return tuple<int, int>(-1, -1);
 }
 
-bool MapPoint::IsInKeyFrame(KeyFrame* pKF) {
+bool MapPoint::IsInKeyFrame(const std::shared_ptr<KeyFrame>& pKF) {
   unique_lock<mutex> lock(mMutexFeatures);
   return (mObservations.count(pKF));
 }
 
 void MapPoint::UpdateNormalAndDepth() {
-  map<KeyFrame*, tuple<int, int>> observations;
-  KeyFrame* pRefKF;
+  map<std::shared_ptr<KeyFrame>, tuple<int, int>> observations;
+  std::shared_ptr<KeyFrame> pRefKF;
   Eigen::Vector3f Pos;
   {
     unique_lock<mutex> lock1(mMutexFeatures);
@@ -476,10 +484,11 @@ void MapPoint::UpdateNormalAndDepth() {
   Eigen::Vector3f normal;
   normal.setZero();
   int n = 0;
-  for (map<KeyFrame*, tuple<int, int>>::iterator mit = observations.begin(),
-                                                 mend = observations.end();
+  for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::iterator
+           mit = observations.begin(),
+           mend = observations.end();
        mit != mend; mit++) {
-    KeyFrame* pKF = mit->first;
+    std::shared_ptr<KeyFrame> pKF = mit->first;
 
     tuple<int, int> indexes = mit->second;
     int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
@@ -539,7 +548,8 @@ float MapPoint::GetMaxDistanceInvariance() {
   return 1.2f * mfMaxDistance;
 }
 
-int MapPoint::PredictScale(const float& currentDist, KeyFrame* pKF) {
+int MapPoint::PredictScale(const float& currentDist,
+                           const std::shared_ptr<KeyFrame>& pKF) {
   float ratio;
   {
     unique_lock<mutex> lock(mMutexPos);
@@ -555,7 +565,8 @@ int MapPoint::PredictScale(const float& currentDist, KeyFrame* pKF) {
   return nScale;
 }
 
-int MapPoint::PredictScale(const float& currentDist, Frame* pF) {
+int MapPoint::PredictScale(const float& currentDist,
+                           const std::shared_ptr<Frame>& pF) {
   float ratio;
   {
     unique_lock<mutex> lock(mMutexPos);
@@ -573,11 +584,7 @@ int MapPoint::PredictScale(const float& currentDist, Frame* pF) {
 
 void MapPoint::PrintObservations() {
   cout << "MP_OBS: MP " << mnId << endl;
-  for (map<KeyFrame*, tuple<int, int>>::iterator mit = mObservations.begin(),
-                                                 mend = mObservations.end();
-       mit != mend; mit++) {
-    KeyFrame* pKFi = mit->first;
-    tuple<int, int> indexes = mit->second;
+  for (auto const& [pKFi, indexes] : mObservations) {
     int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
     cout << "--OBS in KF " << pKFi->mnId << " in map "
          << pKFi->GetMap()->GetId() << endl;
@@ -594,7 +601,8 @@ void MapPoint::UpdateMap(const std::shared_ptr<Map>& pMap) {
   mpMap = pMap;
 }
 
-void MapPoint::PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP) {
+void MapPoint::PreSave(set<std::shared_ptr<KeyFrame>>& spKF,
+                       set<MapPoint*>& spMP) {
   mBackupReplacedId = -1;
   if (mpReplaced && spMP.find(mpReplaced) != spMP.end())
     mBackupReplacedId = mpReplaced->mnId;
@@ -602,11 +610,11 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP) {
   mBackupObservationsId1.clear();
   mBackupObservationsId2.clear();
   // Save the id and position in each KF who view it
-  for (std::map<KeyFrame*, std::tuple<int, int>>::const_iterator
+  for (std::map<std::shared_ptr<KeyFrame>, std::tuple<int, int>>::const_iterator
            it = mObservations.begin(),
            end = mObservations.end();
        it != end; ++it) {
-    KeyFrame* pKFi = it->first;
+    std::shared_ptr<KeyFrame> pKFi = it->first;
     if (spKF.find(pKFi) != spKF.end()) {
       mBackupObservationsId1[it->first->mnId] = get<0>(it->second);
       mBackupObservationsId2[it->first->mnId] = get<1>(it->second);
@@ -621,8 +629,9 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF, set<MapPoint*>& spMP) {
   }
 }
 
-void MapPoint::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
-                        map<long unsigned int, MapPoint*>& mpMPid) {
+void MapPoint::PostLoad(
+    map<long unsigned int, std::shared_ptr<KeyFrame>>& mpKFid,
+    map<long unsigned int, MapPoint*>& mpMPid) {
   mpRefKF = mpKFid[mBackupRefKFId];
   if (!mpRefKF) {
     cout << "ERROR: MP without KF reference " << mBackupRefKFId
@@ -641,7 +650,7 @@ void MapPoint::PostLoad(map<long unsigned int, KeyFrame*>& mpKFid,
            it = mBackupObservationsId1.begin(),
            end = mBackupObservationsId1.end();
        it != end; ++it) {
-    KeyFrame* pKFi = mpKFid[it->first];
+    std::shared_ptr<KeyFrame> pKFi = mpKFid[it->first];
     map<long unsigned int, int>::const_iterator it2 =
         mBackupObservationsId2.find(it->first);
     std::tuple<int, int> indexes = tuple<int, int>(it->second, it2->second);
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index 28aa854ea90..abbb3cb040f 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -42,7 +42,7 @@ const int ORBmatcher::HISTO_LENGTH = 30;
 ORBmatcher::ORBmatcher(float nnratio, bool checkOri)
     : mfNNratio(nnratio), mbCheckOrientation(checkOri) {}
 
-int ORBmatcher::SearchByProjection(Frame &F,
+int ORBmatcher::SearchByProjection(const std::shared_ptr<Frame> &F,
                                    const vector<MapPoint *> &vpMapPoints,
                                    const float th, const bool bFarPoints,
                                    const float thFarPoints) {
@@ -67,9 +67,9 @@ int ORBmatcher::SearchByProjection(Frame &F,
       if (bFactor) r *= th;
 
       const vector<size_t> vIndices =
-          F.GetFeaturesInArea(pMP->mTrackProjX, pMP->mTrackProjY,
-                              r * F.mvScaleFactors[nPredictedLevel],
-                              nPredictedLevel - 1, nPredictedLevel);
+          F->GetFeaturesInArea(pMP->mTrackProjX, pMP->mTrackProjY,
+                               r * F->mvScaleFactors[nPredictedLevel],
+                               nPredictedLevel - 1, nPredictedLevel);
 
       if (!vIndices.empty()) {
         const cv::Mat MPdescriptor = pMP->GetDescriptor();
@@ -86,15 +86,15 @@ int ORBmatcher::SearchByProjection(Frame &F,
              vit != vend; vit++) {
           const size_t idx = *vit;
 
-          if (F.mvpMapPoints[idx])
-            if (F.mvpMapPoints[idx]->Observations() > 0) continue;
+          if (F->mvpMapPoints[idx])
+            if (F->mvpMapPoints[idx]->Observations() > 0) continue;
 
-          if (F.Nleft == -1 && F.mvuRight[idx] > 0) {
-            const float er = fabs(pMP->mTrackProjXR - F.mvuRight[idx]);
-            if (er > r * F.mvScaleFactors[nPredictedLevel]) continue;
+          if (F->Nleft == -1 && F->mvuRight[idx] > 0) {
+            const float er = fabs(pMP->mTrackProjXR - F->mvuRight[idx]);
+            if (er > r * F->mvScaleFactors[nPredictedLevel]) continue;
           }
 
-          const cv::Mat &d = F.mDescriptors.row(idx);
+          const cv::Mat &d = F->mDescriptors.row(idx);
 
           const int dist = DescriptorDistance(MPdescriptor, d);
 
@@ -102,15 +102,16 @@ int ORBmatcher::SearchByProjection(Frame &F,
             bestDist2 = bestDist;
             bestDist = dist;
             bestLevel2 = bestLevel;
-            bestLevel = (F.Nleft == -1)   ? F.mvKeysUn[idx].octave
-                        : (idx < F.Nleft) ? F.mvKeys[idx].octave
-                                          : F.mvKeysRight[idx - F.Nleft].octave;
+            bestLevel = (F->Nleft == -1) ? F->mvKeysUn[idx].octave
+                        : (idx < F->Nleft)
+                            ? F->mvKeys[idx].octave
+                            : F->mvKeysRight[idx - F->Nleft].octave;
             bestIdx = idx;
           } else if (dist < bestDist2) {
-            bestLevel2 = (F.Nleft == -1) ? F.mvKeysUn[idx].octave
-                         : (idx < F.Nleft)
-                             ? F.mvKeys[idx].octave
-                             : F.mvKeysRight[idx - F.Nleft].octave;
+            bestLevel2 = (F->Nleft == -1) ? F->mvKeysUn[idx].octave
+                         : (idx < F->Nleft)
+                             ? F->mvKeys[idx].octave
+                             : F->mvKeysRight[idx - F->Nleft].octave;
             bestDist2 = dist;
           }
         }
@@ -122,12 +123,12 @@ int ORBmatcher::SearchByProjection(Frame &F,
             continue;
 
           if (bestLevel != bestLevel2 || bestDist <= mfNNratio * bestDist2) {
-            F.mvpMapPoints[bestIdx] = pMP;
+            F->mvpMapPoints[bestIdx] = pMP;
 
-            if (F.Nleft != -1 && F.mvLeftToRightMatch[bestIdx] !=
-                                     -1) {  // Also match with the stereo
-                                            // observation at right camera
-              F.mvpMapPoints[F.mvLeftToRightMatch[bestIdx] + F.Nleft] = pMP;
+            if (F->Nleft != -1 && F->mvLeftToRightMatch[bestIdx] !=
+                                      -1) {  // Also match with the stereo
+                                             // observation at right camera
+              F->mvpMapPoints[F->mvLeftToRightMatch[bestIdx] + F->Nleft] = pMP;
               nmatches++;
               right++;
             }
@@ -139,15 +140,15 @@ int ORBmatcher::SearchByProjection(Frame &F,
       }
     }
 
-    if (F.Nleft != -1 && pMP->mbTrackInViewR) {
+    if (F->Nleft != -1 && pMP->mbTrackInViewR) {
       const int &nPredictedLevel = pMP->mnTrackScaleLevelR;
       if (nPredictedLevel != -1) {
         float r = RadiusByViewingCos(pMP->mTrackViewCosR);
 
         const vector<size_t> vIndices =
-            F.GetFeaturesInArea(pMP->mTrackProjXR, pMP->mTrackProjYR,
-                                r * F.mvScaleFactors[nPredictedLevel],
-                                nPredictedLevel - 1, nPredictedLevel, true);
+            F->GetFeaturesInArea(pMP->mTrackProjXR, pMP->mTrackProjYR,
+                                 r * F->mvScaleFactors[nPredictedLevel],
+                                 nPredictedLevel - 1, nPredictedLevel, true);
 
         if (vIndices.empty()) continue;
 
@@ -165,10 +166,10 @@ int ORBmatcher::SearchByProjection(Frame &F,
              vit != vend; vit++) {
           const size_t idx = *vit;
 
-          if (F.mvpMapPoints[idx + F.Nleft])
-            if (F.mvpMapPoints[idx + F.Nleft]->Observations() > 0) continue;
+          if (F->mvpMapPoints[idx + F->Nleft])
+            if (F->mvpMapPoints[idx + F->Nleft]->Observations() > 0) continue;
 
-          const cv::Mat &d = F.mDescriptors.row(idx + F.Nleft);
+          const cv::Mat &d = F->mDescriptors.row(idx + F->Nleft);
 
           const int dist = DescriptorDistance(MPdescriptor, d);
 
@@ -176,10 +177,10 @@ int ORBmatcher::SearchByProjection(Frame &F,
             bestDist2 = bestDist;
             bestDist = dist;
             bestLevel2 = bestLevel;
-            bestLevel = F.mvKeysRight[idx].octave;
+            bestLevel = F->mvKeysRight[idx].octave;
             bestIdx = idx;
           } else if (dist < bestDist2) {
-            bestLevel2 = F.mvKeysRight[idx].octave;
+            bestLevel2 = F->mvKeysRight[idx].octave;
             bestDist2 = dist;
           }
         }
@@ -190,15 +191,15 @@ int ORBmatcher::SearchByProjection(Frame &F,
           if (bestLevel == bestLevel2 && bestDist > mfNNratio * bestDist2)
             continue;
 
-          if (F.Nleft != -1 && F.mvRightToLeftMatch[bestIdx] !=
-                                   -1) {  // Also match with the stereo
-                                          // observation at right camera
-            F.mvpMapPoints[F.mvRightToLeftMatch[bestIdx]] = pMP;
+          if (F->Nleft != -1 && F->mvRightToLeftMatch[bestIdx] !=
+                                    -1) {  // Also match with the stereo
+                                           // observation at right camera
+            F->mvpMapPoints[F->mvRightToLeftMatch[bestIdx]] = pMP;
             nmatches++;
             left++;
           }
 
-          F.mvpMapPoints[bestIdx + F.Nleft] = pMP;
+          F->mvpMapPoints[bestIdx + F->Nleft] = pMP;
           nmatches++;
           right++;
         }
@@ -215,11 +216,12 @@ float ORBmatcher::RadiusByViewingCos(const float &viewCos) {
     return 4.0;
 }
 
-int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
+int ORBmatcher::SearchByBoW(const std::shared_ptr<KeyFrame> &pKF,
+                            const std::shared_ptr<Frame> &F,
                             vector<MapPoint *> &vpMapPointMatches) {
   const vector<MapPoint *> vpMapPointsKF = pKF->GetMapPointMatches();
 
-  vpMapPointMatches = vector<MapPoint *>(F.N, static_cast<MapPoint *>(NULL));
+  vpMapPointMatches = vector<MapPoint *>(F->N, static_cast<MapPoint *>(NULL));
 
   const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;
 
@@ -232,13 +234,13 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
   // We perform the matching over ORB that belong to the same vocabulary node
   // (at a certain level)
   DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
-  DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
+  DBoW2::FeatureVector::const_iterator Fit = F->mFeatVec.begin();
   DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
-  DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();
+  DBoW2::FeatureVector::const_iterator Fend = F->mFeatVec.end();
 
   spdlog::info(
       "[ORBmatcher::SearchByBoW] {} feature vector in Keyframe, {} in Frame",
-      vFeatVecKF.size(), F.mFeatVec.size());
+      vFeatVecKF.size(), F->mFeatVec.size());
 
   while (KFit != KFend && Fit != Fend) {
     if (KFit->first == Fit->first) {
@@ -271,12 +273,12 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
         int bestDist2R = 256;
 
         for (size_t iF = 0; iF < vIndicesF.size(); iF++) {
-          if (F.Nleft == -1) {
+          if (F->Nleft == -1) {
             const unsigned int realIdxF = vIndicesF[iF];
 
             if (vpMapPointMatches[realIdxF]) continue;
 
-            const cv::Mat &dF = F.mDescriptors.row(realIdxF);
+            const cv::Mat &dF = F->mDescriptors.row(realIdxF);
 
             const int dist = DescriptorDistance(dKF, dF);
 
@@ -292,23 +294,23 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
 
             if (vpMapPointMatches[realIdxF]) continue;
 
-            const cv::Mat &dF = F.mDescriptors.row(realIdxF);
+            const cv::Mat &dF = F->mDescriptors.row(realIdxF);
 
             const int dist = DescriptorDistance(dKF, dF);
 
-            if (realIdxF < F.Nleft && dist < bestDist1) {
+            if (realIdxF < F->Nleft && dist < bestDist1) {
               bestDist2 = bestDist1;
               bestDist1 = dist;
               bestIdxF = realIdxF;
-            } else if (realIdxF < F.Nleft && dist < bestDist2) {
+            } else if (realIdxF < F->Nleft && dist < bestDist2) {
               bestDist2 = dist;
             }
 
-            if (realIdxF >= F.Nleft && dist < bestDist1R) {
+            if (realIdxF >= F->Nleft && dist < bestDist1R) {
               bestDist2R = bestDist1R;
               bestDist1R = dist;
               bestIdxFR = realIdxF;
-            } else if (realIdxF >= F.Nleft && dist < bestDist2R) {
+            } else if (realIdxF >= F->Nleft && dist < bestDist2R) {
               bestDist2R = dist;
             }
           }
@@ -327,9 +329,9 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
 
             if (mbCheckOrientation) {
               cv::KeyPoint &Fkp =
-                  (!pKF->mpCamera2 || F.Nleft == -1) ? F.mvKeys[bestIdxF]
-                  : (bestIdxF >= F.Nleft) ? F.mvKeysRight[bestIdxF - F.Nleft]
-                                          : F.mvKeys[bestIdxF];
+                  (!pKF->mpCamera2 || F->Nleft == -1) ? F->mvKeys[bestIdxF]
+                  : (bestIdxF >= F->Nleft) ? F->mvKeysRight[bestIdxF - F->Nleft]
+                                           : F->mvKeys[bestIdxF];
 
               float rot = kp.angle - Fkp.angle;
               if (rot < 0.0) rot += 360.0f;
@@ -354,10 +356,10 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
                       : pKF->mvKeys[realIdxKF];
 
               if (mbCheckOrientation) {
-                cv::KeyPoint &Fkp = (!F.mpCamera2) ? F.mvKeys[bestIdxFR]
-                                    : (bestIdxFR >= F.Nleft)
-                                        ? F.mvKeysRight[bestIdxFR - F.Nleft]
-                                        : F.mvKeys[bestIdxFR];
+                cv::KeyPoint &Fkp = (!F->mpCamera2) ? F->mvKeys[bestIdxFR]
+                                    : (bestIdxFR >= F->Nleft)
+                                        ? F->mvKeysRight[bestIdxFR - F->Nleft]
+                                        : F->mvKeys[bestIdxFR];
 
                 float rot = kp.angle - Fkp.angle;
                 if (rot < 0.0) rot += 360.0f;
@@ -377,7 +379,7 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
     } else if (KFit->first < Fit->first) {
       KFit = vFeatVecKF.lower_bound(Fit->first);
     } else {
-      Fit = F.mFeatVec.lower_bound(KFit->first);
+      Fit = F->mFeatVec.lower_bound(KFit->first);
     }
   }
 
@@ -405,7 +407,8 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF, Frame &F,
   return nmatches;
 }
 
-int ORBmatcher::SearchByProjection(KeyFrame *pKF, Sophus::Sim3f &Scw,
+int ORBmatcher::SearchByProjection(const std::shared_ptr<KeyFrame> &pKF,
+                                   Sophus::Sim3f &Scw,
                                    const vector<MapPoint *> &vpPoints,
                                    vector<MapPoint *> &vpMatched, int th,
                                    float ratioHamming) {
@@ -504,12 +507,13 @@ int ORBmatcher::SearchByProjection(KeyFrame *pKF, Sophus::Sim3f &Scw,
   return nmatches;
 }
 
-int ORBmatcher::SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
-                                   const std::vector<MapPoint *> &vpPoints,
-                                   const std::vector<KeyFrame *> &vpPointsKFs,
-                                   std::vector<MapPoint *> &vpMatched,
-                                   std::vector<KeyFrame *> &vpMatchedKF, int th,
-                                   float ratioHamming) {
+int ORBmatcher::SearchByProjection(
+    const std::shared_ptr<KeyFrame> &pKF, Sophus::Sim3<float> &Scw,
+    const std::vector<MapPoint *> &vpPoints,
+    const std::vector<std::shared_ptr<KeyFrame>> &vpPointsKFs,
+    std::vector<MapPoint *> &vpMatched,
+    std::vector<std::shared_ptr<KeyFrame>> &vpMatchedKF, int th,
+    float ratioHamming) {
   // Get Calibration Parameters for later projection
   const float &fx = pKF->fx;
   const float &fy = pKF->fy;
@@ -529,7 +533,7 @@ int ORBmatcher::SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
   // For each Candidate MapPoint Project and Match
   for (int iMP = 0, iendMP = vpPoints.size(); iMP < iendMP; iMP++) {
     MapPoint *pMP = vpPoints[iMP];
-    KeyFrame *pKFi = vpPointsKFs[iMP];
+    std::shared_ptr<KeyFrame> pKFi = vpPointsKFs[iMP];
 
     // Discard Bad MapPoints and already found
     if (pMP->isBad() || spAlreadyFound.count(pMP)) continue;
@@ -611,31 +615,32 @@ int ORBmatcher::SearchByProjection(KeyFrame *pKF, Sophus::Sim3<float> &Scw,
   return nmatches;
 }
 
-int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2,
+int ORBmatcher::SearchForInitialization(const std::shared_ptr<Frame> &F1,
+                                        const std::shared_ptr<Frame> &F2,
                                         vector<cv::Point2f> &vbPrevMatched,
                                         vector<int> &vnMatches12,
                                         int windowSize) {
   int nmatches = 0;
-  vnMatches12 = vector<int>(F1.mvKeysUn.size(), -1);
+  vnMatches12 = vector<int>(F1->mvKeysUn.size(), -1);
 
   vector<int> rotHist[HISTO_LENGTH];
   for (int i = 0; i < HISTO_LENGTH; i++) rotHist[i].reserve(500);
   const float factor = 1.0f / HISTO_LENGTH;
 
-  vector<int> vMatchedDistance(F2.mvKeysUn.size(), INT_MAX);
-  vector<int> vnMatches21(F2.mvKeysUn.size(), -1);
+  vector<int> vMatchedDistance(F2->mvKeysUn.size(), INT_MAX);
+  vector<int> vnMatches21(F2->mvKeysUn.size(), -1);
 
-  for (size_t i1 = 0, iend1 = F1.mvKeysUn.size(); i1 < iend1; i1++) {
-    cv::KeyPoint kp1 = F1.mvKeysUn[i1];
+  for (size_t i1 = 0, iend1 = F1->mvKeysUn.size(); i1 < iend1; i1++) {
+    cv::KeyPoint kp1 = F1->mvKeysUn[i1];
     int level1 = kp1.octave;
     if (level1 > 0) continue;
 
-    vector<size_t> vIndices2 = F2.GetFeaturesInArea(
+    vector<size_t> vIndices2 = F2->GetFeaturesInArea(
         vbPrevMatched[i1].x, vbPrevMatched[i1].y, windowSize, level1, level1);
 
     if (vIndices2.empty()) continue;
 
-    cv::Mat d1 = F1.mDescriptors.row(i1);
+    cv::Mat d1 = F1->mDescriptors.row(i1);
 
     int bestDist = INT_MAX;
     int bestDist2 = INT_MAX;
@@ -645,7 +650,7 @@ int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2,
          vit != vIndices2.end(); vit++) {
       size_t i2 = *vit;
 
-      cv::Mat d2 = F2.mDescriptors.row(i2);
+      cv::Mat d2 = F2->mDescriptors.row(i2);
 
       int dist = DescriptorDistance(d1, d2);
 
@@ -672,7 +677,7 @@ int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2,
         nmatches++;
 
         if (mbCheckOrientation) {
-          float rot = F1.mvKeysUn[i1].angle - F2.mvKeysUn[bestIdx2].angle;
+          float rot = F1->mvKeysUn[i1].angle - F2->mvKeysUn[bestIdx2].angle;
           if (rot < 0.0) rot += 360.0f;
           int bin = round(rot * factor);
           if (bin == HISTO_LENGTH) bin = 0;
@@ -705,12 +710,13 @@ int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2,
   // Update prev matched
   for (size_t i1 = 0, iend1 = vnMatches12.size(); i1 < iend1; i1++)
     if (vnMatches12[i1] >= 0)
-      vbPrevMatched[i1] = F2.mvKeysUn[vnMatches12[i1]].pt;
+      vbPrevMatched[i1] = F2->mvKeysUn[vnMatches12[i1]].pt;
 
   return nmatches;
 }
 
-int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2,
+int ORBmatcher::SearchByBoW(const std::shared_ptr<KeyFrame> &pKF1,
+                            const std::shared_ptr<KeyFrame> &pKF2,
                             vector<MapPoint *> &vpMatches12) {
   const vector<cv::KeyPoint> &vKeysUn1 = pKF1->mvKeysUn;
   const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
@@ -830,8 +836,9 @@ int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2,
 }
 
 int ORBmatcher::SearchForTriangulation(
-    KeyFrame *pKF1, KeyFrame *pKF2,
-    vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo,
+    const std::shared_ptr<KeyFrame> &pKF1,
+    const std::shared_ptr<KeyFrame> &pKF2,
+    vector<pair<size_t, size_t>> &vMatchedPairs, const bool bOnlyStereo,
     const bool bCoarse) {
   const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
   const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
@@ -1050,8 +1057,9 @@ int ORBmatcher::SearchForTriangulation(
   return nmatches;
 }
 
-int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints,
-                     const float th, const bool bRight) {
+int ORBmatcher::Fuse(const std::shared_ptr<KeyFrame> &pKF,
+                     const vector<MapPoint *> &vpMapPoints, const float th,
+                     const bool bRight) {
   std::shared_ptr<GeometricCamera> pCamera;
   Sophus::SE3f Tcw;
   Eigen::Vector3f Ow;
@@ -1223,7 +1231,7 @@ int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints,
   return nFused;
 }
 
-int ORBmatcher::Fuse(KeyFrame *pKF, Sophus::Sim3f &Scw,
+int ORBmatcher::Fuse(const std::shared_ptr<KeyFrame> &pKF, Sophus::Sim3f &Scw,
                      const vector<MapPoint *> &vpPoints, float th,
                      vector<MapPoint *> &vpReplacePoint) {
   // Get Calibration Parameters for later projection
@@ -1329,7 +1337,8 @@ int ORBmatcher::Fuse(KeyFrame *pKF, Sophus::Sim3f &Scw,
   return nFused;
 }
 
-int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2,
+int ORBmatcher::SearchBySim3(const std::shared_ptr<KeyFrame> &pKF1,
+                             const std::shared_ptr<KeyFrame> &pKF2,
                              std::vector<MapPoint *> &vpMatches12,
                              const Sophus::Sim3f &S12, const float th) {
   const float &fx = pKF1->fx;
@@ -1527,7 +1536,8 @@ int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2,
   return nFound;
 }
 
-int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
+int ORBmatcher::SearchByProjection(const std::shared_ptr<Frame> &CurrentFrame,
+                                   const std::shared_ptr<Frame> &LastFrame,
                                    const float th, const bool bMono) {
   int nmatches = 0;
 
@@ -1536,19 +1546,19 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
   for (int i = 0; i < HISTO_LENGTH; i++) rotHist[i].reserve(500);
   const float factor = 1.0f / HISTO_LENGTH;
 
-  const Sophus::SE3f Tcw = CurrentFrame.GetPose();
+  const Sophus::SE3f Tcw = CurrentFrame->GetPose();
   const Eigen::Vector3f twc = Tcw.inverse().translation();
 
-  const Sophus::SE3f Tlw = LastFrame.GetPose();
+  const Sophus::SE3f Tlw = LastFrame->GetPose();
   const Eigen::Vector3f tlc = Tlw * twc;
 
-  const bool bForward = tlc(2) > CurrentFrame.mb && !bMono;
-  const bool bBackward = -tlc(2) > CurrentFrame.mb && !bMono;
+  const bool bForward = tlc(2) > CurrentFrame->mb && !bMono;
+  const bool bBackward = -tlc(2) > CurrentFrame->mb && !bMono;
 
-  for (int i = 0; i < LastFrame.N; i++) {
-    MapPoint *pMP = LastFrame.mvpMapPoints[i];
+  for (int i = 0; i < LastFrame->N; i++) {
+    MapPoint *pMP = LastFrame->mvpMapPoints[i];
     if (pMP) {
-      if (!LastFrame.mvbOutlier[i]) {
+      if (!LastFrame->mvbOutlier[i]) {
         // Project
         Eigen::Vector3f x3Dw = pMP->GetWorldPos();
         Eigen::Vector3f x3Dc = Tcw * x3Dw;
@@ -1559,31 +1569,31 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
 
         if (invzc < 0) continue;
 
-        Eigen::Vector2f uv = CurrentFrame.mpCamera->project(x3Dc);
+        Eigen::Vector2f uv = CurrentFrame->mpCamera->project(x3Dc);
 
-        if (uv(0) < CurrentFrame.mnMinX || uv(0) > CurrentFrame.mnMaxX)
+        if (uv(0) < CurrentFrame->mnMinX || uv(0) > CurrentFrame->mnMaxX)
           continue;
-        if (uv(1) < CurrentFrame.mnMinY || uv(1) > CurrentFrame.mnMaxY)
+        if (uv(1) < CurrentFrame->mnMinY || uv(1) > CurrentFrame->mnMaxY)
           continue;
 
         int nLastOctave =
-            (LastFrame.Nleft == -1 || i < LastFrame.Nleft)
-                ? LastFrame.mvKeys[i].octave
-                : LastFrame.mvKeysRight[i - LastFrame.Nleft].octave;
+            (LastFrame->Nleft == -1 || i < LastFrame->Nleft)
+                ? LastFrame->mvKeys[i].octave
+                : LastFrame->mvKeysRight[i - LastFrame->Nleft].octave;
 
         // Search in a window. Size depends on scale
-        float radius = th * CurrentFrame.mvScaleFactors[nLastOctave];
+        float radius = th * CurrentFrame->mvScaleFactors[nLastOctave];
 
         vector<size_t> vIndices2;
 
         if (bForward)
-          vIndices2 =
-              CurrentFrame.GetFeaturesInArea(uv(0), uv(1), radius, nLastOctave);
+          vIndices2 = CurrentFrame->GetFeaturesInArea(uv(0), uv(1), radius,
+                                                      nLastOctave);
         else if (bBackward)
-          vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0), uv(1), radius, 0,
-                                                     nLastOctave);
+          vIndices2 = CurrentFrame->GetFeaturesInArea(uv(0), uv(1), radius, 0,
+                                                      nLastOctave);
         else
-          vIndices2 = CurrentFrame.GetFeaturesInArea(
+          vIndices2 = CurrentFrame->GetFeaturesInArea(
               uv(0), uv(1), radius, nLastOctave - 1, nLastOctave + 1);
 
         if (vIndices2.empty()) continue;
@@ -1598,16 +1608,16 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
              vit != vend; vit++) {
           const size_t i2 = *vit;
 
-          if (CurrentFrame.mvpMapPoints[i2])
-            if (CurrentFrame.mvpMapPoints[i2]->Observations() > 0) continue;
+          if (CurrentFrame->mvpMapPoints[i2])
+            if (CurrentFrame->mvpMapPoints[i2]->Observations() > 0) continue;
 
-          if (CurrentFrame.Nleft == -1 && CurrentFrame.mvuRight[i2] > 0) {
-            const float ur = uv(0) - CurrentFrame.mbf * invzc;
-            const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
+          if (CurrentFrame->Nleft == -1 && CurrentFrame->mvuRight[i2] > 0) {
+            const float ur = uv(0) - CurrentFrame->mbf * invzc;
+            const float er = fabs(ur - CurrentFrame->mvuRight[i2]);
             if (er > radius) continue;
           }
 
-          const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);
+          const cv::Mat &d = CurrentFrame->mDescriptors.row(i2);
 
           const int dist = DescriptorDistance(dMP, d);
 
@@ -1618,21 +1628,22 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
         }
 
         if (bestDist <= TH_HIGH) {
-          CurrentFrame.mvpMapPoints[bestIdx2] = pMP;
+          CurrentFrame->mvpMapPoints[bestIdx2] = pMP;
           nmatches++;
 
           if (mbCheckOrientation) {
             cv::KeyPoint kpLF =
-                (LastFrame.Nleft == -1) ? LastFrame.mvKeysUn[i]
-                : (i < LastFrame.Nleft)
-                    ? LastFrame.mvKeys[i]
-                    : LastFrame.mvKeysRight[i - LastFrame.Nleft];
+                (LastFrame->Nleft == -1) ? LastFrame->mvKeysUn[i]
+                : (i < LastFrame->Nleft)
+                    ? LastFrame->mvKeys[i]
+                    : LastFrame->mvKeysRight[i - LastFrame->Nleft];
 
             cv::KeyPoint kpCF =
-                (CurrentFrame.Nleft == -1) ? CurrentFrame.mvKeysUn[bestIdx2]
-                : (bestIdx2 < CurrentFrame.Nleft)
-                    ? CurrentFrame.mvKeys[bestIdx2]
-                    : CurrentFrame.mvKeysRight[bestIdx2 - CurrentFrame.Nleft];
+                (CurrentFrame->Nleft == -1) ? CurrentFrame->mvKeysUn[bestIdx2]
+                : (bestIdx2 < CurrentFrame->Nleft)
+                    ? CurrentFrame->mvKeys[bestIdx2]
+                    : CurrentFrame->mvKeysRight[bestIdx2 - CurrentFrame->Nleft];
+
             float rot = kpLF.angle - kpCF.angle;
             if (rot < 0.0) rot += 360.0f;
             int bin = round(rot * factor);
@@ -1641,28 +1652,28 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
             rotHist[bin].push_back(bestIdx2);
           }
         }
-        if (CurrentFrame.Nleft != -1) {
-          Eigen::Vector3f x3Dr = CurrentFrame.GetRelativePoseTrl() * x3Dc;
-          Eigen::Vector2f uv = CurrentFrame.mpCamera->project(x3Dr);
+        if (CurrentFrame->Nleft != -1) {
+          Eigen::Vector3f x3Dr = CurrentFrame->GetRelativePoseTrl() * x3Dc;
+          Eigen::Vector2f uv = CurrentFrame->mpCamera->project(x3Dr);
 
           int nLastOctave =
-              (LastFrame.Nleft == -1 || i < LastFrame.Nleft)
-                  ? LastFrame.mvKeys[i].octave
-                  : LastFrame.mvKeysRight[i - LastFrame.Nleft].octave;
+              (LastFrame->Nleft == -1 || i < LastFrame->Nleft)
+                  ? LastFrame->mvKeys[i].octave
+                  : LastFrame->mvKeysRight[i - LastFrame->Nleft].octave;
 
           // Search in a window. Size depends on scale
-          float radius = th * CurrentFrame.mvScaleFactors[nLastOctave];
+          float radius = th * CurrentFrame->mvScaleFactors[nLastOctave];
 
           vector<size_t> vIndices2;
 
           if (bForward)
-            vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0), uv(1), radius,
-                                                       nLastOctave, -1, true);
+            vIndices2 = CurrentFrame->GetFeaturesInArea(uv(0), uv(1), radius,
+                                                        nLastOctave, -1, true);
           else if (bBackward)
-            vIndices2 = CurrentFrame.GetFeaturesInArea(uv(0), uv(1), radius, 0,
-                                                       nLastOctave, true);
+            vIndices2 = CurrentFrame->GetFeaturesInArea(uv(0), uv(1), radius, 0,
+                                                        nLastOctave, true);
           else
-            vIndices2 = CurrentFrame.GetFeaturesInArea(
+            vIndices2 = CurrentFrame->GetFeaturesInArea(
                 uv(0), uv(1), radius, nLastOctave - 1, nLastOctave + 1, true);
 
           const cv::Mat dMP = pMP->GetDescriptor();
@@ -1674,13 +1685,13 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
                                               vend = vIndices2.end();
                vit != vend; vit++) {
             const size_t i2 = *vit;
-            if (CurrentFrame.mvpMapPoints[i2 + CurrentFrame.Nleft])
-              if (CurrentFrame.mvpMapPoints[i2 + CurrentFrame.Nleft]
+            if (CurrentFrame->mvpMapPoints[i2 + CurrentFrame->Nleft])
+              if (CurrentFrame->mvpMapPoints[i2 + CurrentFrame->Nleft]
                       ->Observations() > 0)
                 continue;
 
             const cv::Mat &d =
-                CurrentFrame.mDescriptors.row(i2 + CurrentFrame.Nleft);
+                CurrentFrame->mDescriptors.row(i2 + CurrentFrame->Nleft);
 
             const int dist = DescriptorDistance(dMP, d);
 
@@ -1691,23 +1702,23 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
           }
 
           if (bestDist <= TH_HIGH) {
-            CurrentFrame.mvpMapPoints[bestIdx2 + CurrentFrame.Nleft] = pMP;
+            CurrentFrame->mvpMapPoints[bestIdx2 + CurrentFrame->Nleft] = pMP;
             nmatches++;
             if (mbCheckOrientation) {
               cv::KeyPoint kpLF =
-                  (LastFrame.Nleft == -1) ? LastFrame.mvKeysUn[i]
-                  : (i < LastFrame.Nleft)
-                      ? LastFrame.mvKeys[i]
-                      : LastFrame.mvKeysRight[i - LastFrame.Nleft];
+                  (LastFrame->Nleft == -1) ? LastFrame->mvKeysUn[i]
+                  : (i < LastFrame->Nleft)
+                      ? LastFrame->mvKeys[i]
+                      : LastFrame->mvKeysRight[i - LastFrame->Nleft];
 
-              cv::KeyPoint kpCF = CurrentFrame.mvKeysRight[bestIdx2];
+              cv::KeyPoint kpCF = CurrentFrame->mvKeysRight[bestIdx2];
 
               float rot = kpLF.angle - kpCF.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(bestIdx2 + CurrentFrame.Nleft);
+              rotHist[bin].push_back(bestIdx2 + CurrentFrame->Nleft);
             }
           }
         }
@@ -1726,7 +1737,7 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
     for (int i = 0; i < HISTO_LENGTH; i++) {
       if (i != ind1 && i != ind2 && i != ind3) {
         for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) {
-          CurrentFrame.mvpMapPoints[rotHist[i][j]] =
+          CurrentFrame->mvpMapPoints[rotHist[i][j]] =
               static_cast<MapPoint *>(NULL);
           nmatches--;
         }
@@ -1737,12 +1748,13 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame,
   return nmatches;
 }
 
-int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
+int ORBmatcher::SearchByProjection(const std::shared_ptr<Frame> &CurrentFrame,
+                                   const std::shared_ptr<KeyFrame> &pKF,
                                    const set<MapPoint *> &sAlreadyFound,
                                    const float th, const int ORBdist) {
   int nmatches = 0;
 
-  const Sophus::SE3f Tcw = CurrentFrame.GetPose();
+  const Sophus::SE3f Tcw = CurrentFrame->GetPose();
   Eigen::Vector3f Ow = Tcw.inverse().translation();
 
   // Rotation Histogram (to check rotation consistency)
@@ -1761,11 +1773,11 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
         Eigen::Vector3f x3Dw = pMP->GetWorldPos();
         Eigen::Vector3f x3Dc = Tcw * x3Dw;
 
-        const Eigen::Vector2f uv = CurrentFrame.mpCamera->project(x3Dc);
+        const Eigen::Vector2f uv = CurrentFrame->mpCamera->project(x3Dc);
 
-        if (uv(0) < CurrentFrame.mnMinX || uv(0) > CurrentFrame.mnMaxX)
+        if (uv(0) < CurrentFrame->mnMinX || uv(0) > CurrentFrame->mnMaxX)
           continue;
-        if (uv(1) < CurrentFrame.mnMinY || uv(1) > CurrentFrame.mnMaxY)
+        if (uv(1) < CurrentFrame->mnMinY || uv(1) > CurrentFrame->mnMaxY)
           continue;
 
         // Compute predicted scale level
@@ -1778,12 +1790,12 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
         // Depth must be inside the scale pyramid of the image
         if (dist3D < minDistance || dist3D > maxDistance) continue;
 
-        int nPredictedLevel = pMP->PredictScale(dist3D, &CurrentFrame);
+        int nPredictedLevel = pMP->PredictScale(dist3D, CurrentFrame);
 
         // Search in a window
-        const float radius = th * CurrentFrame.mvScaleFactors[nPredictedLevel];
+        const float radius = th * CurrentFrame->mvScaleFactors[nPredictedLevel];
 
-        const vector<size_t> vIndices2 = CurrentFrame.GetFeaturesInArea(
+        const vector<size_t> vIndices2 = CurrentFrame->GetFeaturesInArea(
             uv(0), uv(1), radius, nPredictedLevel - 1, nPredictedLevel + 1);
 
         if (vIndices2.empty()) continue;
@@ -1796,9 +1808,9 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
         for (vector<size_t>::const_iterator vit = vIndices2.begin();
              vit != vIndices2.end(); vit++) {
           const size_t i2 = *vit;
-          if (CurrentFrame.mvpMapPoints[i2]) continue;
+          if (CurrentFrame->mvpMapPoints[i2]) continue;
 
-          const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);
+          const cv::Mat &d = CurrentFrame->mDescriptors.row(i2);
 
           const int dist = DescriptorDistance(dMP, d);
 
@@ -1809,12 +1821,12 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
         }
 
         if (bestDist <= ORBdist) {
-          CurrentFrame.mvpMapPoints[bestIdx2] = pMP;
+          CurrentFrame->mvpMapPoints[bestIdx2] = pMP;
           nmatches++;
 
           if (mbCheckOrientation) {
             float rot =
-                pKF->mvKeysUn[i].angle - CurrentFrame.mvKeysUn[bestIdx2].angle;
+                pKF->mvKeysUn[i].angle - CurrentFrame->mvKeysUn[bestIdx2].angle;
             if (rot < 0.0) rot += 360.0f;
             int bin = round(rot * factor);
             if (bin == HISTO_LENGTH) bin = 0;
@@ -1836,7 +1848,7 @@ int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF,
     for (int i = 0; i < HISTO_LENGTH; i++) {
       if (i != ind1 && i != ind2 && i != ind3) {
         for (size_t j = 0, jend = rotHist[i].size(); j < jend; j++) {
-          CurrentFrame.mvpMapPoints[rotHist[i][j]] = NULL;
+          CurrentFrame->mvpMapPoints[rotHist[i][j]] = NULL;
           nmatches--;
         }
       }
diff --git a/src/Optimizer.cc b/src/Optimizer.cc
index 8f3b73c3b28..0e46f662b91 100644
--- a/src/Optimizer.cc
+++ b/src/Optimizer.cc
@@ -56,12 +56,12 @@ void Optimizer::GlobalBundleAdjustemnt(const std::shared_ptr<Map>& pMap,
                                        int nIterations, bool* pbStopFlag,
                                        const unsigned long nLoopKF,
                                        const bool bRobust) {
-  vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
-  vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
+  auto vpKFs = pMap->GetAllKeyFrames();
+  auto vpMP = pMap->GetAllMapPoints();
   BundleAdjustment(vpKFs, vpMP, nIterations, pbStopFlag, nLoopKF, bRobust);
 }
 
-void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
+void Optimizer::BundleAdjustment(const vector<std::shared_ptr<KeyFrame>>& vpKFs,
                                  const vector<MapPoint*>& vpMP, int nIterations,
                                  bool* pbStopFlag, const unsigned long nLoopKF,
                                  const bool bRobust) {
@@ -100,10 +100,10 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
   vector<ORB_SLAM3::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
   vpEdgesBody.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFMono;
+  vector<std::shared_ptr<KeyFrame>> vpEdgeKFMono;
   vpEdgeKFMono.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFBody;
+  vector<std::shared_ptr<KeyFrame>> vpEdgeKFBody;
   vpEdgeKFBody.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeMono;
@@ -115,7 +115,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
   vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
   vpEdgesStereo.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFStereo;
+  vector<std::shared_ptr<KeyFrame>> vpEdgeKFStereo;
   vpEdgeKFStereo.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeStereo;
@@ -123,8 +123,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
 
   // Set KeyFrame vertices
 
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKF = vpKFs[i];
+  for (auto pKF : vpKFs) {
     if (pKF->isBad()) continue;
     g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
     Sophus::SE3<float> Tcw = pKF->GetPose();
@@ -150,14 +149,15 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
     vPoint->setMarginalized(true);
     optimizer.addVertex(vPoint);
 
-    const map<KeyFrame*, tuple<int, int>> observations = pMP->GetObservations();
+    const map<std::shared_ptr<KeyFrame>, tuple<int, int>> observations =
+        pMP->GetObservations();
 
     int nEdges = 0;
     // SET EDGES
-    for (map<KeyFrame*, tuple<int, int>>::const_iterator mit =
+    for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::const_iterator mit =
              observations.begin();
          mit != observations.end(); mit++) {
-      KeyFrame* pKF = mit->first;
+      std::shared_ptr<KeyFrame> pKF = mit->first;
       if (pKF->isBad() || pKF->mnId > maxKFid) continue;
       if (optimizer.vertex(id) == NULL || optimizer.vertex(pKF->mnId) == NULL)
         continue;
@@ -287,8 +287,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
 
   // Recover optimized data
   // Keyframes
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKF = vpKFs[i];
+  for (auto pKF : vpKFs) {
     if (pKF->isBad()) continue;
     g2o::VertexSE3Expmap* vSE3 =
         static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
@@ -314,7 +313,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
         for (size_t i2 = 0, iend = vpEdgesMono.size(); i2 < iend; i2++) {
           ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i2];
           MapPoint* pMP = vpMapPointEdgeMono[i2];
-          KeyFrame* pKFedge = vpEdgeKFMono[i2];
+          std::shared_ptr<KeyFrame> pKFedge = vpEdgeKFMono[i2];
 
           if (pKF != pKFedge) {
             continue;
@@ -334,7 +333,7 @@ void Optimizer::BundleAdjustment(const vector<KeyFrame*>& vpKFs,
         for (size_t i2 = 0, iend = vpEdgesStereo.size(); i2 < iend; i2++) {
           g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i2];
           MapPoint* pMP = vpMapPointEdgeStereo[i2];
-          KeyFrame* pKFedge = vpEdgeKFMono[i2];
+          std::shared_ptr<KeyFrame> pKFedge = vpEdgeKFMono[i2];
 
           if (pKF != pKFedge) {
             continue;
@@ -380,7 +379,7 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
                                float priorA, Eigen::VectorXd* vSingVal,
                                bool* bHess) {
   long unsigned int maxKFid = pMap->GetMaxKFid();
-  const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
+  const vector<std::shared_ptr<KeyFrame>> vpKFs = pMap->GetAllKeyFrames();
   const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();
 
   // Setup optimizer
@@ -408,9 +407,8 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
   int nNonFixed = 0;
 
   // Set KeyFrame vertices
-  KeyFrame* pIncKF;
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
+  shared_ptr<KeyFrame> pIncKF;
+  for (auto pKFi : vpKFs) {
     if (pKFi->mnId > maxKFid) continue;
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
@@ -458,9 +456,7 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
   }
 
   // IMU links
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
-
+  for (auto pKFi : vpKFs) {
     if (!pKFi->mPrevKF) {
       Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!",
                          Verbose::VERBOSITY_NORMAL);
@@ -588,24 +584,20 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
     vPoint->setMarginalized(true);
     optimizer.addVertex(vPoint);
 
-    const map<KeyFrame*, tuple<int, int>> observations = pMP->GetObservations();
+    const map<shared_ptr<KeyFrame>, tuple<int, int>> observations =
+        pMP->GetObservations();
 
     bool bAllFixed = true;
 
     // Set edges
-    for (map<KeyFrame*, tuple<int, int>>::const_iterator
-             mit = observations.begin(),
-             mend = observations.end();
-         mit != mend; mit++) {
-      KeyFrame* pKFi = mit->first;
-
+    for (auto const& [pKFi, vObs] : observations) {
       if (pKFi->mnId > maxKFid) continue;
 
       if (!pKFi->isBad()) {
-        const int leftIndex = get<0>(mit->second);
+        const int leftIndex = get<0>(vObs);
         cv::KeyPoint kpUn;
 
-        if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] < 0) {
+        if (leftIndex != -1 && pKFi->mvuRight[get<0>(vObs)] < 0) {
           // Monocular observation
           kpUn = pKFi->mvKeysUn[leftIndex];
           Eigen::Matrix<double, 2, 1> obs;
@@ -664,7 +656,7 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
 
         if (pKFi->mpCamera2) {
           // Monocular right observation
-          int rightIndex = get<1>(mit->second);
+          int rightIndex = get<1>(vObs);
 
           if (rightIndex != -1 && rightIndex < pKFi->mvKeysRight.size()) {
             rightIndex -= pKFi->NLeft;
@@ -711,8 +703,7 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
 
   // Recover optimized data
   // Keyframes
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
+  for (auto pKFi : vpKFs) {
     if (pKFi->mnId > maxKFid) continue;
     VertexPose* VP = static_cast<VertexPose*>(optimizer.vertex(pKFi->mnId));
     if (nLoopId == 0) {
@@ -776,7 +767,7 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
   pMap->IncreaseChangeIndex();
 }
 
-int Optimizer::PoseOptimization(Frame* pFrame) {
+int Optimizer::PoseOptimization(const std::shared_ptr<Frame>& pFrame) {
   g2o::SparseOptimizer optimizer;
 
   // g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
@@ -1072,20 +1063,20 @@ int Optimizer::PoseOptimization(Frame* pFrame) {
   return nInitialCorrespondences - nBad;
 }
 
-void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
+void Optimizer::LocalBundleAdjustment(const shared_ptr<KeyFrame>& pKF,
+                                      bool* pbStopFlag,
                                       const std::shared_ptr<Map>& pMap,
                                       int& num_fixedKF, int& num_OptKF,
                                       int& num_MPs, int& num_edges) {
-  // Local KeyFrames: First Breath Search from Current Keyframe
-  list<KeyFrame*> lLocalKeyFrames;
+  // Local KeyFrames: First Breadth Search from Current Keyframe
+  list<shared_ptr<KeyFrame>> lLocalKeyFrames;
 
   lLocalKeyFrames.push_back(pKF);
   pKF->mnBALocalForKF = pKF->mnId;
-  std::shared_ptr<Map> pCurrentMap = pKF->GetMap();
+  auto pCurrentMap = pKF->GetMap();
 
-  const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
-  for (int i = 0, iend = vNeighKFs.size(); i < iend; i++) {
-    KeyFrame* pKFi = vNeighKFs[i];
+  const auto vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
+  for (auto pKFi : vNeighKFs) {
     pKFi->mnBALocalForKF = pKF->mnId;
     if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap)
       lLocalKeyFrames.push_back(pKFi);
@@ -1095,17 +1086,13 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   num_fixedKF = 0;
   list<MapPoint*> lLocalMapPoints;
   set<MapPoint*> sNumObsMP;
-  for (list<KeyFrame*>::iterator lit = lLocalKeyFrames.begin(),
-                                 lend = lLocalKeyFrames.end();
-       lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
+  for (auto pKFi : lLocalKeyFrames) {
     if (pKFi->mnId == pMap->GetInitKFid()) {
       num_fixedKF = 1;
     }
     vector<MapPoint*> vpMPs = pKFi->GetMapPointMatches();
-    for (vector<MapPoint*>::iterator vit = vpMPs.begin(), vend = vpMPs.end();
-         vit != vend; vit++) {
-      MapPoint* pMP = *vit;
+
+    for (auto pMP : vpMPs) {
       if (pMP) {
         if (!pMP->isBad() && pMP->GetMap() == pCurrentMap) {
           if (pMP->mnBALocalForKF != pKF->mnId) {
@@ -1119,15 +1106,14 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
 
   // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local
   // Keyframes
-  list<KeyFrame*> lFixedCameras;
-  for (list<MapPoint*>::iterator lit = lLocalMapPoints.begin(),
-                                 lend = lLocalMapPoints.end();
-       lit != lend; lit++) {
-    map<KeyFrame*, tuple<int, int>> observations = (*lit)->GetObservations();
-    for (map<KeyFrame*, tuple<int, int>>::iterator mit = observations.begin(),
-                                                   mend = observations.end();
+  list<shared_ptr<KeyFrame>> lFixedCameras;
+  for (auto pMP : lLocalMapPoints) {
+    map<shared_ptr<KeyFrame>, tuple<int, int>> observations =
+        pMP->GetObservations();
+
+    for (auto mit = observations.begin(), mend = observations.end();
          mit != mend; mit++) {
-      KeyFrame* pKFi = mit->first;
+      shared_ptr<KeyFrame> pKFi = mit->first;
 
       if (pKFi->mnBALocalForKF != pKF->mnId &&
           pKFi->mnBAFixedForKF != pKF->mnId) {
@@ -1176,10 +1162,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   pCurrentMap->msFixedKFs.clear();
 
   // Set Local KeyFrame vertices
-  for (list<KeyFrame*>::iterator lit = lLocalKeyFrames.begin(),
-                                 lend = lLocalKeyFrames.end();
-       lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
+  for (auto pKFi : lLocalKeyFrames) {
     g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
     Sophus::SE3<float> Tcw = pKFi->GetPose();
     vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast<double>(),
@@ -1194,10 +1177,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   num_OptKF = lLocalKeyFrames.size();
 
   // Set Fixed KeyFrame vertices
-  for (list<KeyFrame*>::iterator lit = lFixedCameras.begin(),
-                                 lend = lFixedCameras.end();
-       lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
+  for (auto pKFi : lFixedCameras) {
     g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
     Sophus::SE3<float> Tcw = pKFi->GetPose();
     vSE3->setEstimate(g2o::SE3Quat(Tcw.unit_quaternion().cast<double>(),
@@ -1220,10 +1200,10 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   vector<ORB_SLAM3::EdgeSE3ProjectXYZToBody*> vpEdgesBody;
   vpEdgesBody.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFMono;
+  vector<shared_ptr<KeyFrame>> vpEdgeKFMono;
   vpEdgeKFMono.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFBody;
+  vector<shared_ptr<KeyFrame>> vpEdgeKFBody;
   vpEdgeKFBody.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeMono;
@@ -1235,7 +1215,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
   vpEdgesStereo.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFStereo;
+  vector<shared_ptr<KeyFrame>> vpEdgeKFStereo;
   vpEdgeKFStereo.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeStereo;
@@ -1260,20 +1240,16 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
     optimizer.addVertex(vPoint);
     nPoints++;
 
-    const map<KeyFrame*, tuple<int, int>> observations = pMP->GetObservations();
+    map<shared_ptr<KeyFrame>, tuple<int, int>> observations =
+        pMP->GetObservations();
 
     // Set edges
-    for (map<KeyFrame*, tuple<int, int>>::const_iterator
-             mit = observations.begin(),
-             mend = observations.end();
-         mit != mend; mit++) {
-      KeyFrame* pKFi = mit->first;
-
+    for (auto const& [pKFi, vObs] : observations) {
       if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) {
-        const int leftIndex = get<0>(mit->second);
+        const int leftIndex = get<0>(vObs);
 
         // Monocular observation
-        if (leftIndex != -1 && pKFi->mvuRight[get<0>(mit->second)] < 0) {
+        if (leftIndex != -1 && pKFi->mvuRight[get<0>(vObs)] < 0) {
           const cv::KeyPoint& kpUn = pKFi->mvKeysUn[leftIndex];
           Eigen::Matrix<double, 2, 1> obs;
           obs << kpUn.pt.x, kpUn.pt.y;
@@ -1300,12 +1276,11 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
           vpMapPointEdgeMono.push_back(pMP);
 
           nEdges++;
-        } else if (leftIndex != -1 &&
-                   pKFi->mvuRight[get<0>(mit->second)] >= 0) {
+        } else if (leftIndex != -1 && pKFi->mvuRight[get<0>(vObs)] >= 0) {
           // Stereo observation
           const cv::KeyPoint& kpUn = pKFi->mvKeysUn[leftIndex];
           Eigen::Matrix<double, 3, 1> obs;
-          const float kp_ur = pKFi->mvuRight[get<0>(mit->second)];
+          const float kp_ur = pKFi->mvuRight[get<0>(vObs)];
           obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
 
           g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
@@ -1338,7 +1313,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
         }
 
         if (pKFi->mpCamera2) {
-          int rightIndex = get<1>(mit->second);
+          int rightIndex = get<1>(vObs);
 
           if (rightIndex != -1) {
             rightIndex -= pKFi->NLeft;
@@ -1387,7 +1362,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   optimizer.initializeOptimization();
   optimizer.optimize(10);
 
-  vector<pair<KeyFrame*, MapPoint*>> vToErase;
+  vector<pair<shared_ptr<KeyFrame>, MapPoint*>> vToErase;
   vToErase.reserve(vpEdgesMono.size() + vpEdgesBody.size() +
                    vpEdgesStereo.size());
 
@@ -1399,7 +1374,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
     if (pMP->isBad()) continue;
 
     if (e->chi2() > 5.991 || !e->isDepthPositive()) {
-      KeyFrame* pKFi = vpEdgeKFMono[i];
+      auto pKFi = vpEdgeKFMono[i];
       vToErase.push_back(make_pair(pKFi, pMP));
     }
   }
@@ -1411,7 +1386,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
     if (pMP->isBad()) continue;
 
     if (e->chi2() > 5.991 || !e->isDepthPositive()) {
-      KeyFrame* pKFi = vpEdgeKFBody[i];
+      auto pKFi = vpEdgeKFBody[i];
       vToErase.push_back(make_pair(pKFi, pMP));
     }
   }
@@ -1423,7 +1398,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
     if (pMP->isBad()) continue;
 
     if (e->chi2() > 7.815 || !e->isDepthPositive()) {
-      KeyFrame* pKFi = vpEdgeKFStereo[i];
+      auto pKFi = vpEdgeKFStereo[i];
       vToErase.push_back(make_pair(pKFi, pMP));
     }
   }
@@ -1433,7 +1408,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
 
   if (!vToErase.empty()) {
     for (size_t i = 0; i < vToErase.size(); i++) {
-      KeyFrame* pKFi = vToErase[i].first;
+      shared_ptr<KeyFrame> pKFi = vToErase[i].first;
       MapPoint* pMPi = vToErase[i].second;
       pKFi->EraseMapPointMatch(pMPi);
       pMPi->EraseObservation(pKFi);
@@ -1442,10 +1417,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
 
   // Recover optimized data
   // Keyframes
-  for (list<KeyFrame*>::iterator lit = lLocalKeyFrames.begin(),
-                                 lend = lLocalKeyFrames.end();
-       lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
+  for (auto pKFi : lLocalKeyFrames) {
     g2o::VertexSE3Expmap* vSE3 =
         static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKFi->mnId));
     g2o::SE3Quat SE3quat = vSE3->estimate();
@@ -1455,10 +1427,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
   }
 
   // Points
-  for (list<MapPoint*>::iterator lit = lLocalMapPoints.begin(),
-                                 lend = lLocalMapPoints.end();
-       lit != lend; lit++) {
-    MapPoint* pMP = *lit;
+  for (auto pMP : lLocalMapPoints) {
     g2o::VertexPointXYZ* vPoint = static_cast<g2o::VertexPointXYZ*>(
         optimizer.vertex(pMP->mnId + maxKFid + 1));
     pMP->SetWorldPos(vPoint->estimate().cast<float>());
@@ -1469,10 +1438,11 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pKF, bool* pbStopFlag,
 }
 
 void Optimizer::OptimizeEssentialGraph(
-    const std::shared_ptr<Map>& pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
+    const shared_ptr<Map>& pMap, const shared_ptr<KeyFrame>& pLoopKF,
+    const shared_ptr<KeyFrame>& pCurKF,
     const LoopClosing::KeyFrameAndPose& NonCorrectedSim3,
     const LoopClosing::KeyFrameAndPose& CorrectedSim3,
-    const map<KeyFrame*, set<KeyFrame*>>& LoopConnections,
+    const map<shared_ptr<KeyFrame>, set<shared_ptr<KeyFrame>>>& LoopConnections,
     const bool& bFixScale) {
   // Setup optimizer
   g2o::SparseOptimizer optimizer;
@@ -1493,7 +1463,7 @@ void Optimizer::OptimizeEssentialGraph(
   solver->setUserLambdaInit(1e-16);
   optimizer.setAlgorithm(solver);
 
-  const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
+  const vector<shared_ptr<KeyFrame>> vpKFs = pMap->GetAllKeyFrames();
   const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();
 
   const unsigned int nMaxKFid = pMap->GetMaxKFid();
@@ -1510,8 +1480,7 @@ void Optimizer::OptimizeEssentialGraph(
   const int minFeat = 100;
 
   // Set KeyFrame vertices
-  for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) {
-    KeyFrame* pKF = vpKFs[i];
+  for (auto pKF : vpKFs) {
     if (pKF->isBad()) continue;
     g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
 
@@ -1548,18 +1517,18 @@ void Optimizer::OptimizeEssentialGraph(
 
   // Set Loop edges
   int count_loop = 0;
-  for (map<KeyFrame*, set<KeyFrame*>>::const_iterator
+  for (map<shared_ptr<KeyFrame>, set<shared_ptr<KeyFrame>>>::const_iterator
            mit = LoopConnections.begin(),
            mend = LoopConnections.end();
        mit != mend; mit++) {
-    KeyFrame* pKF = mit->first;
+    shared_ptr<KeyFrame> pKF = mit->first;
     const long unsigned int nIDi = pKF->mnId;
-    const set<KeyFrame*>& spConnections = mit->second;
+    const set<shared_ptr<KeyFrame>>& spConnections = mit->second;
     const g2o::Sim3 Siw = vScw[nIDi];
     const g2o::Sim3 Swi = Siw.inverse();
 
-    for (set<KeyFrame*>::const_iterator sit = spConnections.begin(),
-                                        send = spConnections.end();
+    for (set<shared_ptr<KeyFrame>>::const_iterator sit = spConnections.begin(),
+                                                   send = spConnections.end();
          sit != send; sit++) {
       const long unsigned int nIDj = (*sit)->mnId;
       if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) &&
@@ -1585,9 +1554,7 @@ void Optimizer::OptimizeEssentialGraph(
   }
 
   // Set normal edges
-  for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) {
-    KeyFrame* pKF = vpKFs[i];
-
+  for (auto pKF : vpKFs) {
     const int nIDi = pKF->mnId;
 
     g2o::Sim3 Swi;
@@ -1600,7 +1567,7 @@ void Optimizer::OptimizeEssentialGraph(
     else
       Swi = vScw[nIDi].inverse();
 
-    KeyFrame* pParentKF = pKF->GetParent();
+    shared_ptr<KeyFrame> pParentKF = pKF->GetParent();
 
     // Spanning tree edge
     if (pParentKF) {
@@ -1629,11 +1596,8 @@ void Optimizer::OptimizeEssentialGraph(
     }
 
     // Loop edges
-    const set<KeyFrame*> sLoopEdges = pKF->GetLoopEdges();
-    for (set<KeyFrame*>::const_iterator sit = sLoopEdges.begin(),
-                                        send = sLoopEdges.end();
-         sit != send; sit++) {
-      KeyFrame* pLKF = *sit;
+    const set<shared_ptr<KeyFrame>> sLoopEdges = pKF->GetLoopEdges();
+    for (auto pLKF : sLoopEdges) {
       if (pLKF->mnId < pKF->mnId) {
         g2o::Sim3 Slw;
 
@@ -1658,11 +1622,12 @@ void Optimizer::OptimizeEssentialGraph(
     }
 
     // Covisibility graph edges
-    const vector<KeyFrame*> vpConnectedKFs =
+    const vector<shared_ptr<KeyFrame>> vpConnectedKFs =
         pKF->GetCovisiblesByWeight(minFeat);
-    for (vector<KeyFrame*>::const_iterator vit = vpConnectedKFs.begin();
+    for (vector<shared_ptr<KeyFrame>>::const_iterator vit =
+             vpConnectedKFs.begin();
          vit != vpConnectedKFs.end(); vit++) {
-      KeyFrame* pKFn = *vit;
+      shared_ptr<KeyFrame> pKFn = *vit;
       if (pKFn && pKFn != pParentKF &&
           !pKF->hasChild(pKFn) /*&& !sLoopEdges.count(pKFn)*/) {
         if (!pKFn->isBad() && pKFn->mnId < pKF->mnId) {
@@ -1723,9 +1688,7 @@ void Optimizer::OptimizeEssentialGraph(
   unique_lock<mutex> lock(pMap->mMutexMapUpdate);
 
   // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
-
+  for (auto pKFi : vpKFs) {
     const int nIDi = pKFi->mnId;
 
     g2o::VertexSim3Expmap* VSim3 =
@@ -1750,7 +1713,7 @@ void Optimizer::OptimizeEssentialGraph(
     if (pMP->mnCorrectedByKF == pCurKF->mnId) {
       nIDr = pMP->mnCorrectedReference;
     } else {
-      KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
+      shared_ptr<KeyFrame> pRefKF = pMP->GetReferenceKeyFrame();
       nIDr = pRefKF->mnId;
     }
 
@@ -1769,11 +1732,12 @@ void Optimizer::OptimizeEssentialGraph(
   pMap->IncreaseChangeIndex();
 }
 
-void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
-                                       vector<KeyFrame*>& vpFixedKFs,
-                                       vector<KeyFrame*>& vpFixedCorrectedKFs,
-                                       vector<KeyFrame*>& vpNonFixedKFs,
-                                       vector<MapPoint*>& vpNonCorrectedMPs) {
+void Optimizer::OptimizeEssentialGraph(
+    const std::shared_ptr<KeyFrame>& pCurKF,
+    vector<shared_ptr<KeyFrame>>& vpFixedKFs,
+    vector<shared_ptr<KeyFrame>>& vpFixedCorrectedKFs,
+    vector<shared_ptr<KeyFrame>>& vpNonFixedKFs,
+    vector<MapPoint*>& vpNonCorrectedMPs) {
   Verbose::PrintMess("Opt_Essential: There are " +
                          to_string(vpFixedKFs.size()) +
                          " KFs fixed in the merged map",
@@ -1822,7 +1786,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
 
   const int minFeat = 100;
 
-  for (KeyFrame* pKFi : vpFixedKFs) {
+  for (auto pKFi : vpFixedKFs) {
     if (pKFi->isBad()) continue;
 
     g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
@@ -1852,7 +1816,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
                      Verbose::VERBOSITY_DEBUG);
 
   set<unsigned long> sIdKF;
-  for (KeyFrame* pKFi : vpFixedCorrectedKFs) {
+  for (auto pKFi : vpFixedCorrectedKFs) {
     if (pKFi->isBad()) continue;
 
     g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
@@ -1884,7 +1848,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
     vpBadPose[nIDi] = true;
   }
 
-  for (KeyFrame* pKFi : vpNonFixedKFs) {
+  for (auto pKFi : vpNonFixedKFs) {
     if (pKFi->isBad()) continue;
 
     const int nIDi = pKFi->mnId;
@@ -1915,19 +1879,19 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
     vpBadPose[nIDi] = true;
   }
 
-  vector<KeyFrame*> vpKFs;
+  vector<shared_ptr<KeyFrame>> vpKFs;
   vpKFs.reserve(vpFixedKFs.size() + vpFixedCorrectedKFs.size() +
                 vpNonFixedKFs.size());
   vpKFs.insert(vpKFs.end(), vpFixedKFs.begin(), vpFixedKFs.end());
   vpKFs.insert(vpKFs.end(), vpFixedCorrectedKFs.begin(),
                vpFixedCorrectedKFs.end());
   vpKFs.insert(vpKFs.end(), vpNonFixedKFs.begin(), vpNonFixedKFs.end());
-  set<KeyFrame*> spKFs(vpKFs.begin(), vpKFs.end());
+  set<shared_ptr<KeyFrame>> spKFs(vpKFs.begin(), vpKFs.end());
 
   const Eigen::Matrix<double, 7, 7> matLambda =
       Eigen::Matrix<double, 7, 7>::Identity();
 
-  for (KeyFrame* pKFi : vpKFs) {
+  for (auto pKFi : vpKFs) {
     int num_connections = 0;
     const int nIDi = pKFi->mnId;
 
@@ -1937,7 +1901,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
     if (vpGoodPose[nIDi]) correctedSwi = vCorrectedSwc[nIDi];
     if (vpBadPose[nIDi]) Swi = vScw[nIDi].inverse();
 
-    KeyFrame* pParentKFi = pKFi->GetParent();
+    auto pParentKFi = pKFi->GetParent();
 
     // Spanning tree edge
     if (pParentKFi && spKFs.find(pParentKFi) != spKFs.end()) {
@@ -1971,11 +1935,8 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
     }
 
     // Loop edges
-    const set<KeyFrame*> sLoopEdges = pKFi->GetLoopEdges();
-    for (set<KeyFrame*>::const_iterator sit = sLoopEdges.begin(),
-                                        send = sLoopEdges.end();
-         sit != send; sit++) {
-      KeyFrame* pLKF = *sit;
+    const set<shared_ptr<KeyFrame>> sLoopEdges = pKFi->GetLoopEdges();
+    for (auto pLKF : sLoopEdges) {
       if (spKFs.find(pLKF) != spKFs.end() && pLKF->mnId < pKFi->mnId) {
         g2o::Sim3 Slw;
         bool bHasRelation = false;
@@ -2004,11 +1965,9 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
     }
 
     // Covisibility graph edges
-    const vector<KeyFrame*> vpConnectedKFs =
+    const vector<shared_ptr<KeyFrame>> vpConnectedKFs =
         pKFi->GetCovisiblesByWeight(minFeat);
-    for (vector<KeyFrame*>::const_iterator vit = vpConnectedKFs.begin();
-         vit != vpConnectedKFs.end(); vit++) {
-      KeyFrame* pKFn = *vit;
+    for (auto pKFn : vpConnectedKFs) {
       if (pKFn && pKFn != pParentKFi && !pKFi->hasChild(pKFn) &&
           !sLoopEdges.count(pKFn) && spKFs.find(pKFn) != spKFs.end()) {
         if (!pKFn->isBad() && pKFn->mnId < pKFi->mnId) {
@@ -2054,7 +2013,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
   unique_lock<mutex> lock(pMap->mMutexMapUpdate);
 
   // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
-  for (KeyFrame* pKFi : vpNonFixedKFs) {
+  for (auto pKFi : vpNonFixedKFs) {
     if (pKFi->isBad()) continue;
 
     const int nIDi = pKFi->mnId;
@@ -2076,7 +2035,7 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
   for (MapPoint* pMPi : vpNonCorrectedMPs) {
     if (pMPi->isBad()) continue;
 
-    KeyFrame* pRefKF = pMPi->GetReferenceKeyFrame();
+    auto pRefKF = pMPi->GetReferenceKeyFrame();
     while (pRefKF->isBad()) {
       if (!pRefKF) {
         Verbose::PrintMess(
@@ -2104,7 +2063,8 @@ void Optimizer::OptimizeEssentialGraph(KeyFrame* pCurKF,
   }
 }
 
-int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
+int Optimizer::OptimizeSim3(const shared_ptr<KeyFrame>& pKF1,
+                            const shared_ptr<KeyFrame>& pKF2,
                             vector<MapPoint*>& vpMatches1, g2o::Sim3& g2oS12,
                             const float th2, const bool bFixScale,
                             Eigen::Matrix<double, 7, 7>& mAcumHessian,
@@ -2368,7 +2328,8 @@ int Optimizer::OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2,
   return nIn;
 }
 
-void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
+void Optimizer::LocalInertialBA(const shared_ptr<KeyFrame>& pKF,
+                                bool* pbStopFlag,
                                 const std::shared_ptr<Map>& pMap,
                                 int& num_fixedKF, int& num_OptKF, int& num_MPs,
                                 int& num_edges, bool bLarge, bool bRecInit) {
@@ -2384,9 +2345,9 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
       std::min(static_cast<int>(pCurrentMap->KeyFramesInMap()) - 2, maxOpt);
   const unsigned long maxKFid = pKF->mnId;
 
-  vector<KeyFrame*> vpOptimizableKFs;
-  const vector<KeyFrame*> vpNeighsKFs = pKF->GetVectorCovisibleKeyFrames();
-  list<KeyFrame*> lpOptVisKFs;
+  vector<shared_ptr<KeyFrame>> vpOptimizableKFs;
+  const auto vpNeighsKFs = pKF->GetVectorCovisibleKeyFrames();
+  list<shared_ptr<KeyFrame>> lpOptVisKFs;
 
   vpOptimizableKFs.reserve(Nd);
   vpOptimizableKFs.push_back(pKF);
@@ -2421,7 +2382,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   }
 
   // Fixed Keyframe: First frame previous KF to optimization window)
-  list<KeyFrame*> lFixedKeyFrames;
+  list<shared_ptr<KeyFrame>> lFixedKeyFrames;
   if (vpOptimizableKFs.back()->mPrevKF) {
     lFixedKeyFrames.push_back(vpOptimizableKFs.back()->mPrevKF);
     vpOptimizableKFs.back()->mPrevKF->mnBAFixedForKF = pKF->mnId;
@@ -2437,7 +2398,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   for (int i = 0, iend = vpNeighsKFs.size(); i < iend; i++) {
     if (lpOptVisKFs.size() >= maxCovKF) break;
 
-    KeyFrame* pKFi = vpNeighsKFs[i];
+    shared_ptr<KeyFrame> pKFi = vpNeighsKFs[i];
     if (pKFi->mnBALocalForKF == pKF->mnId || pKFi->mnBAFixedForKF == pKF->mnId)
       continue;
     pKFi->mnBALocalForKF = pKF->mnId;
@@ -2466,11 +2427,13 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   for (list<MapPoint*>::iterator lit = lLocalMapPoints.begin(),
                                  lend = lLocalMapPoints.end();
        lit != lend; lit++) {
-    map<KeyFrame*, tuple<int, int>> observations = (*lit)->GetObservations();
-    for (map<KeyFrame*, tuple<int, int>>::iterator mit = observations.begin(),
-                                                   mend = observations.end();
+    map<shared_ptr<KeyFrame>, tuple<int, int>> observations =
+        (*lit)->GetObservations();
+    for (map<shared_ptr<KeyFrame>, tuple<int, int>>::iterator
+             mit = observations.begin(),
+             mend = observations.end();
          mit != mend; mit++) {
-      KeyFrame* pKFi = mit->first;
+      shared_ptr<KeyFrame> pKFi = mit->first;
 
       if (pKFi->mnBALocalForKF != pKF->mnId &&
           pKFi->mnBAFixedForKF != pKF->mnId) {
@@ -2519,9 +2482,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
 
   // Set Local temporal KeyFrame vertices
   N = vpOptimizableKFs.size();
-  for (int i = 0; i < N; i++) {
-    KeyFrame* pKFi = vpOptimizableKFs[i];
-
+  for (auto pKFi : vpOptimizableKFs) {
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
     VP->setFixed(false);
@@ -2544,10 +2505,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   }
 
   // Set Local visual KeyFrame vertices
-  for (list<KeyFrame*>::iterator it = lpOptVisKFs.begin(),
-                                 itEnd = lpOptVisKFs.end();
-       it != itEnd; it++) {
-    KeyFrame* pKFi = *it;
+  for (auto pKFi : lpOptVisKFs) {
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
     VP->setFixed(false);
@@ -2555,10 +2513,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   }
 
   // Set Fixed KeyFrame vertices
-  for (list<KeyFrame*>::iterator lit = lFixedKeyFrames.begin(),
-                                 lend = lFixedKeyFrames.end();
-       lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
+  for (auto pKFi : lFixedKeyFrames) {
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
     VP->setFixed(true);
@@ -2588,7 +2543,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   vector<EdgeAccRW*> vear(N, nullptr);
 
   for (int i = 0; i < N; i++) {
-    KeyFrame* pKFi = vpOptimizableKFs[i];
+    auto pKFi = vpOptimizableKFs[i];
 
     if (!pKFi->mPrevKF) {
       cout << "NOT INERTIAL LINK TO PREVIOUS FRAME!!!!" << endl;
@@ -2671,7 +2626,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   vector<EdgeMono*> vpEdgesMono;
   vpEdgesMono.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFMono;
+  vector<shared_ptr<KeyFrame>> vpEdgeKFMono;
   vpEdgeKFMono.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeMono;
@@ -2681,7 +2636,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   vector<EdgeStereo*> vpEdgesStereo;
   vpEdgesStereo.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFStereo;
+  vector<shared_ptr<KeyFrame>> vpEdgeKFStereo;
   vpEdgeKFStereo.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeStereo;
@@ -2695,14 +2650,11 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   const unsigned long iniMPid = maxKFid * 5;
 
   map<int, int> mVisEdges;
-  for (int i = 0; i < N; i++) {
-    KeyFrame* pKFi = vpOptimizableKFs[i];
+  for (auto pKFi : vpOptimizableKFs) {
     mVisEdges[pKFi->mnId] = 0;
   }
-  for (list<KeyFrame*>::iterator lit = lFixedKeyFrames.begin(),
-                                 lend = lFixedKeyFrames.end();
-       lit != lend; lit++) {
-    mVisEdges[(*lit)->mnId] = 0;
+  for (auto lit : lFixedKeyFrames) {
+    mVisEdges[lit->mnId] = 0;
   }
 
   for (list<MapPoint*>::iterator lit = lLocalMapPoints.begin(),
@@ -2716,21 +2668,16 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
     vPoint->setId(id);
     vPoint->setMarginalized(true);
     optimizer.addVertex(vPoint);
-    const map<KeyFrame*, tuple<int, int>> observations = pMP->GetObservations();
+    auto const observations = pMP->GetObservations();
 
     // Create visual constraints
-    for (map<KeyFrame*, tuple<int, int>>::const_iterator
-             mit = observations.begin(),
-             mend = observations.end();
-         mit != mend; mit++) {
-      KeyFrame* pKFi = mit->first;
-
+    for (auto const& [pKFi, vObs] : observations) {
       if (pKFi->mnBALocalForKF != pKF->mnId &&
           pKFi->mnBAFixedForKF != pKF->mnId)
         continue;
 
       if (!pKFi->isBad() && pKFi->GetMap() == pCurrentMap) {
-        const int leftIndex = get<0>(mit->second);
+        const int leftIndex = get<0>(vObs);
 
         cv::KeyPoint kpUn;
 
@@ -2799,7 +2746,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
 
         // Monocular right observation
         if (pKFi->mpCamera2) {
-          int rightIndex = get<1>(mit->second);
+          int rightIndex = get<1>(vObs);
 
           if (rightIndex != -1) {
             rightIndex -= pKFi->NLeft;
@@ -2850,7 +2797,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   float err_end = optimizer.activeRobustChi2();
   if (pbStopFlag) optimizer.setForceStopFlag(pbStopFlag);
 
-  vector<pair<KeyFrame*, MapPoint*>> vToErase;
+  vector<pair<shared_ptr<KeyFrame>, MapPoint*>> vToErase;
   vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());
 
   // Check inlier observations
@@ -2864,7 +2811,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
 
     if ((e->chi2() > chi2Mono2 && !bClose) ||
         (e->chi2() > 1.5f * chi2Mono2 && bClose) || !e->isDepthPositive()) {
-      KeyFrame* pKFi = vpEdgeKFMono[i];
+      shared_ptr<KeyFrame> pKFi = vpEdgeKFMono[i];
       vToErase.push_back(make_pair(pKFi, pMP));
     }
   }
@@ -2877,7 +2824,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
     if (pMP->isBad()) continue;
 
     if (e->chi2() > chi2Stereo2) {
-      KeyFrame* pKFi = vpEdgeKFStereo[i];
+      shared_ptr<KeyFrame> pKFi = vpEdgeKFStereo[i];
       vToErase.push_back(make_pair(pKFi, pMP));
     }
   }
@@ -2894,24 +2841,19 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
 
   if (!vToErase.empty()) {
     for (size_t i = 0; i < vToErase.size(); i++) {
-      KeyFrame* pKFi = vToErase[i].first;
+      shared_ptr<KeyFrame> pKFi = vToErase[i].first;
       MapPoint* pMPi = vToErase[i].second;
       pKFi->EraseMapPointMatch(pMPi);
       pMPi->EraseObservation(pKFi);
     }
   }
 
-  for (list<KeyFrame*>::iterator lit = lFixedKeyFrames.begin(),
-                                 lend = lFixedKeyFrames.end();
-       lit != lend; lit++)
-    (*lit)->mnBAFixedForKF = 0;
+  for (auto pKFi : lFixedKeyFrames) pKFi->mnBAFixedForKF = 0;
 
   // Recover optimized data
   // Local temporal Keyframes
   N = vpOptimizableKFs.size();
-  for (int i = 0; i < N; i++) {
-    KeyFrame* pKFi = vpOptimizableKFs[i];
-
+  for (auto pKFi : vpOptimizableKFs) {
     VertexPose* VP = static_cast<VertexPose*>(optimizer.vertex(pKFi->mnId));
     Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast<float>(),
                      VP->estimate().tcw[0].cast<float>());
@@ -2933,10 +2875,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   }
 
   // Local visual KeyFrame
-  for (list<KeyFrame*>::iterator it = lpOptVisKFs.begin(),
-                                 itEnd = lpOptVisKFs.end();
-       it != itEnd; it++) {
-    KeyFrame* pKFi = *it;
+  for (auto pKFi : lpOptVisKFs) {
     VertexPose* VP = static_cast<VertexPose*>(optimizer.vertex(pKFi->mnId));
     Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast<float>(),
                      VP->estimate().tcw[0].cast<float>());
@@ -2945,10 +2884,7 @@ void Optimizer::LocalInertialBA(KeyFrame* pKF, bool* pbStopFlag,
   }
 
   // Points
-  for (list<MapPoint*>::iterator lit = lLocalMapPoints.begin(),
-                                 lend = lLocalMapPoints.end();
-       lit != lend; lit++) {
-    MapPoint* pMP = *lit;
+  for (auto pMP : lLocalMapPoints) {
     g2o::VertexPointXYZ* vPoint = static_cast<g2o::VertexPointXYZ*>(
         optimizer.vertex(pMP->mnId + iniMPid + 1));
     pMP->SetWorldPos(vPoint->estimate().cast<float>());
@@ -3048,7 +2984,7 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
   Verbose::PrintMess("inertial optimization", Verbose::VERBOSITY_NORMAL);
   int its = 200;
   long unsigned int maxKFid = pMap->GetMaxKFid();
-  const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
+  const auto vpKFs = pMap->GetAllKeyFrames();
 
   // Setup optimizer
   g2o::SparseOptimizer optimizer;
@@ -3071,8 +3007,7 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
   optimizer.setAlgorithm(solver);
 
   // Set KeyFrame vertices (fixed poses and optimizable velocities)
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
+  for (auto pKFi : vpKFs) {
     if (pKFi->mnId > maxKFid) continue;
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
@@ -3134,13 +3069,11 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
   // IMU links with gravity and scale
   vector<EdgeInertialGS*> vpei;
   vpei.reserve(vpKFs.size());
-  vector<pair<KeyFrame*, KeyFrame*>> vppUsedKF;
+  vector<pair<shared_ptr<KeyFrame>, shared_ptr<KeyFrame>>> vppUsedKF;
   vppUsedKF.reserve(vpKFs.size());
   // std::cout << "build optimization graph" << std::endl;
 
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
-
+  for (auto pKFi : vpKFs) {
     if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) {
       if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) continue;
       if (!pKFi->mpImuPreintegrated)
@@ -3205,8 +3138,7 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
 
   // Keyframes velocities and biases
   const int N = vpKFs.size();
-  for (size_t i = 0; i < N; i++) {
-    KeyFrame* pKFi = vpKFs[i];
+  for (auto pKFi : vpKFs) {
     if (pKFi->mnId > maxKFid) continue;
 
     VertexVelocity* VV = static_cast<VertexVelocity*>(
@@ -3228,7 +3160,7 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
                                      float priorG, float priorA) {
   int its = 200;  // Check number of iterations
   long unsigned int maxKFid = pMap->GetMaxKFid();
-  const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
+  const vector<shared_ptr<KeyFrame>> vpKFs = pMap->GetAllKeyFrames();
 
   // Setup optimizer
   g2o::SparseOptimizer optimizer;
@@ -3251,8 +3183,7 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
   optimizer.setAlgorithm(solver);
 
   // Set KeyFrame vertices (fixed poses and optimizable velocities)
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
+  for (auto pKFi : vpKFs) {
     if (pKFi->mnId > maxKFid) continue;
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
@@ -3307,12 +3238,10 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
   // IMU links with gravity and scale
   vector<EdgeInertialGS*> vpei;
   vpei.reserve(vpKFs.size());
-  vector<pair<KeyFrame*, KeyFrame*>> vppUsedKF;
+  vector<pair<std::shared_ptr<KeyFrame>, std::shared_ptr<KeyFrame>>> vppUsedKF;
   vppUsedKF.reserve(vpKFs.size());
 
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
-
+  for (auto pKFi : vpKFs) {
     if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) {
       if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) continue;
 
@@ -3369,8 +3298,7 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
 
   // Keyframes velocities and biases
   const int N = vpKFs.size();
-  for (size_t i = 0; i < N; i++) {
-    KeyFrame* pKFi = vpKFs[i];
+  for (auto pKFi : vpKFs) {
     if (pKFi->mnId > maxKFid) continue;
 
     VertexVelocity* VV = static_cast<VertexVelocity*>(
@@ -3391,7 +3319,7 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
                                      Eigen::Matrix3d& Rwg, double& scale) {
   int its = 10;
   long unsigned int maxKFid = pMap->GetMaxKFid();
-  const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
+  const vector<shared_ptr<KeyFrame>> vpKFs = pMap->GetAllKeyFrames();
 
   // Setup optimizer
   g2o::SparseOptimizer optimizer;
@@ -3412,8 +3340,7 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
   optimizer.setAlgorithm(solver);
 
   // Set KeyFrame vertices (all variables are fixed)
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
+  for (auto pKFi : vpKFs) {
     if (pKFi->mnId > maxKFid) continue;
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
@@ -3448,9 +3375,7 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
 
   // Graph edges
   int count_edges = 0;
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
-
+  for (auto pKFi : vpKFs) {
     if (pKFi->mPrevKF && pKFi->mnId <= maxKFid) {
       if (pKFi->isBad() || pKFi->mPrevKF->mnId > maxKFid) continue;
 
@@ -3506,9 +3431,9 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
   Rwg = VGDir->estimate().Rwg;
 }
 
-void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
-                                      vector<KeyFrame*> vpAdjustKF,
-                                      vector<KeyFrame*> vpFixedKF,
+void Optimizer::LocalBundleAdjustment(const shared_ptr<KeyFrame>& pMainKF,
+                                      vector<shared_ptr<KeyFrame>> vpAdjustKF,
+                                      vector<shared_ptr<KeyFrame>> vpFixedKF,
                                       bool* pbStopFlag) {
   bool bShowImages = false;
 
@@ -3536,13 +3461,13 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   if (pbStopFlag) optimizer.setForceStopFlag(pbStopFlag);
 
   long unsigned int maxKFid = 0;
-  set<KeyFrame*> spKeyFrameBA;
+  set<shared_ptr<KeyFrame>> spKeyFrameBA;
 
   std::shared_ptr<Map> pCurrentMap = pMainKF->GetMap();
 
   // Set fixed KeyFrame vertices
   int numInsertedPoints = 0;
-  for (KeyFrame* pKFi : vpFixedKF) {
+  for (auto pKFi : vpFixedKF) {
     if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap) {
       Verbose::PrintMess("ERROR LBA: KF is bad or is not in the current map",
                          Verbose::VERBOSITY_NORMAL);
@@ -3577,9 +3502,9 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   }
 
   // Set non fixed Keyframe vertices
-  set<KeyFrame*> spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end());
+  set<shared_ptr<KeyFrame>> spAdjustKF(vpAdjustKF.begin(), vpAdjustKF.end());
   numInsertedPoints = 0;
-  for (KeyFrame* pKFi : vpAdjustKF) {
+  for (auto pKFi : vpAdjustKF) {
     if (pKFi->isBad() || pKFi->GetMap() != pCurrentMap) continue;
 
     pKFi->mnBALocalForMerge = pMainKF->mnId;
@@ -3614,7 +3539,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   vector<ORB_SLAM3::EdgeSE3ProjectXYZ*> vpEdgesMono;
   vpEdgesMono.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFMono;
+  vector<shared_ptr<KeyFrame>> vpEdgeKFMono;
   vpEdgeKFMono.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeMono;
@@ -3623,7 +3548,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
   vpEdgesStereo.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFStereo;
+  vector<shared_ptr<KeyFrame>> vpEdgeKFStereo;
   vpEdgeKFStereo.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeStereo;
@@ -3633,11 +3558,10 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   const float thHuber3D = sqrt(7.815);
 
   // Set MapPoint vertices
-  map<KeyFrame*, int> mpObsKFs;
-  map<KeyFrame*, int> mpObsFinalKFs;
+  map<shared_ptr<KeyFrame>, int> mpObsKFs;
+  map<shared_ptr<KeyFrame>, int> mpObsFinalKFs;
   map<MapPoint*, int> mpObsMPs;
-  for (unsigned int i = 0; i < vpMPs.size(); ++i) {
-    MapPoint* pMPi = vpMPs[i];
+  for (auto pMPi : vpMPs) {
     if (pMPi->isBad()) continue;
 
     g2o::VertexPointXYZ* vPoint = new g2o::VertexPointXYZ();
@@ -3647,14 +3571,14 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     vPoint->setMarginalized(true);
     optimizer.addVertex(vPoint);
 
-    const map<KeyFrame*, tuple<int, int>> observations =
+    const map<shared_ptr<KeyFrame>, tuple<int, int>> observations =
         pMPi->GetObservations();
     int nEdges = 0;
     // SET EDGES
-    for (map<KeyFrame*, tuple<int, int>>::const_iterator mit =
+    for (map<shared_ptr<KeyFrame>, tuple<int, int>>::const_iterator mit =
              observations.begin();
          mit != observations.end(); mit++) {
-      KeyFrame* pKF = mit->first;
+      shared_ptr<KeyFrame> pKF = mit->first;
       if (pKF->isBad() || pKF->mnId > maxKFid ||
           pKF->mnBALocalForMerge != pMainKF->mnId ||
           !pKF->GetMapPoint(get<0>(mit->second)))
@@ -3782,10 +3706,10 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     optimizer.optimize(10);
   }
 
-  vector<pair<KeyFrame*, MapPoint*>> vToErase;
+  vector<pair<shared_ptr<KeyFrame>, MapPoint*>> vToErase;
   vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());
   set<MapPoint*> spErasedMPs;
-  set<KeyFrame*> spErasedKFs;
+  set<shared_ptr<KeyFrame>> spErasedKFs;
 
   // Check inlier observations
   int badMonoMP = 0, badStereoMP = 0;
@@ -3796,7 +3720,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     if (pMP->isBad()) continue;
 
     if (e->chi2() > 5.991 || !e->isDepthPositive()) {
-      KeyFrame* pKFi = vpEdgeKFMono[i];
+      shared_ptr<KeyFrame> pKFi = vpEdgeKFMono[i];
       vToErase.push_back(make_pair(pKFi, pMP));
       mWrongObsKF[pKFi->mnId]++;
       badMonoMP++;
@@ -3813,7 +3737,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     if (pMP->isBad()) continue;
 
     if (e->chi2() > 7.815 || !e->isDepthPositive()) {
-      KeyFrame* pKFi = vpEdgeKFStereo[i];
+      shared_ptr<KeyFrame> pKFi = vpEdgeKFStereo[i];
       vToErase.push_back(make_pair(pKFi, pMP));
       mWrongObsKF[pKFi->mnId]++;
       badStereoMP++;
@@ -3833,22 +3757,21 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
 
   if (!vToErase.empty()) {
     for (size_t i = 0; i < vToErase.size(); i++) {
-      KeyFrame* pKFi = vToErase[i].first;
+      shared_ptr<KeyFrame> pKFi = vToErase[i].first;
       MapPoint* pMPi = vToErase[i].second;
       pKFi->EraseMapPointMatch(pMPi);
       pMPi->EraseObservation(pKFi);
     }
   }
-  for (unsigned int i = 0; i < vpMPs.size(); ++i) {
-    MapPoint* pMPi = vpMPs[i];
+  for (auto pMPi : vpMPs) {
     if (pMPi->isBad()) continue;
 
-    const map<KeyFrame*, tuple<int, int>> observations =
+    const map<std::shared_ptr<KeyFrame>, tuple<int, int>> observations =
         pMPi->GetObservations();
-    for (map<KeyFrame*, tuple<int, int>>::const_iterator mit =
+    for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::const_iterator mit =
              observations.begin();
          mit != observations.end(); mit++) {
-      KeyFrame* pKF = mit->first;
+      std::shared_ptr<KeyFrame> pKF = mit->first;
       if (pKF->isBad() || pKF->mnId > maxKFid ||
           pKF->mnBALocalForKF != pMainKF->mnId ||
           !pKF->GetMapPoint(get<0>(mit->second)))
@@ -3866,7 +3789,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
 
   // Recover optimized data
   // Keyframes
-  for (KeyFrame* pKFi : vpAdjustKF) {
+  for (std::shared_ptr<KeyFrame> pKFi : vpAdjustKF) {
     if (pKFi->isBad()) continue;
 
     g2o::VertexSE3Expmap* vSE3 =
@@ -3883,7 +3806,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++) {
       ORB_SLAM3::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
       MapPoint* pMP = vpMapPointEdgeMono[i];
-      KeyFrame* pKFedge = vpEdgeKFMono[i];
+      std::shared_ptr<KeyFrame> pKFedge = vpEdgeKFMono[i];
 
       if (pKFi != pKFedge) {
         continue;
@@ -3904,7 +3827,7 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
     for (size_t i = 0, iend = vpEdgesStereo.size(); i < iend; i++) {
       g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
       MapPoint* pMP = vpMapPointEdgeStereo[i];
-      KeyFrame* pKFedge = vpEdgeKFMono[i];
+      std::shared_ptr<KeyFrame> pKFedge = vpEdgeKFMono[i];
 
       if (pKFi != pKFedge) {
         continue;
@@ -3935,19 +3858,20 @@ void Optimizer::LocalBundleAdjustment(KeyFrame* pMainKF,
   }
 }
 
-void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
+void Optimizer::MergeInertialBA(const std::shared_ptr<KeyFrame>& pCurrKF,
+                                const std::shared_ptr<KeyFrame>& pMergeKF,
                                 bool* pbStopFlag,
                                 const std::shared_ptr<Map>& pMap,
                                 LoopClosing::KeyFrameAndPose& corrPoses) {
   const int Nd = 6;
   const unsigned long maxKFid = pCurrKF->mnId;
 
-  vector<KeyFrame*> vpOptimizableKFs;
+  vector<std::shared_ptr<KeyFrame>> vpOptimizableKFs;
   vpOptimizableKFs.reserve(2 * Nd);
 
   // For cov KFS, inertial parameters are not optimized
   const int maxCovKF = 30;
-  vector<KeyFrame*> vpOptimizableCovKFs;
+  vector<std::shared_ptr<KeyFrame>> vpOptimizableCovKFs;
   vpOptimizableCovKFs.reserve(maxCovKF);
 
   // Add sliding window for current KF
@@ -3962,7 +3886,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
     }
   }
 
-  list<KeyFrame*> lFixedKeyFrames;
+  list<std::shared_ptr<KeyFrame>> lFixedKeyFrames;
   if (vpOptimizableKFs.back()->mPrevKF) {
     vpOptimizableCovKFs.push_back(vpOptimizableKFs.back()->mPrevKF);
     vpOptimizableKFs.back()->mPrevKF->mnBALocalForKF = pCurrKF->mnId;
@@ -4049,13 +3973,13 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
   for (vector<pair<MapPoint*, int>>::iterator lit = pairs.begin(),
                                               lend = pairs.end();
        lit != lend; lit++, i++) {
-    map<KeyFrame*, tuple<int, int>> observations =
+    map<std::shared_ptr<KeyFrame>, tuple<int, int>> observations =
         lit->first->GetObservations();
+
     if (i >= maxCovKF) break;
-    for (map<KeyFrame*, tuple<int, int>>::iterator mit = observations.begin(),
-                                                   mend = observations.end();
-         mit != mend; mit++) {
-      KeyFrame* pKFi = mit->first;
+
+    for (auto mit : observations) {
+      auto pKFi = mit.first;
 
       if (pKFi->mnBALocalForKF != pCurrKF->mnId &&
           pKFi->mnBAFixedForKF != pCurrKF->mnId) {
@@ -4091,9 +4015,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
 
   // Set Local KeyFrame vertices
   N = vpOptimizableKFs.size();
-  for (int i = 0; i < N; i++) {
-    KeyFrame* pKFi = vpOptimizableKFs[i];
-
+  for (auto pKFi : vpOptimizableKFs) {
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
     VP->setFixed(false);
@@ -4117,9 +4039,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
 
   // Set Local cov keyframes vertices
   int Ncov = vpOptimizableCovKFs.size();
-  for (int i = 0; i < Ncov; i++) {
-    KeyFrame* pKFi = vpOptimizableCovKFs[i];
-
+  for (auto pKFi : vpOptimizableCovKFs) {
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
     VP->setFixed(false);
@@ -4142,10 +4062,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
   }
 
   // Set Fixed KeyFrame vertices
-  for (list<KeyFrame*>::iterator lit = lFixedKeyFrames.begin(),
-                                 lend = lFixedKeyFrames.end();
-       lit != lend; lit++) {
-    KeyFrame* pKFi = *lit;
+  for (auto pKFi : lFixedKeyFrames) {
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
     VP->setFixed(true);
@@ -4173,7 +4090,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
   vector<EdgeAccRW*> vear(N, nullptr);
   for (int i = 0; i < N; i++) {
     // cout << "inserting inertial edge " << i << endl;
-    KeyFrame* pKFi = vpOptimizableKFs[i];
+    std::shared_ptr<KeyFrame> pKFi = vpOptimizableKFs[i];
 
     if (!pKFi->mPrevKF) {
       Verbose::PrintMess("NO INERTIAL LINK TO PREVIOUS FRAME!!!!",
@@ -4252,7 +4169,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
   vector<EdgeMono*> vpEdgesMono;
   vpEdgesMono.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFMono;
+  vector<std::shared_ptr<KeyFrame>> vpEdgeKFMono;
   vpEdgeKFMono.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeMono;
@@ -4262,7 +4179,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
   vector<EdgeStereo*> vpEdgesStereo;
   vpEdgesStereo.reserve(nExpectedSize);
 
-  vector<KeyFrame*> vpEdgeKFStereo;
+  vector<std::shared_ptr<KeyFrame>> vpEdgeKFStereo;
   vpEdgeKFStereo.reserve(nExpectedSize);
 
   vector<MapPoint*> vpMapPointEdgeStereo;
@@ -4289,14 +4206,15 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
     vPoint->setMarginalized(true);
     optimizer.addVertex(vPoint);
 
-    const map<KeyFrame*, tuple<int, int>> observations = pMP->GetObservations();
+    const map<std::shared_ptr<KeyFrame>, tuple<int, int>> observations =
+        pMP->GetObservations();
 
     // Create visual constraints
-    for (map<KeyFrame*, tuple<int, int>>::const_iterator
+    for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::const_iterator
              mit = observations.begin(),
              mend = observations.end();
          mit != mend; mit++) {
-      KeyFrame* pKFi = mit->first;
+      std::shared_ptr<KeyFrame> pKFi = mit->first;
 
       if (!pKFi) continue;
 
@@ -4372,7 +4290,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
   optimizer.initializeOptimization();
   optimizer.optimize(8);
 
-  vector<pair<KeyFrame*, MapPoint*>> vToErase;
+  vector<pair<std::shared_ptr<KeyFrame>, MapPoint*>> vToErase;
   vToErase.reserve(vpEdgesMono.size() + vpEdgesStereo.size());
 
   // Check inlier observations
@@ -4384,7 +4302,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
     if (pMP->isBad()) continue;
 
     if (e->chi2() > chi2Mono2) {
-      KeyFrame* pKFi = vpEdgeKFMono[i];
+      std::shared_ptr<KeyFrame> pKFi = vpEdgeKFMono[i];
       vToErase.push_back(make_pair(pKFi, pMP));
     }
   }
@@ -4397,7 +4315,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
     if (pMP->isBad()) continue;
 
     if (e->chi2() > chi2Stereo2) {
-      KeyFrame* pKFi = vpEdgeKFStereo[i];
+      std::shared_ptr<KeyFrame> pKFi = vpEdgeKFStereo[i];
       vToErase.push_back(make_pair(pKFi, pMP));
     }
   }
@@ -4406,7 +4324,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
   unique_lock<mutex> lock(pMap->mMutexMapUpdate);
   if (!vToErase.empty()) {
     for (size_t i = 0; i < vToErase.size(); i++) {
-      KeyFrame* pKFi = vToErase[i].first;
+      std::shared_ptr<KeyFrame> pKFi = vToErase[i].first;
       MapPoint* pMPi = vToErase[i].second;
       pKFi->EraseMapPointMatch(pMPi);
       pMPi->EraseObservation(pKFi);
@@ -4415,9 +4333,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
 
   // Recover optimized data
   // Keyframes
-  for (int i = 0; i < N; i++) {
-    KeyFrame* pKFi = vpOptimizableKFs[i];
-
+  for (auto pKFi : vpOptimizableKFs) {
     VertexPose* VP = static_cast<VertexPose*>(optimizer.vertex(pKFi->mnId));
     Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast<float>(),
                      VP->estimate().tcw[0].cast<float>());
@@ -4441,9 +4357,7 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
     }
   }
 
-  for (int i = 0; i < Ncov; i++) {
-    KeyFrame* pKFi = vpOptimizableCovKFs[i];
-
+  for (auto pKFi : vpOptimizableCovKFs) {
     VertexPose* VP = static_cast<VertexPose*>(optimizer.vertex(pKFi->mnId));
     Sophus::SE3f Tcw(VP->estimate().Rcw[0].cast<float>(),
                      VP->estimate().tcw[0].cast<float>());
@@ -4481,8 +4395,8 @@ void Optimizer::MergeInertialBA(KeyFrame* pCurrKF, KeyFrame* pMergeKF,
   pMap->IncreaseChangeIndex();
 }
 
-int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame* pFrame,
-                                                    bool bRecInit) {
+int Optimizer::PoseInertialOptimizationLastKeyFrame(
+    const std::shared_ptr<Frame>& pFrame, bool bRecInit) {
   g2o::SparseOptimizer optimizer;
   optimizer.setVerbose(false);
 
@@ -4646,7 +4560,7 @@ int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame* pFrame,
   nInitialCorrespondences =
       nInitialMonoCorrespondences + nInitialStereoCorrespondences;
 
-  KeyFrame* pKF = pFrame->mpLastKeyFrame;
+  auto pKF = pFrame->mpLastKeyFrame;
   VertexPose* VPk = new VertexPose(pKF);
   VPk->setId(4);
   VPk->setFixed(true);
@@ -4855,7 +4769,8 @@ int Optimizer::PoseInertialOptimizationLastKeyFrame(Frame* pFrame,
   return nInitialCorrespondences - nBad;
 }
 
-int Optimizer::PoseInertialOptimizationLastFrame(Frame* pFrame, bool bRecInit) {
+int Optimizer::PoseInertialOptimizationLastFrame(
+    const std::shared_ptr<Frame>& pFrame, bool bRecInit) {
   g2o::SparseOptimizer optimizer;
 
   // Taken from g2o/examples/tutorial_slam2d/tutorial_slam2d.cpp
@@ -5022,7 +4937,7 @@ int Optimizer::PoseInertialOptimizationLastFrame(Frame* pFrame, bool bRecInit) {
       nInitialMonoCorrespondences + nInitialStereoCorrespondences;
 
   // Set Previous Frame Vertex
-  Frame* pFp = pFrame->mpPrevFrame;
+  std::shared_ptr<Frame> pFp = pFrame->mpPrevFrame;
 
   VertexPose* VPk = new VertexPose(pFp);
   VPk->setId(4);
@@ -5263,10 +5178,12 @@ int Optimizer::PoseInertialOptimizationLastFrame(Frame* pFrame, bool bRecInit) {
 }
 
 void Optimizer::OptimizeEssentialGraph4DoF(
-    const std::shared_ptr<Map>& pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
+    const std::shared_ptr<Map>& pMap, const std::shared_ptr<KeyFrame>& pLoopKF,
+    const std::shared_ptr<KeyFrame>& pCurKF,
     const LoopClosing::KeyFrameAndPose& NonCorrectedSim3,
     const LoopClosing::KeyFrameAndPose& CorrectedSim3,
-    const map<KeyFrame*, set<KeyFrame*>>& LoopConnections) {
+    const map<std::shared_ptr<KeyFrame>, set<std::shared_ptr<KeyFrame>>>&
+        LoopConnections) {
   // Setup optimizer
   g2o::SparseOptimizer optimizer;
   optimizer.setVerbose(false);
@@ -5289,7 +5206,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
 
   optimizer.setAlgorithm(solver);
 
-  const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
+  const vector<std::shared_ptr<KeyFrame>> vpKFs = pMap->GetAllKeyFrames();
   const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();
 
   const unsigned int nMaxKFid = pMap->GetMaxKFid();
@@ -5302,8 +5219,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
 
   const int minFeat = 100;
   // Set KeyFrame vertices
-  for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) {
-    KeyFrame* pKF = vpKFs[i];
+  for (auto pKF : vpKFs) {
     if (pKF->isBad()) continue;
 
     VertexPose4DoF* V4DoF;
@@ -5346,17 +5262,19 @@ void Optimizer::OptimizeEssentialGraph4DoF(
 
   // Set Loop edges
   Edge4DoF* e_loop;
-  for (map<KeyFrame*, set<KeyFrame*>>::const_iterator
+  for (map<std::shared_ptr<KeyFrame>,
+           set<std::shared_ptr<KeyFrame>>>::const_iterator
            mit = LoopConnections.begin(),
            mend = LoopConnections.end();
        mit != mend; mit++) {
-    KeyFrame* pKF = mit->first;
+    std::shared_ptr<KeyFrame> pKF = mit->first;
     const long unsigned int nIDi = pKF->mnId;
-    const set<KeyFrame*>& spConnections = mit->second;
+    const set<std::shared_ptr<KeyFrame>>& spConnections = mit->second;
     const g2o::Sim3 Siw = vScw[nIDi];
 
-    for (set<KeyFrame*>::const_iterator sit = spConnections.begin(),
-                                        send = spConnections.end();
+    for (set<std::shared_ptr<KeyFrame>>::const_iterator
+             sit = spConnections.begin(),
+             send = spConnections.end();
          sit != send; sit++) {
       const long unsigned int nIDj = (*sit)->mnId;
       if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) &&
@@ -5385,9 +5303,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
   }
 
   // 1. Set normal edges
-  for (size_t i = 0, iend = vpKFs.size(); i < iend; i++) {
-    KeyFrame* pKF = vpKFs[i];
-
+  for (auto pKF : vpKFs) {
     const int nIDi = pKF->mnId;
 
     g2o::Sim3 Siw;
@@ -5402,7 +5318,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
       Siw = vScw[nIDi];
 
     // 1.1.0 Spanning tree edge
-    KeyFrame* pParentKF = static_cast<KeyFrame*>(NULL);
+    std::shared_ptr<KeyFrame> pParentKF;
     if (pParentKF) {
       int nIDj = pParentKF->mnId;
 
@@ -5432,7 +5348,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
     }
 
     // 1.1.1 Inertial edges
-    KeyFrame* prevKF = pKF->mPrevKF;
+    std::shared_ptr<KeyFrame> prevKF = pKF->mPrevKF;
     if (prevKF) {
       int nIDj = prevKF->mnId;
 
@@ -5462,11 +5378,8 @@ void Optimizer::OptimizeEssentialGraph4DoF(
     }
 
     // 1.2 Loop edges
-    const set<KeyFrame*> sLoopEdges = pKF->GetLoopEdges();
-    for (set<KeyFrame*>::const_iterator sit = sLoopEdges.begin(),
-                                        send = sLoopEdges.end();
-         sit != send; sit++) {
-      KeyFrame* pLKF = *sit;
+    const auto sLoopEdges = pKF->GetLoopEdges();
+    for (auto pLKF : sLoopEdges) {
       if (pLKF->mnId < pKF->mnId) {
         g2o::Sim3 Swl;
 
@@ -5495,11 +5408,12 @@ void Optimizer::OptimizeEssentialGraph4DoF(
     }
 
     // 1.3 Covisibility graph edges
-    const vector<KeyFrame*> vpConnectedKFs =
+    const vector<std::shared_ptr<KeyFrame>> vpConnectedKFs =
         pKF->GetCovisiblesByWeight(minFeat);
-    for (vector<KeyFrame*>::const_iterator vit = vpConnectedKFs.begin();
+    for (vector<std::shared_ptr<KeyFrame>>::const_iterator vit =
+             vpConnectedKFs.begin();
          vit != vpConnectedKFs.end(); vit++) {
-      KeyFrame* pKFn = *vit;
+      std::shared_ptr<KeyFrame> pKFn = *vit;
       if (pKFn && pKFn != pParentKF && pKFn != prevKF && pKFn != pKF->mNextKF &&
           !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) {
         if (!pKFn->isBad() && pKFn->mnId < pKF->mnId) {
@@ -5541,9 +5455,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
   unique_lock<mutex> lock(pMap->mMutexMapUpdate);
 
   // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame* pKFi = vpKFs[i];
-
+  for (auto pKFi : vpKFs) {
     const int nIDi = pKFi->mnId;
 
     VertexPose4DoF* Vi = static_cast<VertexPose4DoF*>(optimizer.vertex(nIDi));
@@ -5566,7 +5478,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
 
     int nIDr;
 
-    KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
+    std::shared_ptr<KeyFrame> pRefKF = pMP->GetReferenceKeyFrame();
     nIDr = pRefKF->mnId;
 
     g2o::Sim3 Srw = vScw[nIDr];
diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc
index 577ea00c5c1..d54997f7d64 100644
--- a/src/Sim3Solver.cc
+++ b/src/Sim3Solver.cc
@@ -34,10 +34,11 @@
 
 namespace ORB_SLAM3 {
 
-Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2,
+Sim3Solver::Sim3Solver(const std::shared_ptr<KeyFrame> &pKF1,
+                       const std::shared_ptr<KeyFrame> &pKF2,
                        const vector<MapPoint *> &vpMatched12,
                        const bool bFixScale,
-                       vector<KeyFrame *> vpKeyFrameMatchedMP)
+                       vector<std::shared_ptr<KeyFrame>> vpKeyFrameMatchedMP)
     : mnIterations(0),
       mnBestInliers(0),
       mbFixScale(bFixScale),
@@ -46,7 +47,8 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2,
   bool bDifferentKFs = false;
   if (vpKeyFrameMatchedMP.empty()) {
     bDifferentKFs = true;
-    vpKeyFrameMatchedMP = vector<KeyFrame *>(vpMatched12.size(), pKF2);
+    vpKeyFrameMatchedMP =
+        vector<std::shared_ptr<KeyFrame>>(vpMatched12.size(), pKF2);
   }
 
   mpKF1 = pKF1;
@@ -72,7 +74,7 @@ Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2,
 
   size_t idx = 0;
 
-  KeyFrame *pKFm = pKF2;  // Default variable
+  std::shared_ptr<KeyFrame> pKFm = pKF2;  // Default variable
   for (int i1 = 0; i1 < mN1; i1++) {
     if (vpMatched12[i1]) {
       MapPoint *pMP1 = vpKeyFrameMP1[i1];
diff --git a/src/System.cc b/src/System.cc
index eb684a696f8..ff342ba6ffe 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -54,7 +54,15 @@ SystemFactory::Expected SystemFactory::create(
     return tl::make_unexpected(false);
   }
 
-  return std::make_shared<System>(settings);
+  // Cannot use make_shared with friend constructors?
+  auto sys = std::shared_ptr<System>(new System(settings));
+
+  // Initialization must occur separately because we use shared_from_this
+  if (!sys->initialize()) {
+    return tl::make_unexpected(false);
+  }
+
+  return sys;
 }
 
 SystemFactory::Expected SystemFactory::create(const std::string &configFile,
@@ -62,7 +70,7 @@ SystemFactory::Expected SystemFactory::create(const std::string &configFile,
   auto settings = SettingsLoader::load(configFile, sensor);
 
   if (settings) {
-    return std::make_shared<System>(settings.value());
+    return SystemFactory::create(settings.value());
   }
 
   return tl::make_unexpected(false);
@@ -103,7 +111,8 @@ SystemFactory::Expected SystemFactory::create(const std::string &configFile,
 
 System::System(const std::shared_ptr<Settings> &settings, bool initFr,
                const string &strSequence)
-    : mpViewer(),
+    : enable_shared_from_this<System>(),
+      mpViewer(),
       mbReset(false),
       mbResetActiveMap(false),
       mbActivateLocalizationMode(false),
@@ -111,7 +120,6 @@ System::System(const std::shared_ptr<Settings> &settings, bool initFr,
       mbShutDown(false),
       settings_(settings) {
   printBanner();
-  initialize(initFr, strSequence);
 }
 
 void System::printBanner() {
@@ -133,7 +141,7 @@ void System::printBanner() {
   spdlog::info("Input sensor was set to: {}", sensorType().toString());
 }
 
-void System::initialize(bool initFr, const string &strSequence) {
+bool System::initialize(bool initFr, const string &strSequence) {
   const string mStrLoadAtlasFromFile = settings_->atlasLoadFile();
   const string mStrSaveAtlasToFile = settings_->atlasSaveFile();
 
@@ -142,43 +150,26 @@ void System::initialize(bool initFr, const string &strSequence) {
   const bool activeLC = settings_->loopClosing_;
   const string vocabularyFilePath = settings_->strVocFile_;
 
-  bool loadedAtlas = false;
+  // Load ORB Vocabulary
+  spdlog::info("Loading ORB Vocabulary. This could take a while...");
 
-  if (mStrLoadAtlasFromFile.empty()) {
-    // Load ORB Vocabulary
-    spdlog::info("Loading ORB Vocabulary. This could take a while...");
-
-    mpVocabulary = std::make_shared<ORBVocabulary>();
-    bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyFilePath);
-    if (!bVocLoad) {
-      cerr << "Wrong path to vocabulary. " << endl;
-      cerr << "Falied to open at: " << vocabularyFilePath << endl;
-      exit(-1);
-    }
-    spdlog::info("Vocabulary loaded!");
+  mpVocabulary = std::make_shared<ORBVocabulary>();
+  bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyFilePath);
+  if (!bVocLoad) {
+    cerr << "Wrong path to vocabulary. " << endl;
+    cerr << "Falied to open at: " << vocabularyFilePath << endl;
+    return false;
+  }
+  spdlog::info("Vocabulary loaded!");
 
-    // Create KeyFrame Database
-    mpKeyFrameDatabase = std::make_shared<KeyFrameDatabase>(mpVocabulary);
+  // Create KeyFrame Database
+  mpKeyFrameDatabase = std::make_shared<KeyFrameDatabase>(mpVocabulary);
 
+  if (mStrLoadAtlasFromFile.empty()) {
     // Create the Atlas
     spdlog::info("Initialization of Atlas from scratch ");
     mpAtlas = std::make_shared<Atlas>(0);
   } else {
-    // Load ORB Vocabulary
-    spdlog::info("Loading ORB Vocabulary. This could take a while...");
-
-    mpVocabulary = std::make_shared<ORBVocabulary>();
-    bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyFilePath);
-    if (!bVocLoad) {
-      cerr << "Wrong path to vocabulary. " << endl;
-      cerr << "Falied to open at: " << vocabularyFilePath << endl;
-      exit(-1);
-    }
-    spdlog::info("Vocabulary loaded!");
-
-    // Create KeyFrame Database
-    mpKeyFrameDatabase = std::make_shared<KeyFrameDatabase>(mpVocabulary);
-
     // Load the file with an earlier session
     // clock_t start = clock();
     spdlog::info("Initialization of Atlas from file: {}",
@@ -189,27 +180,15 @@ void System::initialize(bool initFr, const string &strSequence) {
       cout << "Error to load the file, please try with other session file or "
               "vocabulary file"
            << endl;
-      exit(-1);
+      return false;
     }
-    // mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
-
-    // cout << "KF in DB: " << mpKeyFrameDatabase->mnNumKFs << "; words: " <<
-    // mpKeyFrameDatabase->mnNumWords << endl;
-
-    loadedAtlas = true;
 
     mpAtlas->CreateNewMap();
-
-    // clock_t timeElapsed = clock() - start;
-    // unsigned msElapsed = timeElapsed / (CLOCKS_PER_SEC / 1000);
-    // cout << "Binary file read in " << msElapsed << " ms" << endl;
-
-    // usleep(10*1000*1000);
   }
 
   if (sensorType().isImu()) mpAtlas->SetInertialSensor();
 
-  // Only draw left image in stereo modes
+  // Only draw right image in stereo modes
   const bool frame_drawer_both = sensorType().isStereo();
 
   // Create Drawers. These are used by the Viewer
@@ -221,13 +200,13 @@ void System::initialize(bool initFr, const string &strSequence) {
   // constructor)
   spdlog::info("Seq. Name: {}", strSequence);
   mpTracker = std::make_shared<Tracking>(
-      this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpAtlas,
+      shared_from_this(), mpVocabulary, mpFrameDrawer, mpMapDrawer, mpAtlas,
       mpKeyFrameDatabase, settings_, strSequence);
 
   // Initialize the Local Mapping thread and launch
-  mpLocalMapper =
-      std::make_shared<LocalMapping>(this, mpAtlas, sensorType().isMonocular(),
-                                     sensorType().isImu(), strSequence);
+  mpLocalMapper = std::make_shared<LocalMapping>(
+      shared_from_this(), mpAtlas, sensorType().isMonocular(),
+      sensorType().isImu(), strSequence);
   mptLocalMapping =
       std::make_unique<thread>(&ORB_SLAM3::LocalMapping::Run, mpLocalMapper);
   mpLocalMapper->mInitFr = initFr;
@@ -263,8 +242,6 @@ void System::initialize(bool initFr, const string &strSequence) {
   mpLoopCloser->SetTracker(mpTracker);
   mpLoopCloser->SetLocalMapper(mpLocalMapper);
 
-  // usleep(10*1000*1000);
-
   // Initialize the Viewer thread and launch
   if (settings_->useViewer_) {
     mpViewer = std::make_shared<Viewer>(this, mpFrameDrawer, mpMapDrawer,
@@ -277,7 +254,8 @@ void System::initialize(bool initFr, const string &strSequence) {
 
   // Fix verbosity
   Verbose::SetTh(Verbose::VERBOSITY_DEBUG);
-  // Verbose::SetTh(Verbose::VERBOSITY_QUIET);
+
+  return true;
 }
 
 Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight,
@@ -309,52 +287,21 @@ Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight,
   }
 
   // Check mode change
-  {
-    unique_lock<mutex> lock(mMutexMode);
-    if (mbActivateLocalizationMode) {
-      mpLocalMapper->RequestStop();
-
-      // Wait until Local Mapping has effectively stopped
-      while (!mpLocalMapper->isStopped()) {
-        usleep(1000);
-      }
-
-      mpTracker->InformOnlyTracking(true);
-      mbActivateLocalizationMode = false;
-    }
-    if (mbDeactivateLocalizationMode) {
-      mpTracker->InformOnlyTracking(false);
-      mpLocalMapper->Release();
-      mbDeactivateLocalizationMode = false;
-    }
-  }
+  processLocalizationModeChange();
 
   // Check reset
-  {
-    unique_lock<mutex> lock(mMutexReset);
-    if (mbReset) {
-      mpTracker->Reset();
-      mbReset = false;
-      mbResetActiveMap = false;
-    } else if (mbResetActiveMap) {
-      mpTracker->ResetActiveMap();
-      mbResetActiveMap = false;
-    }
-  }
+  processReset();
 
-  if (sensorType() == SensorType::IMU_STEREO) {
-    for (size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) {
-      mpTracker->GrabImuData(vImuMeas[i_imu]);
+  if (sensorType().isImu()) {
+    for (auto const &imuMeas : vImuMeas) {
+      mpTracker->GrabImuData(imuMeas);
     }
   }
 
   Sophus::SE3f Tcw = mpTracker->GrabImageStereo(imLeftToFeed, imRightToFeed,
                                                 timestamp, filename);
 
-  unique_lock<mutex> lock2(mMutexState);
-  mTrackingState = mpTracker->mState;
-  mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
-  mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+  updateTrackingState();
 
   return Tcw;
 }
@@ -379,53 +326,20 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap,
     cv::resize(depthmap, imDepthToFeed, settings_->newImSize());
   }
 
-  // Check mode change
-  {
-    unique_lock<mutex> lock(mMutexMode);
-    if (mbActivateLocalizationMode) {
-      mpLocalMapper->RequestStop();
-
-      // Wait until Local Mapping has effectively stopped
-      while (!mpLocalMapper->isStopped()) {
-        usleep(1000);
-      }
+  processLocalizationModeChange();
+  processReset();
 
-      mpTracker->InformOnlyTracking(true);
-      mbActivateLocalizationMode = false;
-    }
-    if (mbDeactivateLocalizationMode) {
-      mpTracker->InformOnlyTracking(false);
-      mpLocalMapper->Release();
-      mbDeactivateLocalizationMode = false;
-    }
-  }
-
-  // Check reset
-  {
-    unique_lock<mutex> lock(mMutexReset);
-    if (mbReset) {
-      mpTracker->Reset();
-      mbReset = false;
-      mbResetActiveMap = false;
-    } else if (mbResetActiveMap) {
-      mpTracker->ResetActiveMap();
-      mbResetActiveMap = false;
-    }
-  }
-
-  if (sensorType() == SensorType::IMU_RGBD) {
-    for (size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) {
-      mpTracker->GrabImuData(vImuMeas[i_imu]);
+  if (sensorType().isImu()) {
+    for (auto const &imuMeas : vImuMeas) {
+      mpTracker->GrabImuData(imuMeas);
     }
   }
 
   Sophus::SE3f Tcw =
       mpTracker->GrabImageRGBD(imToFeed, imDepthToFeed, timestamp, filename);
 
-  unique_lock<mutex> lock2(mMutexState);
-  mTrackingState = mpTracker->mState;
-  mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
-  mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+  updateTrackingState();
+
   return Tcw;
 }
 
@@ -452,57 +366,25 @@ Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double &timestamp,
   }
 
   // Check mode change
-  {
-    unique_lock<mutex> lock(mMutexMode);
-    if (mbActivateLocalizationMode) {
-      mpLocalMapper->RequestStop();
-
-      // Wait until Local Mapping has effectively stopped
-      while (!mpLocalMapper->isStopped()) {
-        usleep(1000);
-      }
-
-      mpTracker->InformOnlyTracking(true);
-      mbActivateLocalizationMode = false;
-    }
-    if (mbDeactivateLocalizationMode) {
-      mpTracker->InformOnlyTracking(false);
-      mpLocalMapper->Release();
-      mbDeactivateLocalizationMode = false;
-    }
-  }
+  processLocalizationModeChange();
+  processReset();
 
-  // Check reset
-  {
-    unique_lock<mutex> lock(mMutexReset);
-    if (mbReset) {
-      mpTracker->Reset();
-      mbReset = false;
-      mbResetActiveMap = false;
-    } else if (mbResetActiveMap) {
-      cout << "SYSTEM-> Reseting active map in monocular case" << endl;
-      mpTracker->ResetActiveMap();
-      mbResetActiveMap = false;
-    }
-  }
-
-  if (sensorType() == SensorType::IMU_MONOCULAR) {
-    for (size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++) {
-      mpTracker->GrabImuData(vImuMeas[i_imu]);
+  if (sensorType().isImu()) {
+    for (auto const &imuMeas : vImuMeas) {
+      mpTracker->GrabImuData(imuMeas);
     }
   }
 
   Sophus::SE3f Tcw =
       mpTracker->GrabImageMonocular(imToFeed, timestamp, filename);
 
-  unique_lock<mutex> lock2(mMutexState);
-  mTrackingState = mpTracker->mState;
-  mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
-  mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+  updateTrackingState();
 
   return Tcw;
 }
 
+//===============================================
+
 void System::ActivateLocalizationMode() {
   unique_lock<mutex> lock(mMutexMode);
   mbActivateLocalizationMode = true;
@@ -513,17 +395,28 @@ void System::DeactivateLocalizationMode() {
   mbDeactivateLocalizationMode = true;
 }
 
-bool System::MapChanged() {
-  static int n = 0;
-  int curn = mpAtlas->GetLastBigChangeIdx();
-  if (n < curn) {
-    n = curn;
-    return true;
-  } else {
-    return false;
+void System::processLocalizationModeChange() {
+  unique_lock<mutex> lock(mMutexMode);
+  if (mbActivateLocalizationMode) {
+    mpLocalMapper->RequestStop();
+
+    // Wait until Local Mapping has effectively stopped
+    while (!mpLocalMapper->isStopped()) {
+      usleep(1000);
+    }
+
+    mpTracker->InformOnlyTracking(true);
+    mbActivateLocalizationMode = false;
+  }
+  if (mbDeactivateLocalizationMode) {
+    mpTracker->InformOnlyTracking(false);
+    mpLocalMapper->Release();
+    mbDeactivateLocalizationMode = false;
   }
 }
 
+//===============================================
+
 void System::Reset() {
   unique_lock<mutex> lock(mMutexReset);
   mbReset = true;
@@ -534,6 +427,40 @@ void System::ResetActiveMap() {
   mbResetActiveMap = true;
 }
 
+void System::processReset() {
+  unique_lock<mutex> lock(mMutexReset);
+  if (mbReset) {
+    mpTracker->Reset();
+    mbReset = false;
+    mbResetActiveMap = false;
+  } else if (mbResetActiveMap) {
+    mpTracker->ResetActiveMap();
+    mbResetActiveMap = false;
+  }
+}
+
+//===============================================
+
+void System::updateTrackingState() {
+  unique_lock<mutex> lock2(mMutexState);
+  mTrackingState = mpTracker->mState;
+  mTrackedMapPoints = mpTracker->mCurrentFrame->mvpMapPoints;
+  mTrackedKeyPointsUn = mpTracker->mCurrentFrame->mvKeysUn;
+}
+
+//===============================================
+
+bool System::MapChanged() {
+  static int n = 0;
+  int curn = mpAtlas->GetLastBigChangeIdx();
+  if (n < curn) {
+    n = curn;
+    return true;
+  } else {
+    return false;
+  }
+}
+
 void System::Shutdown() {
   {
     unique_lock<mutex> lock(mMutexReset);
@@ -593,7 +520,7 @@ void System::SaveTrajectoryTUM(const string &filename) {
     return;
   }
 
-  vector<KeyFrame *> vpKFs = mpAtlas->GetAllKeyFrames();
+  vector<std::shared_ptr<KeyFrame>> vpKFs = mpAtlas->GetAllKeyFrames();
   sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
 
   // Transform all keyframes so that the first keyframe is at the origin.
@@ -611,7 +538,8 @@ void System::SaveTrajectoryTUM(const string &filename) {
 
   // For each frame we have a reference keyframe (lRit), the timestamp (lT) and
   // a flag which is true when tracking failed (lbL).
-  list<ORB_SLAM3::KeyFrame *>::iterator lRit = mpTracker->mlpReferences.begin();
+  list<std::shared_ptr<KeyFrame>>::iterator lRit =
+      mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   list<bool>::iterator lbL = mpTracker->mlbLost.begin();
   for (list<Sophus::SE3f>::iterator
@@ -620,7 +548,7 @@ void System::SaveTrajectoryTUM(const string &filename) {
        lit != lend; lit++, lRit++, lT++, lbL++) {
     if (*lbL) continue;
 
-    KeyFrame *pKF = *lRit;
+    std::shared_ptr<KeyFrame> pKF = *lRit;
 
     Sophus::SE3f Trw;
 
@@ -651,7 +579,7 @@ void System::SaveKeyFrameTrajectoryTUM(const string &filename) {
   cout << endl
        << "Saving keyframe trajectory to " << filename << " ..." << endl;
 
-  vector<KeyFrame *> vpKFs = mpAtlas->GetAllKeyFrames();
+  vector<std::shared_ptr<KeyFrame>> vpKFs = mpAtlas->GetAllKeyFrames();
   sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
 
   // Transform all keyframes so that the first keyframe is at the origin.
@@ -660,11 +588,7 @@ void System::SaveKeyFrameTrajectoryTUM(const string &filename) {
   f.open(filename.c_str());
   f << fixed;
 
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame *pKF = vpKFs[i];
-
-    // pKF->SetPose(pKF->GetPose()*Two);
-
+  for (auto pKF : vpKFs) {
     if (pKF->isBad()) continue;
 
     Sophus::SE3f Twc = pKF->GetPoseInverse();
@@ -701,7 +625,7 @@ void System::SaveTrajectoryEuRoC(const string &filename) {
     }
   }
 
-  vector<KeyFrame *> vpKFs = pBiggerMap->GetAllKeyFrames();
+  auto vpKFs = pBiggerMap->GetAllKeyFrames();
   sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
 
   // Transform all keyframes so that the first keyframe is at the origin.
@@ -726,7 +650,7 @@ void System::SaveTrajectoryEuRoC(const string &filename) {
 
   // For each frame we have a reference keyframe (lRit), the timestamp (lT) and
   // a flag which is true when tracking failed (lbL).
-  list<ORB_SLAM3::KeyFrame *>::iterator lRit = mpTracker->mlpReferences.begin();
+  auto lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   list<bool>::iterator lbL = mpTracker->mlbLost.begin();
 
@@ -742,7 +666,7 @@ void System::SaveTrajectoryEuRoC(const string &filename) {
     // cout << "1" << endl;
     if (*lbL) continue;
 
-    KeyFrame *pKF = *lRit;
+    std::shared_ptr<KeyFrame> pKF = *lRit;
     // cout << "KF: " << pKF->mnId << endl;
 
     Sophus::SE3f Trw;
@@ -808,7 +732,7 @@ void System::SaveTrajectoryEuRoC(const string &filename,
 
   int numMaxKFs = 0;
 
-  vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames();
+  auto vpKFs = pMap->GetAllKeyFrames();
   sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
 
   // Transform all keyframes so that the first keyframe is at the origin.
@@ -832,7 +756,7 @@ void System::SaveTrajectoryEuRoC(const string &filename,
 
   // For each frame we have a reference keyframe (lRit), the timestamp (lT) and
   // a flag which is true when tracking failed (lbL).
-  list<ORB_SLAM3::KeyFrame *>::iterator lRit = mpTracker->mlpReferences.begin();
+  auto lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   list<bool>::iterator lbL = mpTracker->mlbLost.begin();
 
@@ -848,7 +772,7 @@ void System::SaveTrajectoryEuRoC(const string &filename,
     // cout << "1" << endl;
     if (*lbL) continue;
 
-    KeyFrame *pKF = *lRit;
+    std::shared_ptr<KeyFrame> pKF = *lRit;
     // cout << "KF: " << pKF->mnId << endl;
 
     Sophus::SE3f Trw;
@@ -1113,7 +1037,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) {
     return;
   }
 
-  vector<KeyFrame *> vpKFs = pBiggerMap->GetAllKeyFrames();
+  auto vpKFs = pBiggerMap->GetAllKeyFrames();
   sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
 
   // Transform all keyframes so that the first keyframe is at the origin.
@@ -1122,9 +1046,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) {
   f.open(filename.c_str());
   f << fixed;
 
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame *pKF = vpKFs[i];
-
+  for (auto pKF : vpKFs) {
     // pKF->SetPose(pKF->GetPose()*Two);
 
     if (!pKF || pKF->isBad()) continue;
@@ -1154,7 +1076,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename,
        << "Saving keyframe trajectory of map " << pMap->GetId() << " to "
        << filename << " ..." << endl;
 
-  vector<KeyFrame *> vpKFs = pMap->GetAllKeyFrames();
+  auto vpKFs = pMap->GetAllKeyFrames();
   sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
 
   // Transform all keyframes so that the first keyframe is at the origin.
@@ -1163,10 +1085,9 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename,
   f.open(filename.c_str());
   f << fixed;
 
-  for (size_t i = 0; i < vpKFs.size(); i++) {
-    KeyFrame *pKF = vpKFs[i];
-
+  for (auto pKF : vpKFs) {
     if (!pKF || pKF->isBad()) continue;
+
     if (sensorType().isImu()) {
       Sophus::SE3f Twb = pKF->GetImuPose();
       Eigen::Quaternionf q = Twb.unit_quaternion();
@@ -1174,7 +1095,6 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename,
       f << setprecision(6) << 1e9 * pKF->mTimeStamp << " " << setprecision(9)
         << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " "
         << q.y() << " " << q.z() << " " << q.w() << endl;
-
     } else {
       Sophus::SE3f Twc = pKF->GetPoseInverse();
       Eigen::Quaternionf q = Twc.unit_quaternion();
@@ -1184,6 +1104,7 @@ void System::SaveKeyFrameTrajectoryEuRoC(const string &filename,
         << " " << q.z() << " " << q.w() << endl;
     }
   }
+
   f.close();
 }
 
@@ -1255,7 +1176,7 @@ void System::SaveTrajectoryKITTI(const string &filename) {
     return;
   }
 
-  vector<KeyFrame *> vpKFs = mpAtlas->GetAllKeyFrames();
+  auto vpKFs = mpAtlas->GetAllKeyFrames();
   sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
 
   // Transform all keyframes so that the first keyframe is at the origin.
@@ -1273,13 +1194,13 @@ void System::SaveTrajectoryKITTI(const string &filename) {
 
   // For each frame we have a reference keyframe (lRit), the timestamp (lT) and
   // a flag which is true when tracking failed (lbL).
-  list<ORB_SLAM3::KeyFrame *>::iterator lRit = mpTracker->mlpReferences.begin();
+  auto lRit = mpTracker->mlpReferences.begin();
   list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
   for (list<Sophus::SE3f>::iterator
            lit = mpTracker->mlRelativeFramePoses.begin(),
            lend = mpTracker->mlRelativeFramePoses.end();
        lit != lend; lit++, lRit++, lT++) {
-    ORB_SLAM3::KeyFrame *pKF = *lRit;
+    auto pKF = *lRit;
 
     Sophus::SE3f Trw;
 
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 099e9206f64..15ef7c0cb3a 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -47,14 +47,16 @@
 
 namespace ORB_SLAM3 {
 
-Tracking::Tracking(System* pSys, const std::shared_ptr<ORBVocabulary>& pVoc,
+Tracking::Tracking(const std::shared_ptr<System>& pSys,
+                   const std::shared_ptr<ORBVocabulary>& pVoc,
                    const std::shared_ptr<FrameDrawer>& pFrameDrawer,
                    const std::shared_ptr<MapDrawer>& pMapDrawer,
                    const std::shared_ptr<Atlas>& pAtlas,
                    const std::shared_ptr<KeyFrameDatabase>& pKFDB,
                    const std::shared_ptr<Settings>& settings,
                    const string& _nameSeq)
-    : mState(NO_IMAGES_YET),
+    : std::enable_shared_from_this<Tracking>(),
+      mState(NO_IMAGES_YET),
       mSensor(settings->sensor_),
       mTrackedFr(0),
       mbStep(false),
@@ -76,7 +78,7 @@ Tracking::Tracking(System* pSys, const std::shared_ptr<ORBVocabulary>& pVoc,
       mbCreatedMap(false),
       mnFirstFrameId(0),
       mpCamera2(nullptr),
-      mpLastKeyFrame(static_cast<KeyFrame*>(NULL)) {
+      mpLastKeyFrame() {
   newParameterLoader(settings);
 
   initID = 0;
@@ -563,13 +565,12 @@ void Tracking::newParameterLoader(const std::shared_ptr<Settings>& settings) {
     mpFrameDrawer->both = true;
   }
 
-  if (mSensor == SensorType::STEREO || mSensor == SensorType::RGBD ||
-      mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) {
+  if (mSensor.isStereo() || mSensor.isRGBD()) {
     mbf = settings->bf();
     mThDepth = settings->b() * settings->thDepth();
   }
 
-  if (mSensor == SensorType::RGBD || mSensor == SensorType::IMU_RGBD) {
+  if (mSensor.isRGBD()) {
     mDepthMapFactor = settings->depthMapFactor();
     if (fabs(mDepthMapFactor) < 1e-5)
       mDepthMapFactor = 1;
@@ -610,10 +611,10 @@ void Tracking::newParameterLoader(const std::shared_ptr<Settings>& settings) {
   float Naw = settings->accWalk();
 
   const float sf = sqrt(mImuFreq);
-  mpImuCalib = new IMU::Calib(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf);
+  mImuCalib.Set(Tbc, Ng * sf, Na * sf, Ngw / sf, Naw / sf);
 
   mpImuPreintegratedFromLastKF =
-      std::make_shared<IMU::Preintegrated>(IMU::Bias(), *mpImuCalib);
+      std::make_shared<IMU::Preintegrated>(IMU::Bias(), mImuCalib);
 }
 
 Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
@@ -653,32 +654,35 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
   // cout << "Incoming frame creation" << endl;
 
   if (mSensor == SensorType::STEREO && !mpCamera2)
-    mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
-                          mpORBextractorRight, mpORBVocabulary, mK, mDistCoef,
-                          mbf, mThDepth, mpCamera);
+    mCurrentFrame = std::make_shared<Frame>(
+        mImGray, imGrayRight, timestamp, mpORBextractorLeft,
+        mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth,
+        mpCamera);
   else if (mSensor == SensorType::STEREO && mpCamera2)
-    mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
-                          mpORBextractorRight, mpORBVocabulary, mK, mDistCoef,
-                          mbf, mThDepth, mpCamera, mpCamera2, mTlr);
+    mCurrentFrame = std::make_shared<Frame>(
+        mImGray, imGrayRight, timestamp, mpORBextractorLeft,
+        mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth,
+        mpCamera, mpCamera2, mTlr);
   else if (mSensor == SensorType::IMU_STEREO && !mpCamera2)
-    mCurrentFrame = Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
-                          mpORBextractorRight, mpORBVocabulary, mK, mDistCoef,
-                          mbf, mThDepth, mpCamera, &mLastFrame, *mpImuCalib);
+    mCurrentFrame = std::make_shared<Frame>(
+        mImGray, imGrayRight, timestamp, mpORBextractorLeft,
+        mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth,
+        mpCamera, mLastFrame, mImuCalib);
   else if (mSensor == SensorType::IMU_STEREO && mpCamera2)
-    mCurrentFrame =
-        Frame(mImGray, imGrayRight, timestamp, mpORBextractorLeft,
-              mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf,
-              mThDepth, mpCamera, mpCamera2, mTlr, &mLastFrame, *mpImuCalib);
+    mCurrentFrame = std::make_shared<Frame>(
+        mImGray, imGrayRight, timestamp, mpORBextractorLeft,
+        mpORBextractorRight, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth,
+        mpCamera, mpCamera2, mTlr, mLastFrame, mImuCalib);
 
   std::chrono::steady_clock::time_point t_post_frame_creation =
       std::chrono::steady_clock::now();
 
-  mCurrentFrame.mNameFile = filename;
-  mCurrentFrame.mnDataset = mnNumDataset;
+  mCurrentFrame->mNameFile = filename;
+  mCurrentFrame->mnDataset = mnNumDataset;
 
 #ifdef REGISTER_TIMES
-  vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext);
-  vdStereoMatch_ms.push_back(mCurrentFrame.mTimeStereoMatch);
+  vdORBExtract_ms.push_back(mCurrentFrame->mTimeORB_Ext);
+  vdStereoMatch_ms.push_back(mCurrentFrame->mTimeStereoMatch);
 #endif
 
   Track();
@@ -699,7 +703,7 @@ Sophus::SE3f Tracking::GrabImageStereo(const cv::Mat& imRectLeft,
           t_post_track - t_post_frame_creation)
           .count());
 
-  return mCurrentFrame.GetPose();
+  return mCurrentFrame->GetPose();
 }
 
 Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
@@ -723,24 +727,24 @@ Sophus::SE3f Tracking::GrabImageRGBD(const cv::Mat& imRGB, const cv::Mat& imD,
     imDepth.convertTo(imDepth, CV_32F, mDepthMapFactor);
 
   if (mSensor == SensorType::RGBD)
-    mCurrentFrame =
-        Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary,
-              mK, mDistCoef, mbf, mThDepth, mpCamera);
+    mCurrentFrame = std::make_shared<Frame>(
+        mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, mK,
+        mDistCoef, mbf, mThDepth, mpCamera);
   else if (mSensor == SensorType::IMU_RGBD)
-    mCurrentFrame =
-        Frame(mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary,
-              mK, mDistCoef, mbf, mThDepth, mpCamera, &mLastFrame, *mpImuCalib);
+    mCurrentFrame = std::make_shared<Frame>(
+        mImGray, imDepth, timestamp, mpORBextractorLeft, mpORBVocabulary, mK,
+        mDistCoef, mbf, mThDepth, mpCamera, mLastFrame, mImuCalib);
 
-  mCurrentFrame.mNameFile = filename;
-  mCurrentFrame.mnDataset = mnNumDataset;
+  mCurrentFrame->mNameFile = filename;
+  mCurrentFrame->mnDataset = mnNumDataset;
 
 #ifdef REGISTER_TIMES
-  vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext);
+  vdORBExtract_ms.push_back(mCurrentFrame->mTimeORB_Ext);
 #endif
 
   Track();
 
-  return mCurrentFrame.GetPose();
+  return mCurrentFrame->GetPose();
 }
 
 Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im,
@@ -762,39 +766,39 @@ Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat& im,
   if (mSensor == SensorType::MONOCULAR) {
     if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET ||
         (lastID - initID) < mMaxFrames) {
-      mCurrentFrame =
-          Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary,
-                mpCamera, mDistCoef, mbf, mThDepth);
+      mCurrentFrame = std::make_shared<Frame>(
+          mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, mpCamera,
+          mDistCoef, mbf, mThDepth);
     } else {
-      mCurrentFrame =
-          Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary,
-                mpCamera, mDistCoef, mbf, mThDepth);
+      mCurrentFrame = std::make_shared<Frame>(
+          mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mpCamera,
+          mDistCoef, mbf, mThDepth);
     }
   } else if (mSensor == SensorType::IMU_MONOCULAR) {
     if (mState == NOT_INITIALIZED || mState == NO_IMAGES_YET) {
-      mCurrentFrame =
-          Frame(mImGray, timestamp, mpIniORBextractor, mpORBVocabulary,
-                mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib);
+      mCurrentFrame = std::make_shared<Frame>(
+          mImGray, timestamp, mpIniORBextractor, mpORBVocabulary, mpCamera,
+          mDistCoef, mbf, mThDepth, mLastFrame, mImuCalib);
     } else {
-      mCurrentFrame =
-          Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary,
-                mpCamera, mDistCoef, mbf, mThDepth, &mLastFrame, *mpImuCalib);
+      mCurrentFrame = std::make_shared<Frame>(
+          mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mpCamera,
+          mDistCoef, mbf, mThDepth, mLastFrame, mImuCalib);
     }
   }
 
   if (mState == NO_IMAGES_YET) t0 = timestamp;
 
-  mCurrentFrame.mNameFile = filename;
-  mCurrentFrame.mnDataset = mnNumDataset;
+  mCurrentFrame->mNameFile = filename;
+  mCurrentFrame->mnDataset = mnNumDataset;
 
 #ifdef REGISTER_TIMES
-  vdORBExtract_ms.push_back(mCurrentFrame.mTimeORB_Ext);
+  vdORBExtract_ms.push_back(mCurrentFrame->mTimeORB_Ext);
 #endif
 
-  lastID = mCurrentFrame.mnId;
+  lastID = mCurrentFrame->mnId;
   Track();
 
-  return mCurrentFrame.GetPose();
+  return mCurrentFrame->GetPose();
 }
 
 void Tracking::GrabImuData(const IMU::Point& imuMeasurement) {
@@ -803,9 +807,9 @@ void Tracking::GrabImuData(const IMU::Point& imuMeasurement) {
 }
 
 void Tracking::PreintegrateIMU() {
-  if (!mCurrentFrame.mpPrevFrame) {
+  if (!mCurrentFrame->mpPrevFrame) {
     Verbose::PrintMess("non prev frame ", Verbose::VERBOSITY_NORMAL);
-    mCurrentFrame.setIntegrated();
+    mCurrentFrame->setIntegrated();
     return;
   }
 
@@ -814,7 +818,7 @@ void Tracking::PreintegrateIMU() {
   if (mlQueueImuData.size() == 0) {
     Verbose::PrintMess("No IMU data in mlQueueImuData!!",
                        Verbose::VERBOSITY_NORMAL);
-    mCurrentFrame.setIntegrated();
+    mCurrentFrame->setIntegrated();
     return;
   }
 
@@ -825,9 +829,9 @@ void Tracking::PreintegrateIMU() {
       if (!mlQueueImuData.empty()) {
         IMU::Point* m = &mlQueueImuData.front();
         cout.precision(17);
-        if (m->t < mCurrentFrame.mpPrevFrame->mTimeStamp - mImuPer) {
+        if (m->t < mCurrentFrame->mpPrevFrame->mTimeStamp - mImuPer) {
           mlQueueImuData.pop_front();
-        } else if (m->t < mCurrentFrame.mTimeStamp - mImuPer) {
+        } else if (m->t < mCurrentFrame->mTimeStamp - mImuPer) {
           mvImuFromLastFrame.push_back(*m);
           mlQueueImuData.pop_front();
         } else {
@@ -849,8 +853,8 @@ void Tracking::PreintegrateIMU() {
   }
 
   std::shared_ptr<IMU::Preintegrated> pImuPreintegratedFromLastFrame =
-      std::make_shared<IMU::Preintegrated>(mLastFrame.mImuBias,
-                                           mCurrentFrame.mImuCalib);
+      std::make_shared<IMU::Preintegrated>(mLastFrame->mImuBias,
+                                           mCurrentFrame->mImuCalib);
 
   for (int i = 0; i < n; i++) {
     float tstep;
@@ -858,7 +862,7 @@ void Tracking::PreintegrateIMU() {
     if ((i == 0) && (i < (n - 1))) {
       float tab = mvImuFromLastFrame[i + 1].t - mvImuFromLastFrame[i].t;
       float tini =
-          mvImuFromLastFrame[i].t - mCurrentFrame.mpPrevFrame->mTimeStamp;
+          mvImuFromLastFrame[i].t - mCurrentFrame->mpPrevFrame->mTimeStamp;
       acc = (mvImuFromLastFrame[i].a + mvImuFromLastFrame[i + 1].a -
              (mvImuFromLastFrame[i + 1].a - mvImuFromLastFrame[i].a) *
                  (tini / tab)) *
@@ -868,14 +872,14 @@ void Tracking::PreintegrateIMU() {
                     (tini / tab)) *
                0.5f;
       tstep =
-          mvImuFromLastFrame[i + 1].t - mCurrentFrame.mpPrevFrame->mTimeStamp;
+          mvImuFromLastFrame[i + 1].t - mCurrentFrame->mpPrevFrame->mTimeStamp;
     } else if (i < (n - 1)) {
       acc = (mvImuFromLastFrame[i].a + mvImuFromLastFrame[i + 1].a) * 0.5f;
       angVel = (mvImuFromLastFrame[i].w + mvImuFromLastFrame[i + 1].w) * 0.5f;
       tstep = mvImuFromLastFrame[i + 1].t - mvImuFromLastFrame[i].t;
     } else if ((i > 0) && (i == (n - 1))) {
       float tab = mvImuFromLastFrame[i + 1].t - mvImuFromLastFrame[i].t;
-      float tend = mvImuFromLastFrame[i + 1].t - mCurrentFrame.mTimeStamp;
+      float tend = mvImuFromLastFrame[i + 1].t - mCurrentFrame->mTimeStamp;
       acc = (mvImuFromLastFrame[i].a + mvImuFromLastFrame[i + 1].a -
              (mvImuFromLastFrame[i + 1].a - mvImuFromLastFrame[i].a) *
                  (tend / tab)) *
@@ -884,11 +888,12 @@ void Tracking::PreintegrateIMU() {
                 (mvImuFromLastFrame[i + 1].w - mvImuFromLastFrame[i].w) *
                     (tend / tab)) *
                0.5f;
-      tstep = mCurrentFrame.mTimeStamp - mvImuFromLastFrame[i].t;
+      tstep = mCurrentFrame->mTimeStamp - mvImuFromLastFrame[i].t;
     } else if ((i == 0) && (i == (n - 1))) {
       acc = mvImuFromLastFrame[i].a;
       angVel = mvImuFromLastFrame[i].w;
-      tstep = mCurrentFrame.mTimeStamp - mCurrentFrame.mpPrevFrame->mTimeStamp;
+      tstep =
+          mCurrentFrame->mTimeStamp - mCurrentFrame->mpPrevFrame->mTimeStamp;
     }
 
     if (!mpImuPreintegratedFromLastKF)
@@ -897,18 +902,18 @@ void Tracking::PreintegrateIMU() {
     pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc, angVel, tstep);
   }
 
-  mCurrentFrame.mpImuPreintegratedFrame = pImuPreintegratedFromLastFrame;
-  mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
-  mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame;
+  mCurrentFrame->mpImuPreintegratedFrame = pImuPreintegratedFromLastFrame;
+  mCurrentFrame->mpImuPreintegrated = mpImuPreintegratedFromLastKF;
+  mCurrentFrame->mpLastKeyFrame = mpLastKeyFrame;
 
-  mCurrentFrame.setIntegrated();
+  mCurrentFrame->setIntegrated();
 
   // Verbose::PrintMess("Preintegration is finished!! ",
   // Verbose::VERBOSITY_DEBUG);
 }
 
 bool Tracking::PredictStateIMU() {
-  if (!mCurrentFrame.mpPrevFrame) {
+  if (!mCurrentFrame->mpPrevFrame) {
     Verbose::PrintMess("No last frame", Verbose::VERBOSITY_NORMAL);
     return false;
   }
@@ -932,34 +937,34 @@ bool Tracking::PredictStateIMU() {
         Vwb1 + t12 * Gz +
         Rwb1 * mpImuPreintegratedFromLastKF->GetDeltaVelocity(
                    mpLastKeyFrame->GetImuBias());
-    mCurrentFrame.SetImuPoseVelocity(Rwb2, twb2, Vwb2);
+    mCurrentFrame->SetImuPoseVelocity(Rwb2, twb2, Vwb2);
 
-    mCurrentFrame.mImuBias = mpLastKeyFrame->GetImuBias();
-    mCurrentFrame.mPredBias = mCurrentFrame.mImuBias;
+    mCurrentFrame->mImuBias = mpLastKeyFrame->GetImuBias();
+    mCurrentFrame->mPredBias = mCurrentFrame->mImuBias;
     return true;
   } else if (!mbMapUpdated) {
-    const Eigen::Vector3f twb1 = mLastFrame.GetImuPosition();
-    const Eigen::Matrix3f Rwb1 = mLastFrame.GetImuRotation();
-    const Eigen::Vector3f Vwb1 = mLastFrame.GetVelocity();
+    const Eigen::Vector3f twb1 = mLastFrame->GetImuPosition();
+    const Eigen::Matrix3f Rwb1 = mLastFrame->GetImuRotation();
+    const Eigen::Vector3f Vwb1 = mLastFrame->GetVelocity();
     const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE);
-    const float t12 = mCurrentFrame.mpImuPreintegratedFrame->dT;
+    const float t12 = mCurrentFrame->mpImuPreintegratedFrame->dT;
 
     Eigen::Matrix3f Rwb2 = IMU::NormalizeRotation(
-        Rwb1 * mCurrentFrame.mpImuPreintegratedFrame->GetDeltaRotation(
-                   mLastFrame.mImuBias));
+        Rwb1 * mCurrentFrame->mpImuPreintegratedFrame->GetDeltaRotation(
+                   mLastFrame->mImuBias));
     Eigen::Vector3f twb2 =
         twb1 + Vwb1 * t12 + 0.5f * t12 * t12 * Gz +
-        Rwb1 * mCurrentFrame.mpImuPreintegratedFrame->GetDeltaPosition(
-                   mLastFrame.mImuBias);
+        Rwb1 * mCurrentFrame->mpImuPreintegratedFrame->GetDeltaPosition(
+                   mLastFrame->mImuBias);
     Eigen::Vector3f Vwb2 =
         Vwb1 + t12 * Gz +
-        Rwb1 * mCurrentFrame.mpImuPreintegratedFrame->GetDeltaVelocity(
-                   mLastFrame.mImuBias);
+        Rwb1 * mCurrentFrame->mpImuPreintegratedFrame->GetDeltaVelocity(
+                   mLastFrame->mImuBias);
 
-    mCurrentFrame.SetImuPoseVelocity(Rwb2, twb2, Vwb2);
+    mCurrentFrame->SetImuPoseVelocity(Rwb2, twb2, Vwb2);
 
-    mCurrentFrame.mImuBias = mLastFrame.mImuBias;
-    mCurrentFrame.mPredBias = mCurrentFrame.mImuBias;
+    mCurrentFrame->mImuBias = mLastFrame->mImuBias;
+    mCurrentFrame->mPredBias = mCurrentFrame->mImuBias;
     return true;
   } else {
     cout << "not IMU prediction!!" << endl;
@@ -995,7 +1000,7 @@ void Tracking::Track() {
   }
 
   if (mState != NO_IMAGES_YET) {
-    if (mLastFrame.mTimeStamp > mCurrentFrame.mTimeStamp) {
+    if (mLastFrame->mTimeStamp > mCurrentFrame->mTimeStamp) {
       cerr
           << "ERROR: Frame with a timestamp older than previous frame detected!"
           << endl;
@@ -1003,10 +1008,10 @@ void Tracking::Track() {
       mlQueueImuData.clear();
       CreateMapInAtlas();
       return;
-    } else if (mCurrentFrame.mTimeStamp > mLastFrame.mTimeStamp + 1.0) {
-      // cout << mCurrentFrame.mTimeStamp << ", " << mLastFrame.mTimeStamp <<
-      // endl; cout << "id last: " << mLastFrame.mnId << "    id curr: " <<
-      // mCurrentFrame.mnId << endl;
+    } else if (mCurrentFrame->mTimeStamp > mLastFrame->mTimeStamp + 1.0) {
+      // cout << mCurrentFrame->mTimeStamp << ", " << mLastFrame->mTimeStamp <<
+      // endl; cout << "id last: " << mLastFrame->mnId << "    id curr: " <<
+      // mCurrentFrame->mnId << endl;
       if (mpAtlas->isInertial()) {
         if (mpAtlas->isImuInitialized()) {
           cout << "Timestamp jump detected. State set to LOST. resetting IMU "
@@ -1028,10 +1033,8 @@ void Tracking::Track() {
     }
   }
 
-  if ((mSensor == SensorType::IMU_MONOCULAR ||
-       mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) &&
-      mpLastKeyFrame) {
-    mCurrentFrame.SetNewBias(mpLastKeyFrame->GetImuBias());
+  if (mSensor.isImu() && mpLastKeyFrame) {
+    mCurrentFrame->SetNewBias(mpLastKeyFrame->GetImuBias());
   }
 
   if (mState == NO_IMAGES_YET) {
@@ -1040,9 +1043,7 @@ void Tracking::Track() {
 
   mLastProcessedState = mState;
 
-  if ((mSensor == SensorType::IMU_MONOCULAR ||
-       mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) &&
-      !mbCreatedMap) {
+  if (mSensor.isImu() && !mbCreatedMap) {
 #ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_StartPreIMU =
         std::chrono::steady_clock::now();
@@ -1074,8 +1075,7 @@ void Tracking::Track() {
   }
 
   if (mState == NOT_INITIALIZED) {
-    if (mSensor == SensorType::STEREO || mSensor == SensorType::RGBD ||
-        mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) {
+    if (mSensor.isStereo() || mSensor.isRGBD()) {
       StereoInitialization();
     } else {
       MonocularInitialization();
@@ -1085,12 +1085,12 @@ void Tracking::Track() {
 
     // If rightly initialized, mState=OK
     if (mState != OK) {
-      mLastFrame = Frame(mCurrentFrame);
+      mLastFrame = std::make_shared<Frame>(*mCurrentFrame);
       return;
     }
 
     if (mpAtlas->GetAllMaps().size() == 1) {
-      mnFirstFrameId = mCurrentFrame.mnId;
+      mnFirstFrameId = mCurrentFrame->mnId;
     }
   } else {
     // System is initialized. Track Frame.
@@ -1115,7 +1115,7 @@ void Tracking::Track() {
         CheckReplacedInLastFrame();
 
         if ((!mbVelocity && !pCurrentMap->isImuInitialized()) ||
-            mCurrentFrame.mnId < mnLastRelocFrameId + 2) {
+            mCurrentFrame->mnId < mnLastRelocFrameId + 2) {
           spdlog::info("TRACK: Track with respect to the reference KF");
           bOK = TrackReferenceKeyFrame();
         } else {
@@ -1131,14 +1131,15 @@ void Tracking::Track() {
 
         if (!bOK) {
           spdlog::info("TRACK:   Tracking still bad, I am lost!");
-          if (mCurrentFrame.mnId <= (mnLastRelocFrameId + mnFramesToResetIMU) &&
+          if (mCurrentFrame->mnId <=
+                  (mnLastRelocFrameId + mnFramesToResetIMU) &&
               (mSensor == SensorType::IMU_MONOCULAR ||
                mSensor == SensorType::IMU_STEREO ||
                mSensor == SensorType::IMU_RGBD)) {
             mState = LOST;
           } else if (pCurrentMap->KeyFramesInMap() > 10) {
             mState = RECENTLY_LOST;
-            mTimeStampLost = mCurrentFrame.mTimeStamp;
+            mTimeStampLost = mCurrentFrame->mTimeStamp;
           } else {
             mState = LOST;
           }
@@ -1149,15 +1150,13 @@ void Tracking::Track() {
                              Verbose::VERBOSITY_NORMAL);
 
           bOK = true;
-          if ((mSensor == SensorType::IMU_MONOCULAR ||
-               mSensor == SensorType::IMU_STEREO ||
-               mSensor == SensorType::IMU_RGBD)) {
+          if (mSensor.isImu()) {
             if (pCurrentMap->isImuInitialized())
               PredictStateIMU();
             else
               bOK = false;
 
-            if (mCurrentFrame.mTimeStamp - mTimeStampLost >
+            if (mCurrentFrame->mTimeStamp - mTimeStampLost >
                 time_recently_lost) {
               mState = LOST;
               Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
@@ -1166,10 +1165,10 @@ void Tracking::Track() {
           } else {
             // Relocalization
             bOK = Relocalization();
-            // std::cout << "mCurrentFrame.mTimeStamp:" <<
-            // to_string(mCurrentFrame.mTimeStamp) << std::endl; std::cout <<
+            // std::cout << "mCurrentFrame->mTimeStamp:" <<
+            // to_string(mCurrentFrame->mTimeStamp) << std::endl; std::cout <<
             // "mTimeStampLost:" << to_string(mTimeStampLost) << std::endl;
-            if (mCurrentFrame.mTimeStamp - mTimeStampLost > 3.0f && !bOK) {
+            if (mCurrentFrame->mTimeStamp - mTimeStampLost > 3.0f && !bOK) {
               mState = LOST;
               Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
               bOK = false;
@@ -1187,7 +1186,7 @@ void Tracking::Track() {
             CreateMapInAtlas();
           }
 
-          if (mpLastKeyFrame) mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
+          if (mpLastKeyFrame) mpLastKeyFrame.reset();
 
           return;
         }
@@ -1205,9 +1204,7 @@ void Tracking::Track() {
       // Localization Mode: Local Mapping is deactivated (TODO Not available in
       // inertial mode)
       if (mState == LOST) {
-        if (mSensor == SensorType::IMU_MONOCULAR ||
-            mSensor == SensorType::IMU_STEREO ||
-            mSensor == SensorType::IMU_RGBD)
+        if (mSensor.isImu())
           Verbose::PrintMess("IMU. State LOST", Verbose::VERBOSITY_NORMAL);
         bOK = Relocalization();
       } else {
@@ -1237,22 +1234,22 @@ void Tracking::Track() {
           Sophus::SE3f TcwMM;
           if (mbVelocity) {
             bOKMM = TrackWithMotionModel();
-            vpMPsMM = mCurrentFrame.mvpMapPoints;
-            vbOutMM = mCurrentFrame.mvbOutlier;
-            TcwMM = mCurrentFrame.GetPose();
+            vpMPsMM = mCurrentFrame->mvpMapPoints;
+            vbOutMM = mCurrentFrame->mvbOutlier;
+            TcwMM = mCurrentFrame->GetPose();
           }
           bOKReloc = Relocalization();
 
           if (bOKMM && !bOKReloc) {
-            mCurrentFrame.SetPose(TcwMM);
-            mCurrentFrame.mvpMapPoints = vpMPsMM;
-            mCurrentFrame.mvbOutlier = vbOutMM;
+            mCurrentFrame->SetPose(TcwMM);
+            mCurrentFrame->mvpMapPoints = vpMPsMM;
+            mCurrentFrame->mvbOutlier = vbOutMM;
 
             if (mbVO) {
-              for (int i = 0; i < mCurrentFrame.N; i++) {
-                if (mCurrentFrame.mvpMapPoints[i] &&
-                    !mCurrentFrame.mvbOutlier[i]) {
-                  mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
+              for (int i = 0; i < mCurrentFrame->N; i++) {
+                if (mCurrentFrame->mvpMapPoints[i] &&
+                    !mCurrentFrame->mvbOutlier[i]) {
+                  mCurrentFrame->mvpMapPoints[i]->IncreaseFound();
                 }
               }
             }
@@ -1275,8 +1272,8 @@ void Tracking::Track() {
               .count());
     }
 
-    if (!mCurrentFrame.mpReferenceKF)
-      mCurrentFrame.mpReferenceKF = mpReferenceKF;
+    if (!mCurrentFrame->mpReferenceKF)
+      mCurrentFrame->mpReferenceKF = mpReferenceKF;
 
 #ifdef REGISTER_TIMES
     std::chrono::steady_clock::time_point time_EndPosePred =
@@ -1335,26 +1332,24 @@ void Tracking::Track() {
         mState = RECENTLY_LOST;  // visual to lost
       }
 
-      /*if(mCurrentFrame.mnId>mnLastRelocFrameId+mMaxFrames)
+      /*if(mCurrentFrame->mnId>mnLastRelocFrameId+mMaxFrames)
       {*/
-      mTimeStampLost = mCurrentFrame.mTimeStamp;
+      mTimeStampLost = mCurrentFrame->mTimeStamp;
       //}
     }
 
     // Save frame if recent relocalization, since they are used for IMU reset
     // (as we are making copy, it shluld be once mCurrFrame is completely
     // modified)
-    if ((mCurrentFrame.mnId < (mnLastRelocFrameId + mnFramesToResetIMU)) &&
-        (mCurrentFrame.mnId > mnFramesToResetIMU) &&
-        (mSensor == SensorType::IMU_MONOCULAR ||
-         mSensor == SensorType::IMU_STEREO ||
-         mSensor == SensorType::IMU_RGBD) &&
+    if ((mCurrentFrame->mnId < (mnLastRelocFrameId + mnFramesToResetIMU)) &&
+        (mCurrentFrame->mnId > mnFramesToResetIMU) && mSensor.isImu() &&
         pCurrentMap->isImuInitialized()) {
-      // TODO check this situation
       Verbose::PrintMess("Saving pointer to frame. imu needs reset...",
                          Verbose::VERBOSITY_NORMAL);
-      Frame* pF = new Frame(mCurrentFrame);
-      pF->mpPrevFrame = new Frame(mLastFrame);
+
+      // \todo{} check this situation.  This is a deep copy
+      std::shared_ptr<Frame> pF = std::make_shared<Frame>(*mCurrentFrame);
+      pF->mpPrevFrame = std::make_shared<Frame>(*mLastFrame);
 
       // Load preintegration
       //
@@ -1362,16 +1357,16 @@ void Tracking::Track() {
       // the Preintegration?
       pF->mpImuPreintegratedFrame = std::make_shared<IMU::Preintegrated>();
       pF->mpImuPreintegratedFrame->CopyFrom(
-          mCurrentFrame.mpImuPreintegratedFrame);
+          mCurrentFrame->mpImuPreintegratedFrame);
     }
 
     if (pCurrentMap->isImuInitialized()) {
       if (bOK) {
-        if (mCurrentFrame.mnId == (mnLastRelocFrameId + mnFramesToResetIMU)) {
+        if (mCurrentFrame->mnId == (mnLastRelocFrameId + mnFramesToResetIMU)) {
           cout << "resetting FRAME!!!" << endl;
           ResetFrameIMU();
-        } else if (mCurrentFrame.mnId > (mnLastRelocFrameId + 30)) {
-          mLastBias = mCurrentFrame.mImuBias;
+        } else if (mCurrentFrame->mnId > (mnLastRelocFrameId + 30)) {
+          mLastBias = mCurrentFrame->mImuBias;
         }
       }
     }
@@ -1388,31 +1383,30 @@ void Tracking::Track() {
 #endif
 
     // Update drawer
-    mpFrameDrawer->Update(this);
-    if (mCurrentFrame.isSet())
-      mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose());
+    mpFrameDrawer->Update(shared_from_this());
+    if (mCurrentFrame->isSet())
+      mpMapDrawer->SetCurrentCameraPose(mCurrentFrame->GetPose());
 
     if (bOK || mState == RECENTLY_LOST) {
       // Update motion model
-      if (mLastFrame.isSet() && mCurrentFrame.isSet()) {
-        Sophus::SE3f LastTwc = mLastFrame.GetPose().inverse();
-        mVelocity = mCurrentFrame.GetPose() * LastTwc;
+      if (mLastFrame->isSet() && mCurrentFrame->isSet()) {
+        Sophus::SE3f LastTwc = mLastFrame->GetPose().inverse();
+        mVelocity = mCurrentFrame->GetPose() * LastTwc;
         mbVelocity = true;
       } else {
         mbVelocity = false;
       }
 
-      if (mSensor == SensorType::IMU_MONOCULAR ||
-          mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD)
-        mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose());
+      if (mSensor.isImu())
+        mpMapDrawer->SetCurrentCameraPose(mCurrentFrame->GetPose());
 
       // Clean VO matches
-      for (int i = 0; i < mCurrentFrame.N; i++) {
-        MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
+      for (int i = 0; i < mCurrentFrame->N; i++) {
+        MapPoint* pMP = mCurrentFrame->mvpMapPoints[i];
         if (pMP) {
           if (pMP->Observations() < 1) {
-            mCurrentFrame.mvbOutlier[i] = false;
-            mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+            mCurrentFrame->mvbOutlier[i] = false;
+            mCurrentFrame->mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
           }
         }
       }
@@ -1435,9 +1429,7 @@ void Tracking::Track() {
       // Check if we need to insert a new keyframe
       // if(bNeedKF && bOK)
       if (bNeedKF && (bOK || (mInsertKFsLost && mState == RECENTLY_LOST &&
-                              (mSensor == SensorType::IMU_MONOCULAR ||
-                               mSensor == SensorType::IMU_STEREO ||
-                               mSensor == SensorType::IMU_RGBD))))
+                              mSensor.isImu())))
         CreateNewKeyFrame();
 
 #ifdef REGISTER_TIMES
@@ -1456,9 +1448,9 @@ void Tracking::Track() {
       // finally decide if they are outliers or not. We don't want next frame to
       // estimate its position with those points so we discard them in the
       // frame. Only has effect if lastframe is tracked
-      for (int i = 0; i < mCurrentFrame.N; i++) {
-        if (mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
-          mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+      for (int i = 0; i < mCurrentFrame->N; i++) {
+        if (mCurrentFrame->mvpMapPoints[i] && mCurrentFrame->mvbOutlier[i])
+          mCurrentFrame->mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
       }
     }
 
@@ -1483,21 +1475,21 @@ void Tracking::Track() {
       return;
     }
 
-    if (!mCurrentFrame.mpReferenceKF)
-      mCurrentFrame.mpReferenceKF = mpReferenceKF;
+    if (!mCurrentFrame->mpReferenceKF)
+      mCurrentFrame->mpReferenceKF = mpReferenceKF;
 
-    mLastFrame = Frame(mCurrentFrame);
+    mLastFrame = std::make_shared<Frame>(*mCurrentFrame);
   }
 
   if (mState == OK || mState == RECENTLY_LOST) {
     // Store frame pose information to retrieve the complete camera trajectory
     // afterwards.
-    if (mCurrentFrame.isSet()) {
-      Sophus::SE3f Tcr_ = mCurrentFrame.GetPose() *
-                          mCurrentFrame.mpReferenceKF->GetPoseInverse();
+    if (mCurrentFrame->isSet()) {
+      Sophus::SE3f Tcr_ = mCurrentFrame->GetPose() *
+                          mCurrentFrame->mpReferenceKF->GetPoseInverse();
       mlRelativeFramePoses.push_back(Tcr_);
-      mlpReferences.push_back(mCurrentFrame.mpReferenceKF);
-      mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
+      mlpReferences.push_back(mCurrentFrame->mpReferenceKF);
+      mlFrameTimes.push_back(mCurrentFrame->mTimeStamp);
       mlbLost.push_back(mState == LOST);
     } else {
       // This can happen if tracking is lost
@@ -1519,50 +1511,51 @@ void Tracking::Track() {
 }
 
 void Tracking::StereoInitialization() {
-  if (mCurrentFrame.N > 500) {
+  if (mCurrentFrame->N > 500) {
     if (mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) {
-      if (!mCurrentFrame.mpImuPreintegrated || !mLastFrame.mpImuPreintegrated) {
+      if (!mCurrentFrame->mpImuPreintegrated ||
+          !mLastFrame->mpImuPreintegrated) {
         spdlog::warn("No IMU measurements to initialize this map");
         return;
       }
 
-      if (!mFastInit && (mCurrentFrame.mpImuPreintegratedFrame->avgA -
-                         mLastFrame.mpImuPreintegratedFrame->avgA)
+      if (!mFastInit && (mCurrentFrame->mpImuPreintegratedFrame->avgA -
+                         mLastFrame->mpImuPreintegratedFrame->avgA)
                                 .norm() < 0.5) {
         spdlog::warn("not enough acceleration to initialize");
         return;
       }
 
       mpImuPreintegratedFromLastKF =
-          std::make_shared<IMU::Preintegrated>(IMU::Bias(), *mpImuCalib);
-      mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
+          std::make_shared<IMU::Preintegrated>(IMU::Bias(), mImuCalib);
+      mCurrentFrame->mpImuPreintegrated = mpImuPreintegratedFromLastKF;
     }
 
     // Set Frame pose to the origin (In case of inertial SLAM to imu)
     if (mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) {
-      Eigen::Matrix3f Rwb0 = mCurrentFrame.mImuCalib.mTcb.rotationMatrix();
-      Eigen::Vector3f twb0 = mCurrentFrame.mImuCalib.mTcb.translation();
+      Eigen::Matrix3f Rwb0 = mCurrentFrame->mImuCalib.mTcb.rotationMatrix();
+      Eigen::Vector3f twb0 = mCurrentFrame->mImuCalib.mTcb.translation();
       Eigen::Vector3f Vwb0;
       Vwb0.setZero();
-      mCurrentFrame.SetImuPoseVelocity(Rwb0, twb0, Vwb0);
+      mCurrentFrame->SetImuPoseVelocity(Rwb0, twb0, Vwb0);
     } else {
-      mCurrentFrame.SetPose(Sophus::SE3f());
+      mCurrentFrame->SetPose(Sophus::SE3f());
     }
 
     // Create KeyFrame
-    KeyFrame* pKFini =
-        new KeyFrame(mCurrentFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
+    std::shared_ptr<KeyFrame> pKFini = std::make_shared<KeyFrame>(
+        mCurrentFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
 
     // Insert KeyFrame in the map
     mpAtlas->AddKeyFrame(pKFini);
 
     // Create MapPoints and asscoiate to KeyFrame
     if (!mpCamera2) {
-      for (int i = 0; i < mCurrentFrame.N; i++) {
-        float z = mCurrentFrame.mvDepth[i];
+      for (int i = 0; i < mCurrentFrame->N; i++) {
+        float z = mCurrentFrame->mvDepth[i];
         if (z > 0) {
           Eigen::Vector3f x3D;
-          mCurrentFrame.UnprojectStereo(i, x3D);
+          mCurrentFrame->UnprojectStereo(i, x3D);
           MapPoint* pNewMP =
               new MapPoint(x3D, pKFini, mpAtlas->GetCurrentMap());
           pNewMP->AddObservation(pKFini, i);
@@ -1571,30 +1564,31 @@ void Tracking::StereoInitialization() {
           pNewMP->UpdateNormalAndDepth();
           mpAtlas->AddMapPoint(pNewMP);
 
-          mCurrentFrame.mvpMapPoints[i] = pNewMP;
+          mCurrentFrame->mvpMapPoints[i] = pNewMP;
         }
       }
     } else {
-      for (int i = 0; i < mCurrentFrame.Nleft; i++) {
-        int rightIndex = mCurrentFrame.mvLeftToRightMatch[i];
+      for (int i = 0; i < mCurrentFrame->Nleft; i++) {
+        int rightIndex = mCurrentFrame->mvLeftToRightMatch[i];
         if (rightIndex != -1) {
-          Eigen::Vector3f x3D = mCurrentFrame.mvStereo3Dpoints[i];
+          Eigen::Vector3f x3D = mCurrentFrame->mvStereo3Dpoints[i];
 
           MapPoint* pNewMP =
               new MapPoint(x3D, pKFini, mpAtlas->GetCurrentMap());
 
           pNewMP->AddObservation(pKFini, i);
-          pNewMP->AddObservation(pKFini, rightIndex + mCurrentFrame.Nleft);
+          pNewMP->AddObservation(pKFini, rightIndex + mCurrentFrame->Nleft);
 
           pKFini->AddMapPoint(pNewMP, i);
-          pKFini->AddMapPoint(pNewMP, rightIndex + mCurrentFrame.Nleft);
+          pKFini->AddMapPoint(pNewMP, rightIndex + mCurrentFrame->Nleft);
 
           pNewMP->ComputeDistinctiveDescriptors();
           pNewMP->UpdateNormalAndDepth();
           mpAtlas->AddMapPoint(pNewMP);
 
-          mCurrentFrame.mvpMapPoints[i] = pNewMP;
-          mCurrentFrame.mvpMapPoints[rightIndex + mCurrentFrame.Nleft] = pNewMP;
+          mCurrentFrame->mvpMapPoints[i] = pNewMP;
+          mCurrentFrame->mvpMapPoints[rightIndex + mCurrentFrame->Nleft] =
+              pNewMP;
         }
       }
     }
@@ -1607,21 +1601,21 @@ void Tracking::StereoInitialization() {
 
     mpLocalMapper->InsertKeyFrame(pKFini);
 
-    mLastFrame = Frame(mCurrentFrame);
-    mnLastKeyFrameId = mCurrentFrame.mnId;
+    mLastFrame = std::shared_ptr<Frame>(mCurrentFrame);
+    mnLastKeyFrameId = mCurrentFrame->mnId;
     mpLastKeyFrame = pKFini;
-    // mnLastRelocFrameId = mCurrentFrame.mnId;
+    // mnLastRelocFrameId = mCurrentFrame->mnId;
 
     mvpLocalKeyFrames.push_back(pKFini);
     mvpLocalMapPoints = mpAtlas->GetAllMapPoints();
     mpReferenceKF = pKFini;
-    mCurrentFrame.mpReferenceKF = pKFini;
+    mCurrentFrame->mpReferenceKF = pKFini;
 
     mpAtlas->SetReferenceMapPoints(mvpLocalMapPoints);
 
     mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.push_back(pKFini);
 
-    mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.GetPose());
+    mpMapDrawer->SetCurrentCameraPose(mCurrentFrame->GetPose());
 
     mState = OK;
   }
@@ -1630,19 +1624,20 @@ void Tracking::StereoInitialization() {
 void Tracking::MonocularInitialization() {
   if (!mbReadyToInitializate) {
     // Set Reference Frame
-    if (mCurrentFrame.mvKeys.size() > 100) {
-      mInitialFrame = Frame(mCurrentFrame);
-      mLastFrame = Frame(mCurrentFrame);
-      mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
-      for (size_t i = 0; i < mCurrentFrame.mvKeysUn.size(); i++)
-        mvbPrevMatched[i] = mCurrentFrame.mvKeysUn[i].pt;
+    if (mCurrentFrame->mvKeys.size() > 100) {
+      mInitialFrame = std::make_shared<Frame>(*mCurrentFrame);
+      mLastFrame = std::make_shared<Frame>(*mCurrentFrame);
+
+      mvbPrevMatched.resize(mCurrentFrame->mvKeysUn.size());
+      for (size_t i = 0; i < mCurrentFrame->mvKeysUn.size(); i++)
+        mvbPrevMatched[i] = mCurrentFrame->mvKeysUn[i].pt;
 
       fill(mvIniMatches.begin(), mvIniMatches.end(), -1);
 
       if (mSensor == SensorType::IMU_MONOCULAR) {
         mpImuPreintegratedFromLastKF =
-            std::make_shared<IMU::Preintegrated>(IMU::Bias(), *mpImuCalib);
-        mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
+            std::make_shared<IMU::Preintegrated>(IMU::Bias(), mImuCalib);
+        mCurrentFrame->mpImuPreintegrated = mpImuPreintegratedFromLastKF;
       }
 
       mbReadyToInitializate = true;
@@ -1650,9 +1645,9 @@ void Tracking::MonocularInitialization() {
       return;
     }
   } else {
-    if ((static_cast<int>(mCurrentFrame.mvKeys.size()) <= 100) ||
+    if ((static_cast<int>(mCurrentFrame->mvKeys.size()) <= 100) ||
         ((mSensor == SensorType::IMU_MONOCULAR) &&
-         (mLastFrame.mTimeStamp - mInitialFrame.mTimeStamp > 1.0))) {
+         (mLastFrame->mTimeStamp - mInitialFrame->mTimeStamp > 1.0))) {
       mbReadyToInitializate = false;
 
       return;
@@ -1672,8 +1667,8 @@ void Tracking::MonocularInitialization() {
     Sophus::SE3f Tcw;
     vector<bool> vbTriangulated;  // Triangulated Correspondences (mvIniMatches)
 
-    if (mpCamera->ReconstructWithTwoViews(mInitialFrame.mvKeysUn,
-                                          mCurrentFrame.mvKeysUn, mvIniMatches,
+    if (mpCamera->ReconstructWithTwoViews(mInitialFrame->mvKeysUn,
+                                          mCurrentFrame->mvKeysUn, mvIniMatches,
                                           Tcw, mvIniP3D, vbTriangulated)) {
       for (size_t i = 0, iend = mvIniMatches.size(); i < iend; i++) {
         if (mvIniMatches[i] >= 0 && !vbTriangulated[i]) {
@@ -1683,8 +1678,8 @@ void Tracking::MonocularInitialization() {
       }
 
       // Set Frame Poses
-      mInitialFrame.SetPose(Sophus::SE3f());
-      mCurrentFrame.SetPose(Tcw);
+      mInitialFrame->SetPose(Sophus::SE3f());
+      mCurrentFrame->SetPose(Tcw);
 
       CreateInitialMapMonocular();
     }
@@ -1693,10 +1688,10 @@ void Tracking::MonocularInitialization() {
 
 void Tracking::CreateInitialMapMonocular() {
   // Create KeyFrames
-  KeyFrame* pKFini =
-      new KeyFrame(mInitialFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
-  KeyFrame* pKFcur =
-      new KeyFrame(mCurrentFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
+  std::shared_ptr<KeyFrame> pKFini = std::make_shared<KeyFrame>(
+      mInitialFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
+  std::shared_ptr<KeyFrame> pKFcur = std::make_shared<KeyFrame>(
+      mCurrentFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
 
   if (mSensor == SensorType::IMU_MONOCULAR) pKFini->mpImuPreintegrated.reset();
 
@@ -1725,8 +1720,8 @@ void Tracking::CreateInitialMapMonocular() {
     pMP->UpdateNormalAndDepth();
 
     // Fill Current Frame structure
-    mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
-    mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
+    mCurrentFrame->mvpMapPoints[mvIniMatches[i]] = pMP;
+    mCurrentFrame->mvbOutlier[mvIniMatches[i]] = false;
 
     // Add to Map
     mpAtlas->AddMapPoint(pMP);
@@ -1788,29 +1783,29 @@ void Tracking::CreateInitialMapMonocular() {
   mpLocalMapper->InsertKeyFrame(pKFcur);
   mpLocalMapper->mFirstTs = pKFcur->mTimeStamp;
 
-  mCurrentFrame.SetPose(pKFcur->GetPose());
-  mnLastKeyFrameId = mCurrentFrame.mnId;
+  mCurrentFrame->SetPose(pKFcur->GetPose());
+  mnLastKeyFrameId = mCurrentFrame->mnId;
   mpLastKeyFrame = pKFcur;
-  // mnLastRelocFrameId = mInitialFrame.mnId;
+  // mnLastRelocFrameId = mInitialFrame->mnId;
 
   mvpLocalKeyFrames.push_back(pKFcur);
   mvpLocalKeyFrames.push_back(pKFini);
   mvpLocalMapPoints = mpAtlas->GetAllMapPoints();
   mpReferenceKF = pKFcur;
-  mCurrentFrame.mpReferenceKF = pKFcur;
+  mCurrentFrame->mpReferenceKF = pKFcur;
 
   // Compute here initial velocity
-  vector<KeyFrame*> vKFs = mpAtlas->GetAllKeyFrames();
+  auto vKFs = mpAtlas->GetAllKeyFrames();
 
   Sophus::SE3f deltaT = vKFs.back()->GetPose() * vKFs.front()->GetPoseInverse();
   mbVelocity = false;
   Eigen::Vector3f phi = deltaT.so3().log();
 
-  double aux = (mCurrentFrame.mTimeStamp - mLastFrame.mTimeStamp) /
-               (mCurrentFrame.mTimeStamp - mInitialFrame.mTimeStamp);
+  double aux = (mCurrentFrame->mTimeStamp - mLastFrame->mTimeStamp) /
+               (mCurrentFrame->mTimeStamp - mInitialFrame->mTimeStamp);
   phi *= aux;
 
-  mLastFrame = Frame(mCurrentFrame);
+  mLastFrame = std::make_shared<Frame>(*mCurrentFrame);
 
   mpAtlas->SetReferenceMapPoints(mvpLocalMapPoints);
 
@@ -1824,14 +1819,12 @@ void Tracking::CreateInitialMapMonocular() {
 }
 
 void Tracking::CreateMapInAtlas() {
-  mnLastInitFrameId = mCurrentFrame.mnId;
+  mnLastInitFrameId = mCurrentFrame->mnId;
   mpAtlas->CreateNewMap();
-  if (mSensor == SensorType::IMU_STEREO ||
-      mSensor == SensorType::IMU_MONOCULAR || mSensor == SensorType::IMU_RGBD)
-    mpAtlas->SetInertialSensor();
+  if (mSensor.isImu()) mpAtlas->SetInertialSensor();
   mbSetInit = false;
 
-  mnInitialFrameId = mCurrentFrame.mnId + 1;
+  mnInitialFrameId = mCurrentFrame->mnId + 1;
   mState = NO_IMAGES_YET;
 
   // Restart the variable with information about the last KF
@@ -1843,35 +1836,33 @@ void Tracking::CreateMapInAtlas() {
       Verbose::VERBOSITY_NORMAL);
   mbVO = false;  // Init value for know if there are enough MapPoints in the
                  // last KF
-  if (mSensor == SensorType::MONOCULAR ||
-      mSensor == SensorType::IMU_MONOCULAR) {
-    mbReadyToInitializate = false;
-  }
+
+  if (mSensor.isMonocular()) mbReadyToInitializate = false;
 
   if (mSensor.isImu() && mpImuPreintegratedFromLastKF) {
     mpImuPreintegratedFromLastKF =
-        std::make_shared<IMU::Preintegrated>(IMU::Bias(), *mpImuCalib);
+        std::make_shared<IMU::Preintegrated>(IMU::Bias(), mImuCalib);
   }
 
-  if (mpLastKeyFrame) mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
+  if (mpLastKeyFrame) mpLastKeyFrame.reset();
 
-  if (mpReferenceKF) mpReferenceKF = static_cast<KeyFrame*>(NULL);
+  if (mpReferenceKF) mpReferenceKF.reset();
 
-  mLastFrame = Frame();
-  mCurrentFrame = Frame();
+  mLastFrame = std::make_shared<Frame>();
+  mCurrentFrame = std::make_shared<Frame>();
   mvIniMatches.clear();
 
   mbCreatedMap = true;
 }
 
 void Tracking::CheckReplacedInLastFrame() {
-  for (int i = 0; i < mLastFrame.N; i++) {
-    MapPoint* pMP = mLastFrame.mvpMapPoints[i];
+  for (int i = 0; i < mLastFrame->N; i++) {
+    MapPoint* pMP = mLastFrame->mvpMapPoints[i];
 
     if (pMP) {
       MapPoint* pRep = pMP->GetReplaced();
       if (pRep) {
-        mLastFrame.mvpMapPoints[i] = pRep;
+        mLastFrame->mvpMapPoints[i] = pRep;
       }
     }
   }
@@ -1879,7 +1870,7 @@ void Tracking::CheckReplacedInLastFrame() {
 
 bool Tracking::TrackReferenceKeyFrame() {
   // Compute Bag of Words vector
-  mCurrentFrame.ComputeBoW();
+  mCurrentFrame->ComputeBoW();
 
   // We perform first an ORB matching with the reference keyframe
   // If enough matches are found we setup a PnP solver
@@ -1897,63 +1888,61 @@ bool Tracking::TrackReferenceKeyFrame() {
     return false;
   }
 
-  mCurrentFrame.mvpMapPoints = vpMapPointMatches;
-  mCurrentFrame.SetPose(mLastFrame.GetPose());
+  mCurrentFrame->mvpMapPoints = vpMapPointMatches;
+  mCurrentFrame->SetPose(mLastFrame->GetPose());
 
-  // mCurrentFrame.PrintPointDistribution();
+  // mCurrentFrame->PrintPointDistribution();
 
-  // cout << " TrackReferenceKeyFrame mLastFrame.mTcw:  " << mLastFrame.mTcw <<
-  // endl;
-  Optimizer::PoseOptimization(&mCurrentFrame);
+  // cout << " TrackReferenceKeyFrame mLastFrame->mTcw:  " << mLastFrame->mTcw
+  // << endl;
+  Optimizer::PoseOptimization(mCurrentFrame);
 
   // Discard outliers
   int nmatchesMap = 0;
-  for (int i = 0; i < mCurrentFrame.N; i++) {
-    // if(i >= mCurrentFrame.Nleft) break;
-    if (mCurrentFrame.mvpMapPoints[i]) {
-      if (mCurrentFrame.mvbOutlier[i]) {
-        MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
-
-        mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
-        mCurrentFrame.mvbOutlier[i] = false;
-        if (i < mCurrentFrame.Nleft) {
+  for (int i = 0; i < mCurrentFrame->N; i++) {
+    // if(i >= mCurrentFrame->Nleft) break;
+    if (mCurrentFrame->mvpMapPoints[i]) {
+      if (mCurrentFrame->mvbOutlier[i]) {
+        MapPoint* pMP = mCurrentFrame->mvpMapPoints[i];
+
+        mCurrentFrame->mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+        mCurrentFrame->mvbOutlier[i] = false;
+        if (i < mCurrentFrame->Nleft) {
           pMP->mbTrackInView = false;
         } else {
           pMP->mbTrackInViewR = false;
         }
         pMP->mbTrackInView = false;
-        pMP->mnLastFrameSeen = mCurrentFrame.mnId;
+        pMP->mnLastFrameSeen = mCurrentFrame->mnId;
         nmatches--;
-      } else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0) {
+      } else if (mCurrentFrame->mvpMapPoints[i]->Observations() > 0) {
         nmatchesMap++;
       }
     }
   }
 
-  if (mSensor == SensorType::IMU_MONOCULAR ||
-      mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD)
-    return true;
-  else
-    return nmatchesMap >= 10;
+  if (mSensor.isImu()) return true;
+
+  return nmatchesMap >= 10;
 }
 
 void Tracking::UpdateLastFrame() {
   // Update pose according to reference keyframe
-  KeyFrame* pRef = mLastFrame.mpReferenceKF;
+  auto pRef = mLastFrame->mpReferenceKF;
   Sophus::SE3f Tlr = mlRelativeFramePoses.back();
-  mLastFrame.SetPose(Tlr * pRef->GetPose());
+  mLastFrame->SetPose(Tlr * pRef->GetPose());
 
-  if (mnLastKeyFrameId == mLastFrame.mnId || mSensor == SensorType::MONOCULAR ||
-      mSensor == SensorType::IMU_MONOCULAR || !mbOnlyTracking)
+  if (mnLastKeyFrameId == mLastFrame->mnId || mSensor.isMonocular() ||
+      !mbOnlyTracking)
     return;
 
   // Create "visual odometry" MapPoints
   // We sort points according to their measured depth by the stereo/RGB-D sensor
   vector<pair<float, int>> vDepthIdx;
-  const int Nfeat = mLastFrame.Nleft == -1 ? mLastFrame.N : mLastFrame.Nleft;
+  const int Nfeat = mLastFrame->Nleft == -1 ? mLastFrame->N : mLastFrame->Nleft;
   vDepthIdx.reserve(Nfeat);
   for (int i = 0; i < Nfeat; i++) {
-    float z = mLastFrame.mvDepth[i];
+    float z = mLastFrame->mvDepth[i];
     if (z > 0) {
       vDepthIdx.push_back(make_pair(z, i));
     }
@@ -1971,7 +1960,7 @@ void Tracking::UpdateLastFrame() {
 
     bool bCreateNew = false;
 
-    MapPoint* pMP = mLastFrame.mvpMapPoints[i];
+    MapPoint* pMP = mLastFrame->mvpMapPoints[i];
 
     if (!pMP)
       bCreateNew = true;
@@ -1981,15 +1970,15 @@ void Tracking::UpdateLastFrame() {
     if (bCreateNew) {
       Eigen::Vector3f x3D;
 
-      if (mLastFrame.Nleft == -1) {
-        mLastFrame.UnprojectStereo(i, x3D);
+      if (mLastFrame->Nleft == -1) {
+        mLastFrame->UnprojectStereo(i, x3D);
       } else {
-        x3D = mLastFrame.UnprojectStereoFishEye(i);
+        x3D = mLastFrame->UnprojectStereoFishEye(i);
       }
 
       MapPoint* pNewMP =
-          new MapPoint(x3D, mpAtlas->GetCurrentMap(), &mLastFrame, i);
-      mLastFrame.mvpMapPoints[i] = pNewMP;
+          new MapPoint(x3D, mpAtlas->GetCurrentMap(), mLastFrame, i);
+      mLastFrame->mvpMapPoints[i] = pNewMP;
 
       mlpTemporalPoints.push_back(pNewMP);
       nPoints++;
@@ -2012,15 +2001,15 @@ bool Tracking::TrackWithMotionModel() {
   UpdateLastFrame();
 
   if (mpAtlas->isImuInitialized() &&
-      (mCurrentFrame.mnId > mnLastRelocFrameId + mnFramesToResetIMU)) {
+      (mCurrentFrame->mnId > mnLastRelocFrameId + mnFramesToResetIMU)) {
     // Predict state with IMU if it is initialized and it doesnt need reset
     PredictStateIMU();
     return true;
   } else {
-    mCurrentFrame.SetPose(mVelocity * mLastFrame.GetPose());
+    mCurrentFrame->SetPose(mVelocity * mLastFrame->GetPose());
   }
 
-  fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(),
+  fill(mCurrentFrame->mvpMapPoints.begin(), mCurrentFrame->mvpMapPoints.end(),
        static_cast<MapPoint*>(NULL));
 
   // Project points seen in previous frame
@@ -2031,9 +2020,8 @@ bool Tracking::TrackWithMotionModel() {
   // else
   th = 15;
 
-  int nmatches = matcher.SearchByProjection(
-      mCurrentFrame, mLastFrame, th,
-      mSensor == SensorType::MONOCULAR || mSensor == SensorType::IMU_MONOCULAR);
+  int nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, th,
+                                            mSensor.isMonocular());
 
   spdlog::info("Tracking::TrackWithMotionModel found {}  matches in first pass",
                nmatches);
@@ -2042,13 +2030,11 @@ bool Tracking::TrackWithMotionModel() {
   if (nmatches < 20) {
     Verbose::PrintMess("Not enough matches, wider window search!!",
                        Verbose::VERBOSITY_NORMAL);
-    fill(mCurrentFrame.mvpMapPoints.begin(), mCurrentFrame.mvpMapPoints.end(),
+    fill(mCurrentFrame->mvpMapPoints.begin(), mCurrentFrame->mvpMapPoints.end(),
          static_cast<MapPoint*>(NULL));
 
-    nmatches =
-        matcher.SearchByProjection(mCurrentFrame, mLastFrame, 2 * th,
-                                   mSensor == SensorType::MONOCULAR ||
-                                       mSensor == SensorType::IMU_MONOCULAR);
+    nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 2 * th,
+                                          mSensor.isMonocular());
     Verbose::PrintMess("Matches with wider search: " + to_string(nmatches),
                        Verbose::VERBOSITY_NORMAL);
   }
@@ -2056,33 +2042,32 @@ bool Tracking::TrackWithMotionModel() {
   if (nmatches < 20) {
     Verbose::PrintMess("Not enough matches after wider search!!",
                        Verbose::VERBOSITY_NORMAL);
-    if (mSensor == SensorType::IMU_MONOCULAR ||
-        mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD)
+    if (mSensor.isImu())
       return true;
     else
       return false;
   }
 
   // Optimize frame pose with all matches
-  Optimizer::PoseOptimization(&mCurrentFrame);
+  Optimizer::PoseOptimization(mCurrentFrame);
 
   // Discard outliers
   int nmatchesMap = 0;
-  for (int i = 0; i < mCurrentFrame.N; i++) {
-    if (mCurrentFrame.mvpMapPoints[i]) {
-      if (mCurrentFrame.mvbOutlier[i]) {
-        MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
-
-        mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
-        mCurrentFrame.mvbOutlier[i] = false;
-        if (i < mCurrentFrame.Nleft) {
+  for (int i = 0; i < mCurrentFrame->N; i++) {
+    if (mCurrentFrame->mvpMapPoints[i]) {
+      if (mCurrentFrame->mvbOutlier[i]) {
+        MapPoint* pMP = mCurrentFrame->mvpMapPoints[i];
+
+        mCurrentFrame->mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+        mCurrentFrame->mvbOutlier[i] = false;
+        if (i < mCurrentFrame->Nleft) {
           pMP->mbTrackInView = false;
         } else {
           pMP->mbTrackInViewR = false;
         }
-        pMP->mnLastFrameSeen = mCurrentFrame.mnId;
+        pMP->mnLastFrameSeen = mCurrentFrame->mnId;
         nmatches--;
-      } else if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0) {
+      } else if (mCurrentFrame->mvpMapPoints[i]->Observations() > 0) {
         nmatchesMap++;
       }
     }
@@ -2103,8 +2088,7 @@ bool Tracking::TrackWithMotionModel() {
     return nmatches > 20;
   }
 
-  if (mSensor == SensorType::IMU_MONOCULAR ||
-      mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) {
+  if (mSensor.isImu()) {
     return true;
   } else {
     const bool goodTracking = (nmatchesMap >= 10);
@@ -2135,20 +2119,20 @@ bool Tracking::TrackLocalMap() {
 
   // TOO check outliers before PO
   int aux1 = 0, aux2 = 0;
-  for (int i = 0; i < mCurrentFrame.N; i++) {
-    if (mCurrentFrame.mvpMapPoints[i]) {
+  for (int i = 0; i < mCurrentFrame->N; i++) {
+    if (mCurrentFrame->mvpMapPoints[i]) {
       aux1++;
-      if (mCurrentFrame.mvbOutlier[i]) aux2++;
+      if (mCurrentFrame->mvbOutlier[i]) aux2++;
     }
   }
 
   int inliers;
   if (!mpAtlas->isImuInitialized()) {
-    Optimizer::PoseOptimization(&mCurrentFrame);
+    Optimizer::PoseOptimization(mCurrentFrame);
   } else {
-    if (mCurrentFrame.mnId <= mnLastRelocFrameId + mnFramesToResetIMU) {
+    if (mCurrentFrame->mnId <= mnLastRelocFrameId + mnFramesToResetIMU) {
       spdlog::info("TLM: PoseOptimization");
-      Optimizer::PoseOptimization(&mCurrentFrame);
+      Optimizer::PoseOptimization(mCurrentFrame);
     } else {
       // if(!mbMapUpdated && mState == OK) //  && (mnMatchesInliers>30))
       //
@@ -2156,13 +2140,13 @@ bool Tracking::TrackLocalMap() {
       if (!mbMapUpdated) {
         spdlog::info("TLM: PoseInertialOptimizationLastFrame");
         inliers = Optimizer::PoseInertialOptimizationLastFrame(
-            &mCurrentFrame);  // ,
-                              // !mpLastKeyFrame->GetMap()->GetIniertialBA1());
+            mCurrentFrame);  // ,
+                             // !mpLastKeyFrame->GetMap()->GetIniertialBA1());
       } else {
         spdlog::info("TLM: PoseInertialOptimizationLastKeyFrame");
         inliers = Optimizer::PoseInertialOptimizationLastKeyFrame(
-            &mCurrentFrame);  // ,
-                              // !mpLastKeyFrame->GetMap()->GetIniertialBA1());
+            mCurrentFrame);  // ,
+                             // !mpLastKeyFrame->GetMap()->GetIniertialBA1());
       }
     }
   }
@@ -2171,29 +2155,29 @@ bool Tracking::TrackLocalMap() {
       std::chrono::steady_clock::now();
 
   aux1 = 0, aux2 = 0;
-  for (int i = 0; i < mCurrentFrame.N; i++) {
-    if (mCurrentFrame.mvpMapPoints[i]) {
+  for (int i = 0; i < mCurrentFrame->N; i++) {
+    if (mCurrentFrame->mvpMapPoints[i]) {
       aux1++;
-      if (mCurrentFrame.mvbOutlier[i]) aux2++;
+      if (mCurrentFrame->mvbOutlier[i]) aux2++;
     }
   }
 
   mnMatchesInliers = 0;
 
   // Update MapPoints Statistics
-  for (int i = 0; i < mCurrentFrame.N; i++) {
-    if (mCurrentFrame.mvpMapPoints[i]) {
-      if (!mCurrentFrame.mvbOutlier[i]) {
-        mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
+  for (int i = 0; i < mCurrentFrame->N; i++) {
+    if (mCurrentFrame->mvpMapPoints[i]) {
+      if (!mCurrentFrame->mvbOutlier[i]) {
+        mCurrentFrame->mvpMapPoints[i]->IncreaseFound();
         if (!mbOnlyTracking) {
-          if (mCurrentFrame.mvpMapPoints[i]->Observations() > 0) {
+          if (mCurrentFrame->mvpMapPoints[i]->Observations() > 0) {
             mnMatchesInliers++;
           }
         } else {
           mnMatchesInliers++;
         }
       } else if (mSensor == SensorType::STEREO) {
-        mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+        mCurrentFrame->mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
       }
     }
   }
@@ -2222,7 +2206,7 @@ bool Tracking::TrackLocalMap() {
   // Decide if the tracking was succesful
   // More restrictive if there was a relocalization recently
   mpLocalMapper->mnMatchesInliers = mnMatchesInliers;
-  if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames &&
+  if (mCurrentFrame->mnId < mnLastRelocFrameId + mMaxFrames &&
       mnMatchesInliers < 50)
     return false;
 
@@ -2249,15 +2233,13 @@ bool Tracking::TrackLocalMap() {
 }
 
 bool Tracking::NeedNewKeyFrame() {
-  if ((mSensor == SensorType::IMU_MONOCULAR ||
-       mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) &&
-      !mpAtlas->GetCurrentMap()->isImuInitialized()) {
+  if (mSensor.isImu() && !mpAtlas->GetCurrentMap()->isImuInitialized()) {
     if (mSensor == SensorType::IMU_MONOCULAR &&
-        (mCurrentFrame.mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.25)
+        (mCurrentFrame->mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.25)
       return true;
     else if ((mSensor == SensorType::IMU_STEREO ||
               mSensor == SensorType::IMU_RGBD) &&
-             (mCurrentFrame.mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.25)
+             (mCurrentFrame->mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.25)
       return true;
     else
       return false;
@@ -2278,7 +2260,7 @@ bool Tracking::NeedNewKeyFrame() {
 
   // Do not insert keyframes if not enough frames have passed from last
   // relocalisation
-  if (mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames &&
+  if (mCurrentFrame->mnId < mnLastRelocFrameId + mMaxFrames &&
       nKFs > mMaxFrames) {
     return false;
   }
@@ -2299,10 +2281,12 @@ bool Tracking::NeedNewKeyFrame() {
 
   if (mSensor != SensorType::MONOCULAR &&
       mSensor != SensorType::IMU_MONOCULAR) {
-    int N = (mCurrentFrame.Nleft == -1) ? mCurrentFrame.N : mCurrentFrame.Nleft;
+    int N =
+        (mCurrentFrame->Nleft == -1) ? mCurrentFrame->N : mCurrentFrame->Nleft;
     for (int i = 0; i < N; i++) {
-      if (mCurrentFrame.mvDepth[i] > 0 && mCurrentFrame.mvDepth[i] < mThDepth) {
-        if (mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
+      if (mCurrentFrame->mvDepth[i] > 0 &&
+          mCurrentFrame->mvDepth[i] < mThDepth) {
+        if (mCurrentFrame->mvpMapPoints[i] && !mCurrentFrame->mvbOutlier[i])
           nTrackedClose++;
         else
           nNonTrackedClose++;
@@ -2347,10 +2331,10 @@ bool Tracking::NeedNewKeyFrame() {
 
   // Condition 1a: More than "MaxFrames" have passed from last keyframe
   // insertion
-  const bool c1a = mCurrentFrame.mnId >= mnLastKeyFrameId + mMaxFrames;
+  const bool c1a = mCurrentFrame->mnId >= mnLastKeyFrameId + mMaxFrames;
   // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
   const bool c1b =
-      ((mCurrentFrame.mnId >= mnLastKeyFrameId + mMinFrames) &&
+      ((mCurrentFrame->mnId >= mnLastKeyFrameId + mMinFrames) &&
        bLocalMappingIdle);  // mpLocalMapper->KeyframesInQueue() < 2);
   // Condition 1c: tracking is weak
   const bool c1c =
@@ -2368,11 +2352,11 @@ bool Tracking::NeedNewKeyFrame() {
   bool c3 = false;
   if (mpLastKeyFrame) {
     if (mSensor == SensorType::IMU_MONOCULAR) {
-      if ((mCurrentFrame.mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.5)
+      if ((mCurrentFrame->mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.5)
         c3 = true;
     } else if (mSensor == SensorType::IMU_STEREO ||
                mSensor == SensorType::IMU_RGBD) {
-      if ((mCurrentFrame.mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.5)
+      if ((mCurrentFrame->mTimeStamp - mpLastKeyFrame->mTimeStamp) >= 0.5)
         c3 = true;
     }
   }
@@ -2424,15 +2408,15 @@ void Tracking::CreateNewKeyFrame() {
 
   if (!mpLocalMapper->SetNotStop(true)) return;
 
-  KeyFrame* pKF =
-      new KeyFrame(mCurrentFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
+  std::shared_ptr<KeyFrame> pKF = std::make_shared<KeyFrame>(
+      mCurrentFrame, mpAtlas->GetCurrentMap(), mpKeyFrameDB);
 
   if (mpAtlas->isImuInitialized())  //  || mpLocalMapper->IsInitializing())
     pKF->bImu = true;
 
-  pKF->SetNewBias(mCurrentFrame.mImuBias);
+  pKF->SetNewBias(mCurrentFrame->mImuBias);
   mpReferenceKF = pKF;
-  mCurrentFrame.mpReferenceKF = pKF;
+  mCurrentFrame->mpReferenceKF = pKF;
 
   if (mpLastKeyFrame) {
     pKF->mPrevKF = mpLastKeyFrame;
@@ -2442,29 +2426,27 @@ void Tracking::CreateNewKeyFrame() {
                        Verbose::VERBOSITY_NORMAL);
   }
   // Reset preintegration from last KF (Create new object)
-  if (mSensor == SensorType::IMU_MONOCULAR ||
-      mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) {
+  if (mSensor.isImu()) {
     mpImuPreintegratedFromLastKF =
         std::make_shared<IMU::Preintegrated>(pKF->GetImuBias(), pKF->mImuCalib);
   }
 
   // TODO check if incluide imu_stereo
-  if (mSensor != SensorType::MONOCULAR &&
-      mSensor != SensorType::IMU_MONOCULAR) {
-    mCurrentFrame.UpdatePoseMatrices();
+  if (!mSensor.isMonocular()) {
+    mCurrentFrame->UpdatePoseMatrices();
     // cout << "create new MPs" << endl;
     // We sort points by the measured depth by the stereo/RGBD sensor.
     // We create all those MapPoints whose depth < mThDepth.
     // If there are less than 100 close points we create the 100 closest.
     int maxPoint = 100;
-    if (mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD)
-      maxPoint = 100;
+    if (mSensor.isImu()) maxPoint = 100;
 
     vector<pair<float, int>> vDepthIdx;
-    int N = (mCurrentFrame.Nleft != -1) ? mCurrentFrame.Nleft : mCurrentFrame.N;
-    vDepthIdx.reserve(mCurrentFrame.N);
+    int N =
+        (mCurrentFrame->Nleft != -1) ? mCurrentFrame->Nleft : mCurrentFrame->N;
+    vDepthIdx.reserve(mCurrentFrame->N);
     for (int i = 0; i < N; i++) {
-      float z = mCurrentFrame.mvDepth[i];
+      float z = mCurrentFrame->mvDepth[i];
       if (z > 0) {
         vDepthIdx.push_back(make_pair(z, i));
       }
@@ -2479,21 +2461,21 @@ void Tracking::CreateNewKeyFrame() {
 
         bool bCreateNew = false;
 
-        MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
+        MapPoint* pMP = mCurrentFrame->mvpMapPoints[i];
         if (!pMP) {
           bCreateNew = true;
         } else if (pMP->Observations() < 1) {
           bCreateNew = true;
-          mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+          mCurrentFrame->mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
         }
 
         if (bCreateNew) {
           Eigen::Vector3f x3D;
 
-          if (mCurrentFrame.Nleft == -1) {
-            mCurrentFrame.UnprojectStereo(i, x3D);
+          if (mCurrentFrame->Nleft == -1) {
+            mCurrentFrame->UnprojectStereo(i, x3D);
           } else {
-            x3D = mCurrentFrame.UnprojectStereoFishEye(i);
+            x3D = mCurrentFrame->UnprojectStereoFishEye(i);
           }
 
           MapPoint* pNewMP = new MapPoint(x3D, pKF, mpAtlas->GetCurrentMap());
@@ -2501,15 +2483,16 @@ void Tracking::CreateNewKeyFrame() {
 
           // Check if it is a stereo observation in order to not
           // duplicate mappoints
-          if (mCurrentFrame.Nleft != -1 &&
-              mCurrentFrame.mvLeftToRightMatch[i] >= 0) {
-            mCurrentFrame.mvpMapPoints[mCurrentFrame.Nleft +
-                                       mCurrentFrame.mvLeftToRightMatch[i]] =
+          if (mCurrentFrame->Nleft != -1 &&
+              mCurrentFrame->mvLeftToRightMatch[i] >= 0) {
+            mCurrentFrame->mvpMapPoints[mCurrentFrame->Nleft +
+                                        mCurrentFrame->mvLeftToRightMatch[i]] =
                 pNewMP;
             pNewMP->AddObservation(
-                pKF, mCurrentFrame.Nleft + mCurrentFrame.mvLeftToRightMatch[i]);
-            pKF->AddMapPoint(pNewMP, mCurrentFrame.Nleft +
-                                         mCurrentFrame.mvLeftToRightMatch[i]);
+                pKF,
+                mCurrentFrame->Nleft + mCurrentFrame->mvLeftToRightMatch[i]);
+            pKF->AddMapPoint(pNewMP, mCurrentFrame->Nleft +
+                                         mCurrentFrame->mvLeftToRightMatch[i]);
           }
 
           pKF->AddMapPoint(pNewMP, i);
@@ -2517,7 +2500,7 @@ void Tracking::CreateNewKeyFrame() {
           pNewMP->UpdateNormalAndDepth();
           mpAtlas->AddMapPoint(pNewMP);
 
-          mCurrentFrame.mvpMapPoints[i] = pNewMP;
+          mCurrentFrame->mvpMapPoints[i] = pNewMP;
           nPoints++;
         } else {
           nPoints++;
@@ -2535,14 +2518,14 @@ void Tracking::CreateNewKeyFrame() {
 
   mpLocalMapper->SetNotStop(false);
 
-  mnLastKeyFrameId = mCurrentFrame.mnId;
+  mnLastKeyFrameId = mCurrentFrame->mnId;
   mpLastKeyFrame = pKF;
 }
 
 void Tracking::SearchLocalPoints() {
   // Do not search map points already matched
-  for (vector<MapPoint*>::iterator vit = mCurrentFrame.mvpMapPoints.begin(),
-                                   vend = mCurrentFrame.mvpMapPoints.end();
+  for (vector<MapPoint*>::iterator vit = mCurrentFrame->mvpMapPoints.begin(),
+                                   vend = mCurrentFrame->mvpMapPoints.end();
        vit != vend; vit++) {
     MapPoint* pMP = *vit;
     if (pMP) {
@@ -2550,7 +2533,7 @@ void Tracking::SearchLocalPoints() {
         *vit = static_cast<MapPoint*>(NULL);
       } else {
         pMP->IncreaseVisible();
-        pMP->mnLastFrameSeen = mCurrentFrame.mnId;
+        pMP->mnLastFrameSeen = mCurrentFrame->mnId;
         pMP->mbTrackInView = false;
         pMP->mbTrackInViewR = false;
       }
@@ -2565,15 +2548,15 @@ void Tracking::SearchLocalPoints() {
        vit != vend; vit++) {
     MapPoint* pMP = *vit;
 
-    if (pMP->mnLastFrameSeen == mCurrentFrame.mnId) continue;
+    if (pMP->mnLastFrameSeen == mCurrentFrame->mnId) continue;
     if (pMP->isBad()) continue;
     // Project (this fills MapPoint variables for matching)
-    if (mCurrentFrame.isInFrustum(pMP, 0.5)) {
+    if (mCurrentFrame->isInFrustum(pMP, 0.5)) {
       pMP->IncreaseVisible();
       nToMatch++;
     }
     if (pMP->mbTrackInView) {
-      mCurrentFrame.mmProjectPoints[pMP->mnId] =
+      mCurrentFrame->mmProjectPoints[pMP->mnId] =
           cv::Point2f(pMP->mTrackProjX, pMP->mTrackProjY);
     }
   }
@@ -2595,7 +2578,7 @@ void Tracking::SearchLocalPoints() {
     }
 
     // If the camera has been relocalised recently, perform a coarser search
-    if (mCurrentFrame.mnId < mnLastRelocFrameId + 2) th = 5;
+    if (mCurrentFrame->mnId < mnLastRelocFrameId + 2) th = 5;
 
     if (mState == LOST ||
         mState == RECENTLY_LOST)  // Lost for less than 1 second
@@ -2621,11 +2604,7 @@ void Tracking::UpdateLocalPoints() {
 
   int count_pts = 0;
 
-  for (vector<KeyFrame*>::const_reverse_iterator
-           itKF = mvpLocalKeyFrames.rbegin(),
-           itEndKF = mvpLocalKeyFrames.rend();
-       itKF != itEndKF; ++itKF) {
-    KeyFrame* pKF = *itKF;
+  for (auto pKF : mvpLocalKeyFrames) {
     const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
 
     for (vector<MapPoint*>::const_iterator itMP = vpMPs.begin(),
@@ -2633,11 +2612,11 @@ void Tracking::UpdateLocalPoints() {
          itMP != itEndMP; itMP++) {
       MapPoint* pMP = *itMP;
       if (!pMP) continue;
-      if (pMP->mnTrackReferenceForFrame == mCurrentFrame.mnId) continue;
+      if (pMP->mnTrackReferenceForFrame == mCurrentFrame->mnId) continue;
       if (!pMP->isBad()) {
         count_pts++;
         mvpLocalMapPoints.push_back(pMP);
-        pMP->mnTrackReferenceForFrame = mCurrentFrame.mnId;
+        pMP->mnTrackReferenceForFrame = mCurrentFrame->mnId;
       }
     }
   }
@@ -2645,133 +2624,119 @@ void Tracking::UpdateLocalPoints() {
 
 void Tracking::UpdateLocalKeyFrames() {
   // Each map point vote for the keyframes in which it has been observed
-  map<KeyFrame*, int> keyframeCounter;
+  map<std::shared_ptr<KeyFrame>, int> keyframeCounter;
+
   if (!mpAtlas->isImuInitialized() ||
-      (mCurrentFrame.mnId < mnLastRelocFrameId + 2)) {
-    for (int i = 0; i < mCurrentFrame.N; i++) {
-      MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
+      (mCurrentFrame->mnId < mnLastRelocFrameId + 2)) {
+    for (int i = 0; i < mCurrentFrame->N; i++) {
+      MapPoint* pMP = mCurrentFrame->mvpMapPoints[i];
       if (pMP) {
         if (!pMP->isBad()) {
-          const map<KeyFrame*, tuple<int, int>> observations =
-              pMP->GetObservations();
-          for (map<KeyFrame*, tuple<int, int>>::const_iterator
+          const auto observations = pMP->GetObservations();
+          for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::const_iterator
                    it = observations.begin(),
                    itend = observations.end();
                it != itend; it++)
             keyframeCounter[it->first]++;
         } else {
-          mCurrentFrame.mvpMapPoints[i] = NULL;
+          mCurrentFrame->mvpMapPoints[i] = NULL;
         }
       }
     }
   } else {
-    for (int i = 0; i < mLastFrame.N; i++) {
+    for (int i = 0; i < mLastFrame->N; i++) {
       // Using lastframe since current frame has not matches yet
-      if (mLastFrame.mvpMapPoints[i]) {
-        MapPoint* pMP = mLastFrame.mvpMapPoints[i];
+      if (mLastFrame->mvpMapPoints[i]) {
+        MapPoint* pMP = mLastFrame->mvpMapPoints[i];
         if (!pMP) continue;
         if (!pMP->isBad()) {
-          const map<KeyFrame*, tuple<int, int>> observations =
-              pMP->GetObservations();
-          for (map<KeyFrame*, tuple<int, int>>::const_iterator
+          const auto observations = pMP->GetObservations();
+          for (map<std::shared_ptr<KeyFrame>, tuple<int, int>>::const_iterator
                    it = observations.begin(),
                    itend = observations.end();
                it != itend; it++)
             keyframeCounter[it->first]++;
         } else {
           // MODIFICATION
-          mLastFrame.mvpMapPoints[i] = NULL;
+          mLastFrame->mvpMapPoints[i] = NULL;
         }
       }
     }
   }
 
   int max = 0;
-  KeyFrame* pKFmax = static_cast<KeyFrame*>(NULL);
+  std::shared_ptr<KeyFrame> pKFmax;
 
   mvpLocalKeyFrames.clear();
   mvpLocalKeyFrames.reserve(3 * keyframeCounter.size());
 
   // All keyframes that observe a map point are included in the local map. Also
   // check which keyframe shares most points
-  for (map<KeyFrame*, int>::const_iterator it = keyframeCounter.begin(),
-                                           itEnd = keyframeCounter.end();
-       it != itEnd; it++) {
-    KeyFrame* pKF = it->first;
+  for (auto const& it : keyframeCounter) {
+    std::shared_ptr<KeyFrame> pKF = it.first;
 
     if (pKF->isBad()) continue;
 
-    if (it->second > max) {
-      max = it->second;
+    if (it.second > max) {
+      max = it.second;
       pKFmax = pKF;
     }
 
     mvpLocalKeyFrames.push_back(pKF);
-    pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
+    pKF->mnTrackReferenceForFrame = mCurrentFrame->mnId;
   }
 
   // Include also some not-already-included keyframes that are neighbors to
   // already-included keyframes
-  for (vector<KeyFrame*>::const_iterator itKF = mvpLocalKeyFrames.begin(),
-                                         itEndKF = mvpLocalKeyFrames.end();
-       itKF != itEndKF; itKF++) {
+  for (auto const& pKF : mvpLocalKeyFrames) {
     // Limit the number of keyframes
     if (mvpLocalKeyFrames.size() > 80)  // 80
       break;
 
-    KeyFrame* pKF = *itKF;
-
-    const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
+    const auto vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
 
-    for (vector<KeyFrame*>::const_iterator itNeighKF = vNeighs.begin(),
-                                           itEndNeighKF = vNeighs.end();
-         itNeighKF != itEndNeighKF; itNeighKF++) {
-      KeyFrame* pNeighKF = *itNeighKF;
+    for (auto pNeighKF : vNeighs) {
       if (!pNeighKF->isBad()) {
-        if (pNeighKF->mnTrackReferenceForFrame != mCurrentFrame.mnId) {
+        if (pNeighKF->mnTrackReferenceForFrame != mCurrentFrame->mnId) {
           mvpLocalKeyFrames.push_back(pNeighKF);
-          pNeighKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
+          pNeighKF->mnTrackReferenceForFrame = mCurrentFrame->mnId;
           break;
         }
       }
     }
 
-    const set<KeyFrame*> spChilds = pKF->GetChilds();
-    for (set<KeyFrame*>::const_iterator sit = spChilds.begin(),
-                                        send = spChilds.end();
-         sit != send; sit++) {
-      KeyFrame* pChildKF = *sit;
+    const set<std::shared_ptr<KeyFrame>> spChilds = pKF->GetChilds();
+
+    for (auto pChildKF : spChilds) {
       if (!pChildKF->isBad()) {
-        if (pChildKF->mnTrackReferenceForFrame != mCurrentFrame.mnId) {
+        if (pChildKF->mnTrackReferenceForFrame != mCurrentFrame->mnId) {
           mvpLocalKeyFrames.push_back(pChildKF);
-          pChildKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
+          pChildKF->mnTrackReferenceForFrame = mCurrentFrame->mnId;
           break;
         }
       }
     }
 
-    KeyFrame* pParent = pKF->GetParent();
+    auto pParent = pKF->GetParent();
     if (pParent) {
-      if (pParent->mnTrackReferenceForFrame != mCurrentFrame.mnId) {
+      if (pParent->mnTrackReferenceForFrame != mCurrentFrame->mnId) {
         mvpLocalKeyFrames.push_back(pParent);
-        pParent->mnTrackReferenceForFrame = mCurrentFrame.mnId;
+        pParent->mnTrackReferenceForFrame = mCurrentFrame->mnId;
         break;
       }
     }
   }
 
   // Add 10 last temporal KFs (mainly for IMU)
-  if ((mSensor == SensorType::IMU_MONOCULAR ||
-       mSensor == SensorType::IMU_STEREO || mSensor == SensorType::IMU_RGBD) &&
-      mvpLocalKeyFrames.size() < 80) {
-    KeyFrame* tempKeyFrame = mCurrentFrame.mpLastKeyFrame;
+  if (mSensor.isImu() && mvpLocalKeyFrames.size() < 80) {
+    std::shared_ptr<KeyFrame> tempKeyFrame = mCurrentFrame->mpLastKeyFrame;
 
     const int Nd = 20;
     for (int i = 0; i < Nd; i++) {
       if (!tempKeyFrame) break;
-      if (tempKeyFrame->mnTrackReferenceForFrame != mCurrentFrame.mnId) {
+      if (tempKeyFrame->mnTrackReferenceForFrame != mCurrentFrame->mnId) {
         mvpLocalKeyFrames.push_back(tempKeyFrame);
-        tempKeyFrame->mnTrackReferenceForFrame = mCurrentFrame.mnId;
+        tempKeyFrame->mnTrackReferenceForFrame = mCurrentFrame->mnId;
         tempKeyFrame = tempKeyFrame->mPrevKF;
       }
     }
@@ -2779,7 +2744,7 @@ void Tracking::UpdateLocalKeyFrames() {
 
   if (pKFmax) {
     mpReferenceKF = pKFmax;
-    mCurrentFrame.mpReferenceKF = mpReferenceKF;
+    mCurrentFrame->mpReferenceKF = mpReferenceKF;
   }
 }
 
@@ -2787,14 +2752,13 @@ bool Tracking::Relocalization() {
   Verbose::PrintMess("[Tracking::Relocalization] Starting relocalization",
                      Verbose::VERBOSITY_NORMAL);
   // Compute Bag of Words Vector
-  mCurrentFrame.ComputeBoW();
+  mCurrentFrame->ComputeBoW();
 
   // Relocalization is performed when tracking is lost
   // Track Lost: Query KeyFrame Database for keyframe candidates for
   // relocalisation
-  vector<KeyFrame*> vpCandidateKFs =
-      mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame,
-                                                   mpAtlas->GetCurrentMap());
+  auto vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(
+      mCurrentFrame, mpAtlas->GetCurrentMap());
 
   if (vpCandidateKFs.empty()) {
     spdlog::warn(
@@ -2823,7 +2787,7 @@ bool Tracking::Relocalization() {
   int nCandidates = 0;
 
   for (int i = 0; i < nKFs; i++) {
-    KeyFrame* pKF = vpCandidateKFs[i];
+    std::shared_ptr<KeyFrame> pKF = vpCandidateKFs[i];
     if (pKF->isBad()) {
       vbDiscarded[i] = true;
     } else {
@@ -2876,8 +2840,8 @@ bool Tracking::Relocalization() {
       // If a Camera Pose is computed, optimize
       if (bTcw) {
         Sophus::SE3f Tcw(eigTcw);
-        mCurrentFrame.SetPose(Tcw);
-        // Tcw.copyTo(mCurrentFrame.mTcw);
+        mCurrentFrame->SetPose(Tcw);
+        // Tcw.copyTo(mCurrentFrame->mTcw);
 
         set<MapPoint*> sFound;
 
@@ -2885,20 +2849,20 @@ bool Tracking::Relocalization() {
 
         for (int j = 0; j < np; j++) {
           if (vbInliers[j]) {
-            mCurrentFrame.mvpMapPoints[j] = vvpMapPointMatches[i][j];
+            mCurrentFrame->mvpMapPoints[j] = vvpMapPointMatches[i][j];
             sFound.insert(vvpMapPointMatches[i][j]);
           } else {
-            mCurrentFrame.mvpMapPoints[j] = NULL;
+            mCurrentFrame->mvpMapPoints[j] = NULL;
           }
         }
 
-        int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
+        int nGood = Optimizer::PoseOptimization(mCurrentFrame);
 
         if (nGood < 10) continue;
 
-        for (int io = 0; io < mCurrentFrame.N; io++)
-          if (mCurrentFrame.mvbOutlier[io])
-            mCurrentFrame.mvpMapPoints[io] = static_cast<MapPoint*>(NULL);
+        for (int io = 0; io < mCurrentFrame->N; io++)
+          if (mCurrentFrame->mvbOutlier[io])
+            mCurrentFrame->mvpMapPoints[io] = static_cast<MapPoint*>(NULL);
 
         // If few inliers, search by projection in a coarse window and optimize
         // again
@@ -2907,26 +2871,29 @@ bool Tracking::Relocalization() {
               mCurrentFrame, vpCandidateKFs[i], sFound, 10, 100);
 
           if (nadditional + nGood >= 50) {
-            nGood = Optimizer::PoseOptimization(&mCurrentFrame);
+            nGood = Optimizer::PoseOptimization(mCurrentFrame);
 
             // If many inliers but still not enough, search by projection again
             // in a narrower window the camera has been already optimized with
             // many points
             if (nGood > 30 && nGood < 50) {
               sFound.clear();
-              for (int ip = 0; ip < mCurrentFrame.N; ip++)
-                if (mCurrentFrame.mvpMapPoints[ip])
-                  sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
+              for (int ip = 0; ip < mCurrentFrame->N; ip++) {
+                if (mCurrentFrame->mvpMapPoints[ip])
+                  sFound.insert(mCurrentFrame->mvpMapPoints[ip]);
+              }
+
               nadditional = matcher2.SearchByProjection(
                   mCurrentFrame, vpCandidateKFs[i], sFound, 3, 64);
 
               // Final optimization
               if (nGood + nadditional >= 50) {
-                nGood = Optimizer::PoseOptimization(&mCurrentFrame);
+                nGood = Optimizer::PoseOptimization(mCurrentFrame);
 
-                for (int io = 0; io < mCurrentFrame.N; io++)
-                  if (mCurrentFrame.mvbOutlier[io])
-                    mCurrentFrame.mvpMapPoints[io] = NULL;
+                for (int io = 0; io < mCurrentFrame->N; io++) {
+                  if (mCurrentFrame->mvbOutlier[io])
+                    mCurrentFrame->mvpMapPoints[io] = NULL;
+                }
               }
             }
           }
@@ -2944,7 +2911,7 @@ bool Tracking::Relocalization() {
   if (!bMatch) {
     return false;
   } else {
-    mnLastRelocFrameId = mCurrentFrame.mnId;
+    mnLastRelocFrameId = mCurrentFrame->mnId;
     spdlog::warn("[Tracking::Relocalization] ... Successful relocalization!!");
     return true;
   }
@@ -2995,11 +2962,11 @@ void Tracking::Reset(bool bLocMap) {
   mlpReferences.clear();
   mlFrameTimes.clear();
   mlbLost.clear();
-  mCurrentFrame = Frame();
+  mCurrentFrame = std::make_shared<Frame>();
   mnLastRelocFrameId = 0;
-  mLastFrame = Frame();
-  mpReferenceKF = static_cast<KeyFrame*>(NULL);
-  mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
+  mLastFrame = std::make_shared<Frame>();
+  mpReferenceKF.reset();
+  mpLastKeyFrame.reset();
   mvIniMatches.clear();
 
   if (mpViewer) mpViewer->Release();
@@ -3073,13 +3040,13 @@ void Tracking::ResetActiveMap(bool bLocMap) {
 
   mlbLost = lbLost;
 
-  mnInitialFrameId = mCurrentFrame.mnId;
-  mnLastRelocFrameId = mCurrentFrame.mnId;
+  mnInitialFrameId = mCurrentFrame->mnId;
+  mnLastRelocFrameId = mCurrentFrame->mnId;
 
-  mCurrentFrame = Frame();
-  mLastFrame = Frame();
-  mpReferenceKF = static_cast<KeyFrame*>(NULL);
-  mpLastKeyFrame = static_cast<KeyFrame*>(NULL);
+  mCurrentFrame = std::make_shared<Frame>();
+  mLastFrame = std::make_shared<Frame>();
+  mpReferenceKF.reset();
+  mpLastKeyFrame.reset();
   mvIniMatches.clear();
 
   mbVelocity = false;
@@ -3130,18 +3097,20 @@ void Tracking::ChangeCalibration(const string& strSettingPath) {
 
 void Tracking::InformOnlyTracking(const bool& flag) { mbOnlyTracking = flag; }
 
-void Tracking::UpdateFrameIMU(const float s, const IMU::Bias& b,
-                              KeyFrame* pCurrentKeyFrame) {
+void Tracking::UpdateFrameIMU(
+    const float s, const IMU::Bias& b,
+    const std::shared_ptr<KeyFrame>& pCurrentKeyFrame) {
   std::shared_ptr<Map> pMap = pCurrentKeyFrame->GetMap();
   unsigned int index = mnFirstFrameId;
-  list<ORB_SLAM3::KeyFrame*>::iterator lRit = mlpReferences.begin();
+  list<std::shared_ptr<KeyFrame>>::iterator lRit = mlpReferences.begin();
   list<bool>::iterator lbL = mlbLost.begin();
+
   for (auto lit = mlRelativeFramePoses.begin(),
             lend = mlRelativeFramePoses.end();
        lit != lend; lit++, lRit++, lbL++) {
     if (*lbL) continue;
 
-    KeyFrame* pKF = *lRit;
+    std::shared_ptr<KeyFrame> pKF = *lRit;
 
     while (pKF->isBad()) {
       pKF = pKF->GetParent();
@@ -3156,51 +3125,55 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias& b,
 
   mpLastKeyFrame = pCurrentKeyFrame;
 
-  mLastFrame.SetNewBias(mLastBias);
-  mCurrentFrame.SetNewBias(mLastBias);
+  mLastFrame->SetNewBias(mLastBias);
+  mCurrentFrame->SetNewBias(mLastBias);
 
-  while (!mCurrentFrame.imuIsPreintegrated()) {
+  while (!mCurrentFrame->imuIsPreintegrated()) {
     usleep(500);
   }
 
-  if (mLastFrame.mnId == mLastFrame.mpLastKeyFrame->mnFrameId) {
-    mLastFrame.SetImuPoseVelocity(mLastFrame.mpLastKeyFrame->GetImuRotation(),
-                                  mLastFrame.mpLastKeyFrame->GetImuPosition(),
-                                  mLastFrame.mpLastKeyFrame->GetVelocity());
+  if (mLastFrame->mnId == mLastFrame->mpLastKeyFrame->mnFrameId) {
+    mLastFrame->SetImuPoseVelocity(mLastFrame->mpLastKeyFrame->GetImuRotation(),
+                                   mLastFrame->mpLastKeyFrame->GetImuPosition(),
+                                   mLastFrame->mpLastKeyFrame->GetVelocity());
   } else {
     const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE);
-    const Eigen::Vector3f twb1 = mLastFrame.mpLastKeyFrame->GetImuPosition();
-    const Eigen::Matrix3f Rwb1 = mLastFrame.mpLastKeyFrame->GetImuRotation();
-    const Eigen::Vector3f Vwb1 = mLastFrame.mpLastKeyFrame->GetVelocity();
-    float t12 = mLastFrame.mpImuPreintegrated->dT;
+    const Eigen::Vector3f twb1 = mLastFrame->mpLastKeyFrame->GetImuPosition();
+    const Eigen::Matrix3f Rwb1 = mLastFrame->mpLastKeyFrame->GetImuRotation();
+    const Eigen::Vector3f Vwb1 = mLastFrame->mpLastKeyFrame->GetVelocity();
+    float t12 = mLastFrame->mpImuPreintegrated->dT;
 
-    mLastFrame.SetImuPoseVelocity(
+    mLastFrame->SetImuPoseVelocity(
         IMU::NormalizeRotation(
-            Rwb1 * mLastFrame.mpImuPreintegrated->GetUpdatedDeltaRotation()),
+            Rwb1 * mLastFrame->mpImuPreintegrated->GetUpdatedDeltaRotation()),
         twb1 + Vwb1 * t12 + 0.5f * t12 * t12 * Gz +
-            Rwb1 * mLastFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(),
+            Rwb1 * mLastFrame->mpImuPreintegrated->GetUpdatedDeltaPosition(),
         Vwb1 + Gz * t12 +
-            Rwb1 * mLastFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity());
+            Rwb1 * mLastFrame->mpImuPreintegrated->GetUpdatedDeltaVelocity());
   }
 
-  if (mCurrentFrame.mpImuPreintegrated) {
+  if (mCurrentFrame->mpImuPreintegrated) {
     const Eigen::Vector3f Gz(0, 0, -IMU::GRAVITY_VALUE);
 
-    const Eigen::Vector3f twb1 = mCurrentFrame.mpLastKeyFrame->GetImuPosition();
-    const Eigen::Matrix3f Rwb1 = mCurrentFrame.mpLastKeyFrame->GetImuRotation();
-    const Eigen::Vector3f Vwb1 = mCurrentFrame.mpLastKeyFrame->GetVelocity();
-    float t12 = mCurrentFrame.mpImuPreintegrated->dT;
+    const Eigen::Vector3f twb1 =
+        mCurrentFrame->mpLastKeyFrame->GetImuPosition();
+    const Eigen::Matrix3f Rwb1 =
+        mCurrentFrame->mpLastKeyFrame->GetImuRotation();
+    const Eigen::Vector3f Vwb1 = mCurrentFrame->mpLastKeyFrame->GetVelocity();
+    float t12 = mCurrentFrame->mpImuPreintegrated->dT;
 
-    mCurrentFrame.SetImuPoseVelocity(
+    mCurrentFrame->SetImuPoseVelocity(
         IMU::NormalizeRotation(
-            Rwb1 * mCurrentFrame.mpImuPreintegrated->GetUpdatedDeltaRotation()),
+            Rwb1 *
+            mCurrentFrame->mpImuPreintegrated->GetUpdatedDeltaRotation()),
         twb1 + Vwb1 * t12 + 0.5f * t12 * t12 * Gz +
-            Rwb1 * mCurrentFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(),
+            Rwb1 * mCurrentFrame->mpImuPreintegrated->GetUpdatedDeltaPosition(),
         Vwb1 + Gz * t12 +
-            Rwb1 * mCurrentFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity());
+            Rwb1 *
+                mCurrentFrame->mpImuPreintegrated->GetUpdatedDeltaVelocity());
   }
 
-  mnFirstImuFrameId = mCurrentFrame.mnId;
+  mnFirstImuFrameId = mCurrentFrame->mnId;
 }
 
 void Tracking::NewDataset() { mnNumDataset++; }

From 2bee9d22fc29aa4cb63a9b614a2421b7dbee0c3c Mon Sep 17 00:00:00 2001
From: Aaron Marburg <amarburg@uw.edu>
Date: Sat, 3 Jan 2026 22:01:30 +0000
Subject: [PATCH 5/9] Further code cleanup

---
 include/KeyFrame.h  |   4 +-
 include/Map.h       |   8 +-
 src/FrameDrawer.cc  |   2 +
 src/KeyFrame.cc     |   4 +
 src/LocalMapping.cc |  16 +--
 src/LoopClosing.cc  |  20 ++--
 src/MLPnPsolver.cpp |   2 +
 src/Map.cc          |  17 ++-
 src/Sim3Solver.cc   |   2 +-
 src/System.cc       |  24 ++---
 src/Tracking.cc     | 258 ++++++++++++++++++--------------------------
 11 files changed, 159 insertions(+), 198 deletions(-)

diff --git a/include/KeyFrame.h b/include/KeyFrame.h
index ea82e778f1a..fff23f193f6 100644
--- a/include/KeyFrame.h
+++ b/include/KeyFrame.h
@@ -196,7 +196,9 @@ class KeyFrame : public std::enable_shared_from_this<KeyFrame> {
 
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  // KeyFrame();
+
+  KeyFrame() = delete;
+
   KeyFrame(const std::shared_ptr<Frame>& F, const std::shared_ptr<Map>& pMap,
            const std::shared_ptr<KeyFrameDatabase>& pKFDB);
 
diff --git a/include/Map.h b/include/Map.h
index 39fb8c6ea98..c4d20656f2c 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -124,10 +124,10 @@ class Map : public std::enable_shared_from_this<Map> {
 
   void SetInertialSensor();
   bool IsInertial();
-  void SetIniertialBA1();
-  void SetIniertialBA2();
-  bool GetIniertialBA1();
-  bool GetIniertialBA2();
+  void SetInertialBA1();
+  void SetInertialBA2();
+  bool GetInertialBA1();
+  bool GetInertialBA2();
 
   void PrintEssentialGraph();
   bool CheckEssentialGraph();
diff --git a/src/FrameDrawer.cc b/src/FrameDrawer.cc
index fd49e30022b..26376ca0014 100644
--- a/src/FrameDrawer.cc
+++ b/src/FrameDrawer.cc
@@ -361,6 +361,8 @@ void FrameDrawer::Update(const std::shared_ptr<Tracking> &pTracker) {
   mvpOutlierMPs.reserve(N);
 
   if (pTracker->mLastProcessedState == Tracking::NOT_INITIALIZED) {
+    if (!pTracker->mInitialFrame) return;
+
     mvIniKeys = pTracker->mInitialFrame->mvKeys;
     mvIniMatches = pTracker->mvIniMatches;
 
diff --git a/src/KeyFrame.cc b/src/KeyFrame.cc
index 4e685d22c5d..123ef48f8d9 100644
--- a/src/KeyFrame.cc
+++ b/src/KeyFrame.cc
@@ -34,6 +34,7 @@
 
 #include "Converter.h"
 #include "ImuTypes.h"
+#include "Logging.h"
 
 namespace ORB_SLAM3 {
 
@@ -185,6 +186,8 @@ KeyFrame::KeyFrame(const std::shared_ptr<Frame> &F,
       mbHasVelocity(false) {
   mnId = nNextId++;
 
+  spdlog::trace("New kF; map {}", mpMap ? "(exists)" : "NULL");
+
   mGrid.resize(mnGridCols);
   if (F->Nleft != -1) mGridRight.resize(mnGridCols);
   for (int i = 0; i < mnGridCols; i++) {
@@ -881,6 +884,7 @@ std::shared_ptr<Map> KeyFrame::GetMap() {
 
 void KeyFrame::UpdateMap(const std::shared_ptr<Map> &pMap) {
   unique_lock<mutex> lock(mMutexMap);
+  spdlog::trace("Updating mpMap to {}", pMap ? "(exists)" : "NULL");
   mpMap = pMap;
 }
 
diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc
index e8457474974..16f29fb02c9 100644
--- a/src/LocalMapping.cc
+++ b/src/LocalMapping.cc
@@ -175,7 +175,7 @@ void LocalMapping::Run() {
             if (dist > 0.05)
               mTinit += mpCurrentKeyFrame->mTimeStamp -
                         mpCurrentKeyFrame->mPrevKF->mTimeStamp;
-            if (!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()) {
+            if (!mpCurrentKeyFrame->GetMap()->GetInertialBA2()) {
               if ((mTinit < 10.f) && (dist < 0.02)) {
                 cout << "Not enough motion for initializing. Reseting..."
                      << endl;
@@ -192,7 +192,7 @@ void LocalMapping::Run() {
             Optimizer::LocalInertialBA(
                 mpCurrentKeyFrame, &mbAbortBA, mpCurrentKeyFrame->GetMap(),
                 num_FixedKF_BA, num_OptKF_BA, num_MPs_BA, num_edges_BA, bLarge,
-                !mpCurrentKeyFrame->GetMap()->GetIniertialBA2());
+                !mpCurrentKeyFrame->GetMap()->GetInertialBA2());
             b_doneLBA = true;
           } else {
             Optimizer::LocalBundleAdjustment(
@@ -250,10 +250,10 @@ void LocalMapping::Run() {
           // Enter here everytime local-mapping is called
           if (mpCurrentKeyFrame->GetMap()->isImuInitialized() &&
               mpTracker->mState == Tracking::OK) {
-            if (!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()) {
+            if (!mpCurrentKeyFrame->GetMap()->GetInertialBA1()) {
               if (mTinit > 5.0f) {
                 cout << "start VIBA 1" << endl;
-                mpCurrentKeyFrame->GetMap()->SetIniertialBA1();
+                mpCurrentKeyFrame->GetMap()->SetInertialBA1();
                 if (mbMonocular)
                   InitializeIMU(1.f, 1e5, true);
                 else
@@ -261,10 +261,10 @@ void LocalMapping::Run() {
 
                 cout << "end VIBA 1" << endl;
               }
-            } else if (!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()) {
+            } else if (!mpCurrentKeyFrame->GetMap()->GetInertialBA2()) {
               if (mTinit > 15.0f) {
                 cout << "start VIBA 2" << endl;
-                mpCurrentKeyFrame->GetMap()->SetIniertialBA2();
+                mpCurrentKeyFrame->GetMap()->SetInertialBA2();
                 if (mbMonocular)
                   InitializeIMU(0.f, 0.f, true);
                 else
@@ -482,7 +482,7 @@ void LocalMapping::CreateNewMapPoints() {
     // Search matches that fullfil epipolar constraint
     vector<pair<size_t, size_t>> vMatchedIndices;
     bool bCoarse = mbInertial && mpTracker->mState == Tracking::RECENTLY_LOST &&
-                   mpCurrentKeyFrame->GetMap()->GetIniertialBA2();
+                   mpCurrentKeyFrame->GetMap()->GetInertialBA2();
 
     matcher.SearchForTriangulation(mpCurrentKeyFrame, pKF2, vMatchedIndices,
                                    false, bCoarse);
@@ -997,7 +997,7 @@ void LocalMapping::KeyFrameCulling() {
             pKF->mNextKF = NULL;
             pKF->mPrevKF = NULL;
             pKF->SetBadFlag();
-          } else if (!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() &&
+          } else if (!mpCurrentKeyFrame->GetMap()->GetInertialBA2() &&
                      ((pKF->GetImuPosition() - pKF->mPrevKF->GetImuPosition())
                           .norm() < 0.02) &&
                      (t < 3)) {
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
index e6870ba8762..765d5d946f4 100644
--- a/src/LoopClosing.cc
+++ b/src/LoopClosing.cc
@@ -177,7 +177,7 @@ void LoopClosing::Run() {
               }
               // If inertial, force only yaw
               if ((mpTracker->mSensor.isImu()) &&
-                  mpCurrentKF->GetMap()->GetIniertialBA1()) {
+                  mpCurrentKF->GetMap()->GetInertialBA1()) {
                 Eigen::Vector3d phi =
                     LogSO3(mSold_new.rotation().toRotationMatrix());
                 phi(0) = 0;
@@ -269,7 +269,7 @@ void LoopClosing::Run() {
               if (mpCurrentKF->GetMap()->IsInertial()) {
                 // If inertial, force only yaw
                 if ((mpTracker->mSensor.isImu()) &&
-                    mpCurrentKF->GetMap()->GetIniertialBA2()) {
+                    mpCurrentKF->GetMap()->GetInertialBA2()) {
                   phi(0) = 0;
                   phi(1) = 0;
                   g2oSww_new =
@@ -368,7 +368,7 @@ bool LoopClosing::NewDetectCommonRegions() {
     mpLastMap = mpCurrentKF->GetMap();
   }
 
-  if (mpLastMap->IsInertial() && !mpLastMap->GetIniertialBA2()) {
+  if (mpLastMap->IsInertial() && !mpLastMap->GetInertialBA2()) {
     mpKeyFrameDB->add(mpCurrentKF);
     mpCurrentKF->SetErase();
     return false;
@@ -601,7 +601,7 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(
     bool bFixedScale =
         mbFixScale;  // TODO CHECK; Solo para el monocular inertial
     if (mpTracker->mSensor == SensorType::IMU_MONOCULAR &&
-        !pCurrentKF->GetMap()->GetIniertialBA2())
+        !pCurrentKF->GetMap()->GetInertialBA2())
       bFixedScale = false;
     int numOptMatches =
         Optimizer::OptimizeSim3(mpCurrentKF, pMatchedKF, vpMatchedMPs, gScm, 10,
@@ -744,7 +744,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
       // Geometric validation
       bool bFixedScale = mbFixScale;
       if (mpTracker->mSensor == SensorType::IMU_MONOCULAR &&
-          !mpCurrentKF->GetMap()->GetIniertialBA2())
+          !mpCurrentKF->GetMap()->GetInertialBA2())
         bFixedScale = false;
 
       Sim3Solver solver =
@@ -826,7 +826,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
 
           bool bFixedScale = mbFixScale;
           if (mpTracker->mSensor == SensorType::IMU_MONOCULAR &&
-              !mpCurrentKF->GetMap()->GetIniertialBA2())
+              !mpCurrentKF->GetMap()->GetInertialBA2())
             bFixedScale = false;
 
           int numOptMatches =
@@ -1223,7 +1223,7 @@ void LoopClosing::CorrectLoop() {
   bool bFixedScale = mbFixScale;
   // TODO CHECK; Solo para el monocular inertial
   if (mpTracker->mSensor == SensorType::IMU_MONOCULAR &&
-      !mpCurrentKF->GetMap()->GetIniertialBA2())
+      !mpCurrentKF->GetMap()->GetInertialBA2())
     bFixedScale = false;
 
 #ifdef REGISTER_TIMES
@@ -1936,7 +1936,7 @@ void LoopClosing::MergeLocal2() {
 
   const int numKFnew = pCurrentMap->KeyFramesInMap();
 
-  if ((mpTracker->mSensor.isImu()) && !pCurrentMap->GetIniertialBA2()) {
+  if ((mpTracker->mSensor.isImu()) && !pCurrentMap->GetInertialBA2()) {
     // Map is not completly initialized
     Eigen::Vector3d bg, ba;
     bg << 0., 0., 0.;
@@ -1947,8 +1947,8 @@ void LoopClosing::MergeLocal2() {
     mpTracker->UpdateFrameIMU(1.0f, b, mpTracker->GetLastKeyFrame());
 
     // Set map initialized
-    pCurrentMap->SetIniertialBA2();
-    pCurrentMap->SetIniertialBA1();
+    pCurrentMap->SetInertialBA2();
+    pCurrentMap->SetInertialBA1();
     pCurrentMap->SetImuInitialized();
   }
 
diff --git a/src/MLPnPsolver.cpp b/src/MLPnPsolver.cpp
index 14c9c305c1b..0590cb281b0 100644
--- a/src/MLPnPsolver.cpp
+++ b/src/MLPnPsolver.cpp
@@ -107,6 +107,8 @@ MLPnPsolver::MLPnPsolver(const std::shared_ptr<Frame> &F,
   SetRansacParameters();
 }
 
+MLPnPsolver::~MLPnPsolver() {}
+
 // RANSAC methods
 bool MLPnPsolver::iterate(int nIterations, bool &bNoMore,
                           vector<bool> &vbInliers, int &nInliers,
diff --git a/src/Map.cc b/src/Map.cc
index e62835b9c98..6fdcc569364 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -211,16 +211,13 @@ void Map::clear() {
   //    send=mspMapPoints.end(); sit!=send; sit++)
   //        delete *sit;
 
-  for (set<std::shared_ptr<KeyFrame>>::iterator sit = mspKeyFrames.begin(),
-                                                send = mspKeyFrames.end();
-       sit != send; sit++) {
-    std::shared_ptr<KeyFrame> pKF = *sit;
+  for (auto pKF : mspKeyFrames) {
     pKF->UpdateMap(nullptr);
-    //        delete *sit;
   }
+  mspKeyFrames.clear();
 
   mspMapPoints.clear();
-  mspKeyFrames.clear();
+
   mnMaxKFid = mnInitKFid;
   mbImuInitialized = false;
   mvpReferenceMapPoints.clear();
@@ -277,22 +274,22 @@ bool Map::IsInertial() {
   return mbIsInertial;
 }
 
-void Map::SetIniertialBA1() {
+void Map::SetInertialBA1() {
   unique_lock<mutex> lock(mMutexMap);
   mbIMU_BA1 = true;
 }
 
-void Map::SetIniertialBA2() {
+void Map::SetInertialBA2() {
   unique_lock<mutex> lock(mMutexMap);
   mbIMU_BA2 = true;
 }
 
-bool Map::GetIniertialBA1() {
+bool Map::GetInertialBA1() {
   unique_lock<mutex> lock(mMutexMap);
   return mbIMU_BA1;
 }
 
-bool Map::GetIniertialBA2() {
+bool Map::GetInertialBA2() {
   unique_lock<mutex> lock(mMutexMap);
   return mbIMU_BA2;
 }
diff --git a/src/Sim3Solver.cc b/src/Sim3Solver.cc
index d54997f7d64..eaab87ba5df 100644
--- a/src/Sim3Solver.cc
+++ b/src/Sim3Solver.cc
@@ -127,7 +127,7 @@ void Sim3Solver::SetRansacParameters(double probability, int minInliers,
   mRansacMinInliers = minInliers;
   mRansacMaxIts = maxIterations;
 
-  N = mvpMapPoints1.size();  // number of correspondences
+  N = std::max<size_t>(1, mvpMapPoints1.size());  // number of correspondences
 
   mvbInliersi.resize(N);
 
diff --git a/src/System.cc b/src/System.cc
index ff342ba6ffe..3b14b8331bb 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -143,7 +143,6 @@ void System::printBanner() {
 
 bool System::initialize(bool initFr, const string &strSequence) {
   const string mStrLoadAtlasFromFile = settings_->atlasLoadFile();
-  const string mStrSaveAtlasToFile = settings_->atlasSaveFile();
 
   cout << (*settings_) << endl;
 
@@ -167,19 +166,18 @@ bool System::initialize(bool initFr, const string &strSequence) {
 
   if (mStrLoadAtlasFromFile.empty()) {
     // Create the Atlas
-    spdlog::info("Initialization of Atlas from scratch ");
+    spdlog::info("Initializing Atlas from scratch ");
     mpAtlas = std::make_shared<Atlas>(0);
   } else {
     // Load the file with an earlier session
     // clock_t start = clock();
-    spdlog::info("Initialization of Atlas from file: {}",
-                 mStrLoadAtlasFromFile);
+    spdlog::info("Initializing Atlas from file: {}", mStrLoadAtlasFromFile);
     bool isRead = LoadAtlas(FileType::BINARY_FILE);
 
     if (!isRead) {
-      cout << "Error to load the file, please try with other session file or "
-              "vocabulary file"
-           << endl;
+      spdlog::error(
+          "Unable to load Atlas file, please try with other session file or "
+          "vocabulary file");
       return false;
     }
 
@@ -207,8 +205,6 @@ bool System::initialize(bool initFr, const string &strSequence) {
   mpLocalMapper = std::make_shared<LocalMapping>(
       shared_from_this(), mpAtlas, sensorType().isMonocular(),
       sensorType().isImu(), strSequence);
-  mptLocalMapping =
-      std::make_unique<thread>(&ORB_SLAM3::LocalMapping::Run, mpLocalMapper);
   mpLocalMapper->mInitFr = initFr;
   mpLocalMapper->mThFarPoints = settings_->thFarPoints();
 
@@ -223,12 +219,14 @@ bool System::initialize(bool initFr, const string &strSequence) {
     mpLocalMapper->mbFarPoints = false;
   }
 
+  mptLocalMapping =
+      std::make_unique<thread>(&ORB_SLAM3::LocalMapping::Run, mpLocalMapper);
+
   // Initialize the Loop Closing thread and launch
   //  sensorType()!=MONOCULAR && sensorType()!=IMU_MONOCULAR
-  mpLoopCloser =
-      std::make_shared<LoopClosing>(mpAtlas, mpKeyFrameDatabase, mpVocabulary,
-                                    sensorType() != SensorType::MONOCULAR,
-                                    activeLC);  // sensorType()!=MONOCULAR);
+  mpLoopCloser = std::make_shared<LoopClosing>(
+      mpAtlas, mpKeyFrameDatabase, mpVocabulary,
+      sensorType() != SensorType::MONOCULAR, activeLC);
   mptLoopClosing =
       std::make_unique<thread>(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);
 
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 15ef7c0cb3a..52152f36d8c 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -67,7 +67,7 @@ Tracking::Tracking(const std::shared_ptr<System>& pSys,
       mpKeyFrameDB(pKFDB),
       mbReadyToInitializate(false),
       mpSystem(pSys),
-      mpViewer(NULL),
+      mpViewer(nullptr),
       bStepByStep(false),
       mpFrameDrawer(pFrameDrawer),
       mpMapDrawer(pMapDrawer),
@@ -78,25 +78,25 @@ Tracking::Tracking(const std::shared_ptr<System>& pSys,
       mbCreatedMap(false),
       mnFirstFrameId(0),
       mpCamera2(nullptr),
-      mpLastKeyFrame() {
+      mpLastKeyFrame(),
+      initID(0),
+      lastID(0),
+      mbInitWith3KFs(false),
+      mnNumDataset(0) {
   newParameterLoader(settings);
 
-  initID = 0;
-  lastID = 0;
-  mbInitWith3KFs = false;
-  mnNumDataset = 0;
-
-  vector<std::shared_ptr<GeometricCamera>> vpCams = mpAtlas->GetAllCameras();
-  std::cout << "There are " << vpCams.size() << " cameras in the atlas"
-            << std::endl;
-  for (auto pCam : vpCams) {
-    std::cout << "Camera " << pCam->GetId();
-    if (pCam->GetType() == GeometricCamera::CAM_PINHOLE) {
-      std::cout << " is pinhole" << std::endl;
-    } else if (pCam->GetType() == GeometricCamera::CAM_FISHEYE) {
-      std::cout << " is fisheye" << std::endl;
-    } else {
-      std::cout << " is unknown" << std::endl;
+  {
+    vector<std::shared_ptr<GeometricCamera>> vpCams = mpAtlas->GetAllCameras();
+    spdlog::debug("There are {} cameras in the atlas", vpCams.size());
+
+    for (auto const& pCam : vpCams) {
+      if (pCam->GetType() == GeometricCamera::CAM_PINHOLE) {
+        spdlog::debug("Camera {} is pinhole", pCam->GetId());
+      } else if (pCam->GetType() == GeometricCamera::CAM_FISHEYE) {
+        spdlog::debug("Camera {} is fisheye", pCam->GetId());
+      } else {
+        spdlog::debug("Camera {} is unknown", pCam->GetId());
+      }
     }
   }
 
@@ -582,23 +582,25 @@ void Tracking::newParameterLoader(const std::shared_ptr<Settings>& settings) {
   mMaxFrames = settings->fps();
   mbRGB = settings->rgb();
 
-  // ORB parameters
-  int nFeatures = settings->nFeatures();
-  int nLevels = settings->nLevels();
-  int fIniThFAST = settings->initThFAST();
-  int fMinThFAST = settings->minThFAST();
-  float fScaleFactor = settings->scaleFactor();
-
-  mpORBextractorLeft = std::make_shared<ORBextractor>(
-      nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST);
-
-  if (mSensor.isStereo())
-    mpORBextractorRight = std::make_shared<ORBextractor>(
+  {
+    // ORB parameters
+    const int nFeatures = settings->nFeatures();
+    const int nLevels = settings->nLevels();
+    const int fIniThFAST = settings->initThFAST();
+    const int fMinThFAST = settings->minThFAST();
+    const float fScaleFactor = settings->scaleFactor();
+
+    mpORBextractorLeft = std::make_shared<ORBextractor>(
         nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST);
 
-  if (mSensor.isMonocular())
-    mpIniORBextractor = std::make_shared<ORBextractor>(
-        5 * nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST);
+    if (mSensor.isStereo())
+      mpORBextractorRight = std::make_shared<ORBextractor>(
+          nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST);
+
+    if (mSensor.isMonocular())
+      mpIniORBextractor = std::make_shared<ORBextractor>(
+          5 * nFeatures, fScaleFactor, nLevels, fIniThFAST, fMinThFAST);
+  }
 
   // IMU parameters
   Sophus::SE3f Tbc = settings->Tbc();
@@ -828,7 +830,7 @@ void Tracking::PreintegrateIMU() {
       unique_lock<mutex> lock(mMutexImuQueue);
       if (!mlQueueImuData.empty()) {
         IMU::Point* m = &mlQueueImuData.front();
-        cout.precision(17);
+
         if (m->t < mCurrentFrame->mpPrevFrame->mTimeStamp - mImuPer) {
           mlQueueImuData.pop_front();
         } else if (m->t < mCurrentFrame->mTimeStamp - mImuPer) {
@@ -848,7 +850,7 @@ void Tracking::PreintegrateIMU() {
 
   const int n = mvImuFromLastFrame.size() - 1;
   if (n == 0) {
-    cout << "Empty IMU measurements vector!!!\n";
+    spdlog::warn("Empty IMU measurements vector!!!");
     return;
   }
 
@@ -907,14 +909,11 @@ void Tracking::PreintegrateIMU() {
   mCurrentFrame->mpLastKeyFrame = mpLastKeyFrame;
 
   mCurrentFrame->setIntegrated();
-
-  // Verbose::PrintMess("Preintegration is finished!! ",
-  // Verbose::VERBOSITY_DEBUG);
 }
 
 bool Tracking::PredictStateIMU() {
   if (!mCurrentFrame->mpPrevFrame) {
-    Verbose::PrintMess("No last frame", Verbose::VERBOSITY_NORMAL);
+    spdlog::debug("[Tracking::PredictStateIMU] No last frame");
     return false;
   }
 
@@ -982,28 +981,26 @@ void Tracking::Track() {
       std::chrono::steady_clock::now();
 
   if (bStepByStep) {
-    std::cout << "Tracking: Waiting to the next step" << std::endl;
+    spdlog::trace("Tracking: Waiting to the next step");
     while (!mbStep && bStepByStep) usleep(500);
     mbStep = false;
   }
 
   if (mpLocalMapper->mbBadImu) {
-    cout << "TRACK: Reset map because local mapper set the bad imu flag "
-         << endl;
+    spdlog::info("TRACK: Reset map because local mapper set the bad imu flag ");
     mpSystem->ResetActiveMap();
     return;
   }
 
   std::shared_ptr<Map> pCurrentMap = mpAtlas->GetCurrentMap();
   if (!pCurrentMap) {
-    cout << "ERROR: There is not an active map in the atlas" << endl;
+    spdlog::error("ERROR: There is not an active map in the atlas");
   }
 
   if (mState != NO_IMAGES_YET) {
     if (mLastFrame->mTimeStamp > mCurrentFrame->mTimeStamp) {
-      cerr
-          << "ERROR: Frame with a timestamp older than previous frame detected!"
-          << endl;
+      spdlog::error(
+          "ERROR: Frame with a timestamp older than previous frame detected!");
       unique_lock<mutex> lock(mMutexImuQueue);
       mlQueueImuData.clear();
       CreateMapInAtlas();
@@ -1014,18 +1011,19 @@ void Tracking::Track() {
       // mCurrentFrame->mnId << endl;
       if (mpAtlas->isInertial()) {
         if (mpAtlas->isImuInitialized()) {
-          cout << "Timestamp jump detected. State set to LOST. resetting IMU "
-                  "integration..."
-               << endl;
-          if (!pCurrentMap->GetIniertialBA2()) {
+          spdlog::warn(
+              "Timestamp jump detected. State set to LOST. resetting IMU "
+              "integration...");
+
+          if (!pCurrentMap->GetInertialBA2()) {
             mpSystem->ResetActiveMap();
           } else {
             CreateMapInAtlas();
           }
         } else {
-          cout << "Timestamp jump detected, before IMU initialization. "
-                  "resetting..."
-               << endl;
+          spdlog::warn(
+              "Timestamp jump detected, before IMU initialization. "
+              "resetting...");
           mpSystem->ResetActiveMap();
         }
         return;
@@ -1081,7 +1079,7 @@ void Tracking::Track() {
       MonocularInitialization();
     }
 
-    // mpFrameDrawer->Update(this);
+    if (mpFrameDrawer) mpFrameDrawer->Update(shared_from_this());
 
     // If rightly initialized, mState=OK
     if (mState != OK) {
@@ -1133,9 +1131,7 @@ void Tracking::Track() {
           spdlog::info("TRACK:   Tracking still bad, I am lost!");
           if (mCurrentFrame->mnId <=
                   (mnLastRelocFrameId + mnFramesToResetIMU) &&
-              (mSensor == SensorType::IMU_MONOCULAR ||
-               mSensor == SensorType::IMU_STEREO ||
-               mSensor == SensorType::IMU_RGBD)) {
+              mSensor.isImu()) {
             mState = LOST;
           } else if (pCurrentMap->KeyFramesInMap() > 10) {
             mState = RECENTLY_LOST;
@@ -1146,8 +1142,7 @@ void Tracking::Track() {
         }
       } else {
         if (mState == RECENTLY_LOST) {
-          Verbose::PrintMess("Lost for a short time",
-                             Verbose::VERBOSITY_NORMAL);
+          spdlog::info("Lost for a short time");
 
           bOK = true;
           if (mSensor.isImu()) {
@@ -1159,7 +1154,7 @@ void Tracking::Track() {
             if (mCurrentFrame->mTimeStamp - mTimeStampLost >
                 time_recently_lost) {
               mState = LOST;
-              Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
+              spdlog::info("Track Lost...");
               bOK = false;
             }
           } else {
@@ -1170,18 +1165,16 @@ void Tracking::Track() {
             // "mTimeStampLost:" << to_string(mTimeStampLost) << std::endl;
             if (mCurrentFrame->mTimeStamp - mTimeStampLost > 3.0f && !bOK) {
               mState = LOST;
-              Verbose::PrintMess("Track Lost...", Verbose::VERBOSITY_NORMAL);
+              spdlog::info("Track Lost...");
               bOK = false;
             }
           }
         } else if (mState == LOST) {
-          Verbose::PrintMess("A new map is started...",
-                             Verbose::VERBOSITY_NORMAL);
+          spdlog::info("A new map is started...");
 
           if (pCurrentMap->KeyFramesInMap() < 10) {
             mpSystem->ResetActiveMap();
-            Verbose::PrintMess("resetting current map...",
-                               Verbose::VERBOSITY_NORMAL);
+            spdlog::info("resetting current map...");
           } else {
             CreateMapInAtlas();
           }
@@ -1296,7 +1289,7 @@ void Tracking::Track() {
       if (bOK) {
         bOK = TrackLocalMap();
       }
-      if (!bOK) cout << "Fail to track local map!" << endl;
+      if (!bOK) spdlog::warn("Fail to track local map!");
     } else {
       // mbVO true means that there are few matches to MapPoints in the map. We
       // cannot retrieve a local map and therefore we do not perform
@@ -1315,15 +1308,12 @@ void Tracking::Track() {
       mState = OK;
     } else if (mState == OK) {
       spdlog::info("[Tracking::Track] TrackLocalMap() not OK");
-      if (mSensor == SensorType::IMU_MONOCULAR ||
-          mSensor == SensorType::IMU_STEREO ||
-          mSensor == SensorType::IMU_RGBD) {
-        Verbose::PrintMess("Track lost for less than one second...",
-                           Verbose::VERBOSITY_NORMAL);
+      if (mSensor.isImu()) {
+        spdlog::info("Track lost for less than one second...");
         if (!pCurrentMap->isImuInitialized() ||
-            !pCurrentMap->GetIniertialBA2()) {
-          cout << "IMU is not or recently initialized. resetting active map..."
-               << endl;
+            !pCurrentMap->GetInertialBA2()) {
+          spdlog::info(
+              "IMU is not or recently initialized. resetting active map...");
           mpSystem->ResetActiveMap();
         }
 
@@ -1344,8 +1334,7 @@ void Tracking::Track() {
     if ((mCurrentFrame->mnId < (mnLastRelocFrameId + mnFramesToResetIMU)) &&
         (mCurrentFrame->mnId > mnFramesToResetIMU) && mSensor.isImu() &&
         pCurrentMap->isImuInitialized()) {
-      Verbose::PrintMess("Saving pointer to frame. imu needs reset...",
-                         Verbose::VERBOSITY_NORMAL);
+      spdlog::info("Saving pointer to frame. imu needs reset...");
 
       // \todo{} check this situation.  This is a deep copy
       std::shared_ptr<Frame> pF = std::make_shared<Frame>(*mCurrentFrame);
@@ -1363,7 +1352,7 @@ void Tracking::Track() {
     if (pCurrentMap->isImuInitialized()) {
       if (bOK) {
         if (mCurrentFrame->mnId == (mnLastRelocFrameId + mnFramesToResetIMU)) {
-          cout << "resetting FRAME!!!" << endl;
+          spdlog::warn("resetting FRAME!!!");
           ResetFrameIMU();
         } else if (mCurrentFrame->mnId > (mnLastRelocFrameId + 30)) {
           mLastBias = mCurrentFrame->mImuBias;
@@ -1383,8 +1372,8 @@ void Tracking::Track() {
 #endif
 
     // Update drawer
-    mpFrameDrawer->Update(shared_from_this());
-    if (mCurrentFrame->isSet())
+    if (mpFrameDrawer) mpFrameDrawer->Update(shared_from_this());
+    if (mCurrentFrame->isSet() && mpMapDrawer)
       mpMapDrawer->SetCurrentCameraPose(mCurrentFrame->GetPose());
 
     if (bOK || mState == RECENTLY_LOST) {
@@ -1463,9 +1452,7 @@ void Tracking::Track() {
 
       if (mSensor.isImu()) {
         if (!pCurrentMap->isImuInitialized()) {
-          Verbose::PrintMess(
-              "Track lost before IMU initialisation, resetting...",
-              Verbose::VERBOSITY_QUIET);
+          spdlog::warn("Track lost before IMU initialisation, resetting...");
           mpSystem->ResetActiveMap();
           return;
         }
@@ -1831,9 +1818,7 @@ void Tracking::CreateMapInAtlas() {
   mbVelocity = false;
   // mnLastRelocFrameId = mnLastInitFrameId; // The last relocation KF_id is the
   // current id, because it is the new starting point for new map
-  Verbose::PrintMess(
-      "First frame id in map: " + to_string(mnLastInitFrameId + 1),
-      Verbose::VERBOSITY_NORMAL);
+  spdlog::debug("First frame id in map: {}", to_string(mnLastInitFrameId + 1));
   mbVO = false;  // Init value for know if there are enough MapPoints in the
                  // last KF
 
@@ -1880,11 +1865,10 @@ bool Tracking::TrackReferenceKeyFrame() {
   int nmatches =
       matcher.SearchByBoW(mpReferenceKF, mCurrentFrame, vpMapPointMatches);
 
-  cout << "Tracking::TrackReferenceKeyFrame:  Got " << nmatches << " matches"
-       << std::endl;
+  spdlog::debug("Tracking::TrackReferenceKeyFrame:  Got {} matches", nmatches);
 
   if (nmatches < 15) {
-    cout << "TRACK_REF_KF: Less than 15 matches!!" << std::endl;
+    spdlog::warn("TRACK_REF_KF: Less than 15 matches!!");
     return false;
   }
 
@@ -2028,20 +2012,18 @@ bool Tracking::TrackWithMotionModel() {
 
   // If few matches, uses a wider window search
   if (nmatches < 20) {
-    Verbose::PrintMess("Not enough matches, wider window search!!",
-                       Verbose::VERBOSITY_NORMAL);
+    spdlog::info("Not enough matches, wider window search!!");
     fill(mCurrentFrame->mvpMapPoints.begin(), mCurrentFrame->mvpMapPoints.end(),
          static_cast<MapPoint*>(NULL));
 
     nmatches = matcher.SearchByProjection(mCurrentFrame, mLastFrame, 2 * th,
                                           mSensor.isMonocular());
-    Verbose::PrintMess("Matches with wider search: " + to_string(nmatches),
-                       Verbose::VERBOSITY_NORMAL);
+    spdlog::debug("Matches with wider search: {}", to_string(nmatches));
   }
 
   if (nmatches < 20) {
-    Verbose::PrintMess("Not enough matches after wider search!!",
-                       Verbose::VERBOSITY_NORMAL);
+    spdlog::info("Not enough matches after wider search!!");
+
     if (mSensor.isImu())
       return true;
     else
@@ -2141,12 +2123,12 @@ bool Tracking::TrackLocalMap() {
         spdlog::info("TLM: PoseInertialOptimizationLastFrame");
         inliers = Optimizer::PoseInertialOptimizationLastFrame(
             mCurrentFrame);  // ,
-                             // !mpLastKeyFrame->GetMap()->GetIniertialBA1());
+                             // !mpLastKeyFrame->GetMap()->GetInertialBA1());
       } else {
         spdlog::info("TLM: PoseInertialOptimizationLastKeyFrame");
         inliers = Optimizer::PoseInertialOptimizationLastKeyFrame(
             mCurrentFrame);  // ,
-                             // !mpLastKeyFrame->GetMap()->GetIniertialBA1());
+                             // !mpLastKeyFrame->GetMap()->GetInertialBA1());
       }
     }
   }
@@ -2279,9 +2261,8 @@ bool Tracking::NeedNewKeyFrame() {
   int nNonTrackedClose = 0;
   int nTrackedClose = 0;
 
-  if (mSensor != SensorType::MONOCULAR &&
-      mSensor != SensorType::IMU_MONOCULAR) {
-    int N =
+  if (!mSensor.isMonocular()) {
+    const int N =
         (mCurrentFrame->Nleft == -1) ? mCurrentFrame->N : mCurrentFrame->Nleft;
     for (int i = 0; i < N; i++) {
       if (mCurrentFrame->mvDepth[i] > 0 &&
@@ -2292,10 +2273,9 @@ bool Tracking::NeedNewKeyFrame() {
           nNonTrackedClose++;
       }
     }
-    Verbose::PrintMess(
-        "[NEEDNEWKF]-> tracked close points: " + to_string(nTrackedClose) +
-            "; non tracked close points: " + to_string(nNonTrackedClose),
-        Verbose::VERBOSITY_NORMAL);  // Verbose::VERBOSITY_DEBUG);
+    spdlog::info(
+        "[NEEDNEWKF]-> tracked close points: {}; non tracked close points: {}",
+        to_string(nTrackedClose), to_string(nNonTrackedClose));
   }
 
   bool bNeedToInsertClose;
@@ -2325,9 +2305,8 @@ bool Tracking::NeedNewKeyFrame() {
       thRefRatio = 0.90f;
   }
 
-  spdlog::info("mnMatchesInliers: {}; nRefMatches: {}", mnMatchesInliers,
-               nRefMatches);
-  spdlog::info("thRefRatio: {}", thRefRatio);
+  spdlog::debug("mnMatchesInliers: {}; nRefMatches: {}; thRefRatio: {}",
+                mnMatchesInliers, nRefMatches, thRefRatio);
 
   // Condition 1a: More than "MaxFrames" have passed from last keyframe
   // insertion
@@ -2422,8 +2401,7 @@ void Tracking::CreateNewKeyFrame() {
     pKF->mPrevKF = mpLastKeyFrame;
     mpLastKeyFrame->mNextKF = pKF;
   } else {
-    Verbose::PrintMess("No last KF in KF creation!!",
-                       Verbose::VERBOSITY_NORMAL);
+    spdlog::info("No last KF in KF creation!!");
   }
   // Reset preintegration from last KF (Create new object)
   if (mSensor.isImu()) {
@@ -2566,14 +2544,11 @@ void Tracking::SearchLocalPoints() {
     int th = 10;
     if (mSensor == SensorType::RGBD || mSensor == SensorType::IMU_RGBD) th = 3;
     if (mpAtlas->isImuInitialized()) {
-      if (mpAtlas->GetCurrentMap()->GetIniertialBA2())
+      if (mpAtlas->GetCurrentMap()->GetInertialBA2())
         th = 2;
       else
         th = 6;
-    } else if (!mpAtlas->isImuInitialized() &&
-               (mSensor == SensorType::IMU_MONOCULAR ||
-                mSensor == SensorType::IMU_STEREO ||
-                mSensor == SensorType::IMU_RGBD)) {
+    } else if (!mpAtlas->isImuInitialized() && mSensor.isImu()) {
       th = 10;
     }
 
@@ -2607,10 +2582,7 @@ void Tracking::UpdateLocalPoints() {
   for (auto pKF : mvpLocalKeyFrames) {
     const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
 
-    for (vector<MapPoint*>::const_iterator itMP = vpMPs.begin(),
-                                           itEndMP = vpMPs.end();
-         itMP != itEndMP; itMP++) {
-      MapPoint* pMP = *itMP;
+    for (auto pMP : vpMPs) {
       if (!pMP) continue;
       if (pMP->mnTrackReferenceForFrame == mCurrentFrame->mnId) continue;
       if (!pMP->isBad()) {
@@ -2690,8 +2662,7 @@ void Tracking::UpdateLocalKeyFrames() {
   // already-included keyframes
   for (auto const& pKF : mvpLocalKeyFrames) {
     // Limit the number of keyframes
-    if (mvpLocalKeyFrames.size() > 80)  // 80
-      break;
+    if (mvpLocalKeyFrames.size() > 80) break;
 
     const auto vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
 
@@ -2749,8 +2720,7 @@ void Tracking::UpdateLocalKeyFrames() {
 }
 
 bool Tracking::Relocalization() {
-  Verbose::PrintMess("[Tracking::Relocalization] Starting relocalization",
-                     Verbose::VERBOSITY_NORMAL);
+  spdlog::debug("[Tracking::Relocalization] Starting relocalization");
   // Compute Bag of Words Vector
   mCurrentFrame->ComputeBoW();
 
@@ -2769,13 +2739,12 @@ bool Tracking::Relocalization() {
   spdlog::info("[Tracking::Relocalization] Found {} candidate KFs",
                vpCandidateKFs.size());
 
-  const int nKFs = vpCandidateKFs.size();
-
   // We perform first an ORB matching with each candidate
   // If enough matches are found we setup a PnP solver
   ORBmatcher matcher(0.75, true);
 
-  vector<MLPnPsolver*> vpMLPnPsolvers;
+  const int nKFs = vpCandidateKFs.size();
+  vector<std::shared_ptr<MLPnPsolver>> vpMLPnPsolvers;
   vpMLPnPsolvers.resize(nKFs);
 
   vector<vector<MapPoint*>> vvpMapPointMatches;
@@ -2799,8 +2768,8 @@ bool Tracking::Relocalization() {
         vbDiscarded[i] = true;
         continue;
       } else {
-        MLPnPsolver* pSolver =
-            new MLPnPsolver(mCurrentFrame, vvpMapPointMatches[i]);
+        std::shared_ptr<MLPnPsolver> pSolver =
+            std::make_shared<MLPnPsolver>(mCurrentFrame, vvpMapPointMatches[i]);
         pSolver->SetRansacParameters(
             0.99, 10, 300, 6, 0.5,
             5.991);  // This solver needs at least 6 points
@@ -2827,7 +2796,7 @@ bool Tracking::Relocalization() {
       int nInliers;
       bool bNoMore;
 
-      MLPnPsolver* pSolver = vpMLPnPsolvers[i];
+      std::shared_ptr<MLPnPsolver>& pSolver = vpMLPnPsolvers[i];
       Eigen::Matrix4f eigTcw;
       bool bTcw = pSolver->iterate(5, bNoMore, vbInliers, nInliers, eigTcw);
 
@@ -2918,7 +2887,7 @@ bool Tracking::Relocalization() {
 }
 
 void Tracking::Reset(bool bLocMap) {
-  Verbose::PrintMess("System resetting", Verbose::VERBOSITY_NORMAL);
+  spdlog::info("System resetting");
 
   if (mpViewer) {
     mpViewer->RequestStop();
@@ -2927,28 +2896,22 @@ void Tracking::Reset(bool bLocMap) {
 
   // Reset Local Mapping
   if (!bLocMap) {
-    Verbose::PrintMess("!! resetting Local Mapper...",
-                       Verbose::VERBOSITY_NORMAL);
+    spdlog::debug("!! resetting Local Mapper...");
     mpLocalMapper->RequestReset();
-    Verbose::PrintMess("!!    done", Verbose::VERBOSITY_NORMAL);
   }
 
   // Reset Loop Closing
-  Verbose::PrintMess("!! resetting Loop Closing...", Verbose::VERBOSITY_NORMAL);
+  spdlog::debug("!! resetting Loop Closing...");
   mpLoopClosing->RequestReset();
-  Verbose::PrintMess("!!   done", Verbose::VERBOSITY_NORMAL);
 
   // Clear BoW Database
-  Verbose::PrintMess("!! resetting Database...", Verbose::VERBOSITY_NORMAL);
+  spdlog::debug("!! resetting Database...");
   mpKeyFrameDB->clear();
-  Verbose::PrintMess("!!    done", Verbose::VERBOSITY_NORMAL);
 
   // Clear Map (this erase MapPoints and KeyFrames)
   mpAtlas->clearAtlas();
   mpAtlas->CreateNewMap();
-  if (mSensor == SensorType::IMU_STEREO ||
-      mSensor == SensorType::IMU_MONOCULAR || mSensor == SensorType::IMU_RGBD)
-    mpAtlas->SetInertialSensor();
+  if (mSensor.isImu()) mpAtlas->SetInertialSensor();
   mnInitialFrameId = 0;
 
   KeyFrame::nNextId = 0;
@@ -2971,11 +2934,11 @@ void Tracking::Reset(bool bLocMap) {
 
   if (mpViewer) mpViewer->Release();
 
-  Verbose::PrintMess("!! End resetting! ", Verbose::VERBOSITY_NORMAL);
+  spdlog::info("!! End resetting!");
 }
 
 void Tracking::ResetActiveMap(bool bLocMap) {
-  Verbose::PrintMess("!! Active map resetting", Verbose::VERBOSITY_NORMAL);
+  spdlog::info("!! Active map resetting");
   if (mpViewer) {
     mpViewer->RequestStop();
     while (!mpViewer->isStopped()) usleep(3000);
@@ -2984,21 +2947,17 @@ void Tracking::ResetActiveMap(bool bLocMap) {
   std::shared_ptr<Map> pMap(mpAtlas->GetCurrentMap());
 
   if (!bLocMap) {
-    Verbose::PrintMess("!! resetting Local Mapper...",
-                       Verbose::VERBOSITY_VERY_VERBOSE);
+    spdlog::info("!! resetting Local Mapper...");
     mpLocalMapper->RequestResetActiveMap(pMap);
-    Verbose::PrintMess("!!   done", Verbose::VERBOSITY_VERY_VERBOSE);
   }
 
   // Reset Loop Closing
-  Verbose::PrintMess("!! resetting Loop Closing...", Verbose::VERBOSITY_NORMAL);
+  spdlog::info("!! resetting Loop Closing...");
   mpLoopClosing->RequestResetActiveMap(pMap);
-  Verbose::PrintMess("!!   done", Verbose::VERBOSITY_NORMAL);
 
   // Clear BoW Database
-  Verbose::PrintMess("!! resetting Database", Verbose::VERBOSITY_NORMAL);
+  spdlog::info("!! resetting Database");
   mpKeyFrameDB->clearMap(pMap);  // Only clear the active map references
-  Verbose::PrintMess("!!    done", Verbose::VERBOSITY_NORMAL);
 
   // Clear Map (this erase MapPoints and KeyFrames)
   mpAtlas->clearMap();
@@ -3014,16 +2973,13 @@ void Tracking::ResetActiveMap(bool bLocMap) {
   list<bool> lbLost;
   // lbLost.reserve(mlbLost.size());
   unsigned int index = mnFirstFrameId;
-  cout << "mnFirstFrameId = " << mnFirstFrameId << endl;
-  for (auto pMap : mpAtlas->GetAllMaps()) {
+  for (auto const& pMap : mpAtlas->GetAllMaps()) {
     if (pMap->GetAllKeyFrames().size() > 0) {
       if (index > pMap->GetLowerKFID()) index = pMap->GetLowerKFID();
     }
   }
 
-  // cout << "First Frame id: " << index << endl;
   int num_lost = 0;
-  cout << "mnInitialFrameId = " << mnInitialFrameId << endl;
 
   for (list<bool>::iterator ilbL = mlbLost.begin(); ilbL != mlbLost.end();
        ilbL++) {

From 1599138d33c67fe419d3fc3dc2c8ed474dfa4a0e Mon Sep 17 00:00:00 2001
From: Aaron Marburg <amarburg@uw.edu>
Date: Sun, 4 Jan 2026 04:11:03 +0000
Subject: [PATCH 6/9] Clean up Pinhole

---
 include/CameraModels/Pinhole.h | 26 +++++---------------------
 src/CameraModels/Pinhole.cpp   | 28 ++++++++++++++++++++++++++--
 2 files changed, 31 insertions(+), 23 deletions(-)

diff --git a/include/CameraModels/Pinhole.h b/include/CameraModels/Pinhole.h
index d3ec38eb653..f88b0df8026 100644
--- a/include/CameraModels/Pinhole.h
+++ b/include/CameraModels/Pinhole.h
@@ -39,29 +39,13 @@ class Pinhole : public GeometricCamera {
   }
 
  public:
-  Pinhole() {
-    mvParameters.resize(4);
-    mnId = nNextId++;
-    mnType = CAM_PINHOLE;
-  }
+  Pinhole();
 
-  explicit Pinhole(const std::vector<float> _vParameters)
-      : GeometricCamera(_vParameters), tvr(nullptr) {
-    assert(mvParameters.size() == 4);
-    mnId = nNextId++;
-    mnType = CAM_PINHOLE;
-  }
+  explicit Pinhole(const std::vector<float> _vParameters);
 
-  explicit Pinhole(Pinhole* pPinhole)
-      : GeometricCamera(pPinhole->mvParameters), tvr(nullptr) {
-    assert(mvParameters.size() == 4);
-    mnId = nNextId++;
-    mnType = CAM_PINHOLE;
-  }
+  explicit Pinhole(const Pinhole& pinhole);
 
-  ~Pinhole() {
-    if (tvr) delete tvr;
-  }
+  ~Pinhole();
 
   cv::Point2f project(const cv::Point3f& p3D);
   Eigen::Vector2d project(const Eigen::Vector3d& v3D);
@@ -106,7 +90,7 @@ class Pinhole : public GeometricCamera {
  private:
   // Parameters vector corresponds to
   //       [fx, fy, cx, cy]
-  TwoViewReconstruction* tvr;
+  std::shared_ptr<TwoViewReconstruction> tvr;
 };
 }  // namespace ORB_SLAM3
 
diff --git a/src/CameraModels/Pinhole.cpp b/src/CameraModels/Pinhole.cpp
index 667c7866ae3..2223e66bc0c 100644
--- a/src/CameraModels/Pinhole.cpp
+++ b/src/CameraModels/Pinhole.cpp
@@ -32,6 +32,30 @@ namespace ORB_SLAM3 {
 
 long unsigned int GeometricCamera::nNextId = 0;
 
+Pinhole::Pinhole() : GeometricCamera(), tvr() {
+  mvParameters.resize(4);
+  mnId = nNextId++;
+  mnType = CAM_PINHOLE;
+}
+
+Pinhole::Pinhole(const std::vector<float> _vParameters)
+    : GeometricCamera(_vParameters), tvr() {
+  assert(mvParameters.size() == 4);
+  mnId = nNextId++;
+  mnType = CAM_PINHOLE;
+}
+
+Pinhole::Pinhole(const Pinhole &pinhole)
+    : GeometricCamera(pinhole.mvParameters), tvr() {
+  assert(mvParameters.size() == 4);
+  mnId = nNextId++;
+  mnType = CAM_PINHOLE;
+}
+
+Pinhole::~Pinhole() {
+  if (tvr) tvr.reset();
+}
+
 cv::Point2f Pinhole::project(const cv::Point3f &p3D) {
   return cv::Point2f(mvParameters[0] * p3D.x / p3D.z + mvParameters[2],
                      mvParameters[1] * p3D.y / p3D.z + mvParameters[3]);
@@ -91,8 +115,8 @@ bool Pinhole::ReconstructWithTwoViews(const std::vector<cv::KeyPoint> &vKeys1,
                                       std::vector<cv::Point3f> &vP3D,
                                       std::vector<bool> &vbTriangulated) {
   if (!tvr) {
-    Eigen::Matrix3f K = this->toK_();
-    tvr = new TwoViewReconstruction(K);
+    const Eigen::Matrix3f K = this->toK_();
+    tvr = std::make_shared<TwoViewReconstruction>(K);
   }
 
   return tvr->Reconstruct(vKeys1, vKeys2, vMatches12, T21, vP3D,

From 64d3fdbb5b21129e473694e56c3863aad05da9b6 Mon Sep 17 00:00:00 2001
From: Aaron Marburg <amarburg@uw.edu>
Date: Tue, 6 Jan 2026 03:53:29 +0000
Subject: [PATCH 7/9] Re-enabled building examples with CMake

---
 CMakeLists.txt                                | 217 ++----
 .../Monocular-Inertial/mono_inertial_euroc.cc |  55 +-
 .../mono_inertial_tum_vi.cc                   |  59 +-
 Examples/Monocular/mono_euroc.cc              |  57 +-
 Examples/Monocular/mono_kitti.cc              |  46 +-
 Examples/Monocular/mono_tum.cc                |  44 +-
 Examples/Monocular/mono_tum_vi.cc             |  53 +-
 Examples/{REAMDME.md => README.md}            |   0
 Examples/RGB-D-Inertial/RealSense_D435i.yaml  |  90 ---
 .../rgbd_inertial_realsense_D435i.cc          | 556 ----------------
 Examples/RGB-D/rgbd_tum.cc                    |  31 +-
 Examples/ROS/ORB_SLAM3/Asus.yaml              |  69 --
 Examples/ROS/ORB_SLAM3/CMakeLists.txt         | 118 ----
 Examples/ROS/ORB_SLAM3/Tello.yaml             | 101 ---
 Examples/ROS/ORB_SLAM3/manifest.xml           |  12 -
 Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.cc     | 629 ------------------
 Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.h      | 122 ----
 Examples/ROS/ORB_SLAM3/src/AR/ros_mono_ar.cc  | 138 ----
 Examples/ROS/ORB_SLAM3/src/ros_mono.cc        |  89 ---
 .../ROS/ORB_SLAM3/src/ros_mono_inertial.cc    | 184 -----
 Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc        | 111 ----
 Examples/ROS/ORB_SLAM3/src/ros_stereo.cc      | 172 -----
 .../ROS/ORB_SLAM3/src/ros_stereo_inertial.cc  | 270 --------
 .../Stereo-Inertial/stereo_inertial_euroc.cc  |  37 +-
 .../Stereo-Inertial/stereo_inertial_tum_vi.cc |  53 +-
 Examples/Stereo/stereo_euroc.cc               |  41 +-
 Examples/Stereo/stereo_kitti.cc               |  48 +-
 Examples/Stereo/stereo_tum_vi.cc              |  52 +-
 README.md                                     |  17 +-
 include/Expected.h                            |  56 ++
 include/Frame.h                               |   2 +-
 include/ImuTypes.h                            |   2 +-
 include/KeyFrameDatabase.h                    |   2 +-
 include/Map.h                                 |   3 -
 include/ORBmatcher.h                          |   2 +-
 include/Settings.h                            |   6 +-
 include/System.h                              |  22 +-
 src/Frame.cc                                  |   8 +-
 src/G2oTypes.cc                               |  10 +-
 src/KeyFrameDatabase.cc                       |   4 +-
 src/Map.cc                                    |   5 -
 src/ORBmatcher.cc                             |   2 +-
 src/Optimizer.cc                              | 208 +++---
 src/SettingsLoader.cc                         |   5 +-
 src/System.cc                                 |  40 +-
 src/Tracking.cc                               |  14 +-
 46 files changed, 517 insertions(+), 3345 deletions(-)
 rename Examples/{REAMDME.md => README.md} (100%)
 delete mode 100755 Examples/RGB-D-Inertial/RealSense_D435i.yaml
 delete mode 100644 Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
 delete mode 100644 Examples/ROS/ORB_SLAM3/Asus.yaml
 delete mode 100644 Examples/ROS/ORB_SLAM3/CMakeLists.txt
 delete mode 100644 Examples/ROS/ORB_SLAM3/Tello.yaml
 delete mode 100644 Examples/ROS/ORB_SLAM3/manifest.xml
 delete mode 100644 Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.cc
 delete mode 100644 Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.h
 delete mode 100644 Examples/ROS/ORB_SLAM3/src/AR/ros_mono_ar.cc
 delete mode 100644 Examples/ROS/ORB_SLAM3/src/ros_mono.cc
 delete mode 100644 Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc
 delete mode 100644 Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc
 delete mode 100644 Examples/ROS/ORB_SLAM3/src/ros_stereo.cc
 delete mode 100644 Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc
 create mode 100644 include/Expected.h

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 20644fd84de..a03a584b5f2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8)
+cmake_minimum_required(VERSION 3.28)
 project(ORB_SLAM3)
 
 IF(NOT CMAKE_BUILD_TYPE)
@@ -7,14 +7,13 @@ ENDIF()
 
 MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
 
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall   -O3")
-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")
+# Squelch "reorder" warnings, there are many
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS}  -Wall -Wno-reorder")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wno-reorder")
+set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -march=native -O3")
+set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native -O3")
 
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_EXTENSIONS OFF)
-add_definitions(-DCOMPILEDWITHC14)
+set(CMAKE_CXX_STANDARD 17)
 
 LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
 
@@ -28,102 +27,69 @@ MESSAGE(${OpenCV_VERSION})
 
 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
-find_package(realsense2)
 find_package(Sophus REQUIRED)
+find_package(g2o REQUIRED)
+find_package(fmt REQUIRED)
+find_package(spdlog REQUIRED)
+find_package(Boost REQUIRED COMPONENTS serialization)
 
 include_directories(
-${PROJECT_SOURCE_DIR}
-${PROJECT_SOURCE_DIR}/include
-${PROJECT_SOURCE_DIR}/include/CameraModels
-${EIGEN3_INCLUDE_DIR}
-${Pangolin_INCLUDE_DIRS}
+        ${PROJECT_SOURCE_DIR}
+        ${PROJECT_SOURCE_DIR}/include
+        ${PROJECT_SOURCE_DIR}/include/CameraModels
+        ${EIGEN3_INCLUDE_DIR}
+        ${Pangolin_INCLUDE_DIRS}
 )
 
 set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
 
 add_library(${PROJECT_NAME} SHARED
-src/System.cc
-src/Tracking.cc
-src/LocalMapping.cc
-src/LoopClosing.cc
-src/ORBextractor.cc
-src/ORBmatcher.cc
-src/FrameDrawer.cc
-src/Converter.cc
-src/MapPoint.cc
-src/KeyFrame.cc
-src/Atlas.cc
-src/Map.cc
-src/MapDrawer.cc
-src/Optimizer.cc
-src/Frame.cc
-src/KeyFrameDatabase.cc
-src/Sim3Solver.cc
-src/Viewer.cc
-src/ImuTypes.cc
-src/G2oTypes.cc
-src/CameraModels/Pinhole.cpp
-src/CameraModels/KannalaBrandt8.cpp
-src/OptimizableTypes.cpp
-src/MLPnPsolver.cpp
-src/GeometricTools.cc
-src/TwoViewReconstruction.cc
-src/Config.cc
-src/Settings.cc
-include/System.h
-include/Tracking.h
-include/LocalMapping.h
-include/LoopClosing.h
-include/ORBextractor.h
-include/ORBmatcher.h
-include/FrameDrawer.h
-include/Converter.h
-include/MapPoint.h
-include/KeyFrame.h
-include/Atlas.h
-include/Map.h
-include/MapDrawer.h
-include/Optimizer.h
-include/Frame.h
-include/KeyFrameDatabase.h
-include/Sim3Solver.h
-include/Viewer.h
-include/ImuTypes.h
-include/G2oTypes.h
-include/CameraModels/GeometricCamera.h
-include/CameraModels/Pinhole.h
-include/CameraModels/KannalaBrandt8.h
-include/OptimizableTypes.h
-include/MLPnPsolver.h
-include/GeometricTools.h
-include/TwoViewReconstruction.h
-include/SerializationUtils.h
-include/Config.h
-include/Settings.h)
-
-add_subdirectory(Thirdparty/g2o)
+        src/Atlas.cc
+        src/CameraModels/KannalaBrandt8.cpp
+        src/CameraModels/Pinhole.cpp
+        src/Converter.cc
+        src/Frame.cc
+        src/FrameDrawer.cc
+        src/G2oTypes.cc
+        src/GeometricTools.cc
+        src/ImuTypes.cc
+        src/KeyFrame.cc
+        src/KeyFrameDatabase.cc
+        src/LocalMapping.cc
+        src/LoopClosing.cc
+        src/Map.cc
+        src/MapDrawer.cc
+        src/MapPoint.cc
+        src/MLPnPsolver.cpp
+        src/OptimizableTypes.cpp
+        src/Optimizer.cc
+        src/ORBextractor.cc
+        src/ORBmatcher.cc
+        src/Settings.cc
+        src/SettingsLoader.cc
+        src/Sim3Solver.cc
+        src/System.cc
+        src/Tracking.cc
+        src/TwoViewReconstruction.cc
+        src/Viewer.cc
+)
 
 target_link_libraries(${PROJECT_NAME}
-${OpenCV_LIBS}
-${EIGEN3_LIBS}
-${Pangolin_LIBRARIES}
-${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
-${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
--lboost_serialization
--lcrypto
+        ${OpenCV_LIBS}
+        Eigen3::Eigen
+        Sophus::Sophus
+        ${Pangolin_LIBRARIES}
+        ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
+        fmt::fmt
+        g2o::core
+        g2o::types_sim3
+        g2o::types_slam3d
+        spdlog::spdlog
+        Boost::boost
+        Boost::serialization
+        -lcrypto
 )
 
-# If RealSense SDK is found the library is added and its examples compiled
-if(realsense2_FOUND)
-    include_directories(${PROJECT_NAME}
-    ${realsense_INCLUDE_DIR}
-    )
-    target_link_libraries(${PROJECT_NAME}
-    ${realsense2_LIBRARY}
-    )
-endif()
-
-
 # Build examples
 
 # RGB-D examples
@@ -133,22 +99,6 @@ add_executable(rgbd_tum
         Examples/RGB-D/rgbd_tum.cc)
 target_link_libraries(rgbd_tum ${PROJECT_NAME})
 
-if(realsense2_FOUND)
-    add_executable(rgbd_realsense_D435i
-            Examples/RGB-D/rgbd_realsense_D435i.cc)
-    target_link_libraries(rgbd_realsense_D435i ${PROJECT_NAME})
-endif()
-
-
-# RGB-D inertial examples
-set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D-Inertial)
-
-if(realsense2_FOUND)
-    add_executable(rgbd_inertial_realsense_D435i
-            Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc)
-    target_link_libraries(rgbd_inertial_realsense_D435i ${PROJECT_NAME})
-endif()
-
 #Stereo examples
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo)
 
@@ -164,16 +114,6 @@ add_executable(stereo_tum_vi
         Examples/Stereo/stereo_tum_vi.cc)
 target_link_libraries(stereo_tum_vi ${PROJECT_NAME})
 
-if(realsense2_FOUND)
-    add_executable(stereo_realsense_t265
-            Examples/Stereo/stereo_realsense_t265.cc)
-    target_link_libraries(stereo_realsense_t265 ${PROJECT_NAME})
-
-    add_executable(stereo_realsense_D435i
-            Examples/Stereo/stereo_realsense_D435i.cc)
-    target_link_libraries(stereo_realsense_D435i ${PROJECT_NAME})
-endif()
-
 #Monocular examples
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular)
 
@@ -193,16 +133,6 @@ add_executable(mono_tum_vi
         Examples/Monocular/mono_tum_vi.cc)
 target_link_libraries(mono_tum_vi ${PROJECT_NAME})
 
-if(realsense2_FOUND)
-    add_executable(mono_realsense_t265
-            Examples/Monocular/mono_realsense_t265.cc)
-    target_link_libraries(mono_realsense_t265 ${PROJECT_NAME})
-
-    add_executable(mono_realsense_D435i
-            Examples/Monocular/mono_realsense_D435i.cc)
-    target_link_libraries(mono_realsense_D435i ${PROJECT_NAME})
-endif()
-
 #Monocular inertial examples
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular-Inertial)
 
@@ -214,16 +144,6 @@ add_executable(mono_inertial_tum_vi
         Examples/Monocular-Inertial/mono_inertial_tum_vi.cc)
 target_link_libraries(mono_inertial_tum_vi ${PROJECT_NAME})
 
-if(realsense2_FOUND)
-    add_executable(mono_inertial_realsense_t265
-            Examples/Monocular-Inertial/mono_inertial_realsense_t265.cc)
-    target_link_libraries(mono_inertial_realsense_t265 ${PROJECT_NAME})
-
-    add_executable(mono_inertial_realsense_D435i
-            Examples/Monocular-Inertial/mono_inertial_realsense_D435i.cc)
-    target_link_libraries(mono_inertial_realsense_D435i ${PROJECT_NAME})
-endif()
-
 #Stereo Inertial examples
 set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo-Inertial)
 
@@ -234,24 +154,3 @@ target_link_libraries(stereo_inertial_euroc ${PROJECT_NAME})
 add_executable(stereo_inertial_tum_vi
         Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc)
 target_link_libraries(stereo_inertial_tum_vi ${PROJECT_NAME})
-
-if(realsense2_FOUND)
-    add_executable(stereo_inertial_realsense_t265
-            Examples/Stereo-Inertial/stereo_inertial_realsense_t265.cc)
-    target_link_libraries(stereo_inertial_realsense_t265 ${PROJECT_NAME})
-
-    add_executable(stereo_inertial_realsense_D435i
-            Examples/Stereo-Inertial/stereo_inertial_realsense_D435i.cc)
-    target_link_libraries(stereo_inertial_realsense_D435i ${PROJECT_NAME})
-endif()
-
-set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Calibration)
-if(realsense2_FOUND)
-    add_executable(recorder_realsense_D435i
-            Examples/Calibration/recorder_realsense_D435i.cc)
-    target_link_libraries(recorder_realsense_D435i ${PROJECT_NAME})
-
-    add_executable(recorder_realsense_T265
-            Examples/Calibration/recorder_realsense_T265.cc)
-    target_link_libraries(recorder_realsense_T265 ${PROJECT_NAME})
-endif()
diff --git a/Examples/Monocular-Inertial/mono_inertial_euroc.cc b/Examples/Monocular-Inertial/mono_inertial_euroc.cc
index df4a3ab12f4..a85094371bd 100644
--- a/Examples/Monocular-Inertial/mono_inertial_euroc.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_euroc.cc
@@ -120,9 +120,17 @@ int main(int argc, char *argv[]) {
 
   // 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);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::IMU_MONOCULAR, true);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   double t_resize = 0.f;
   double t_track = 0.f;
@@ -149,30 +157,21 @@ int main(int argc, char *argv[]) {
 
       if (imageScale != 1.f) {
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
 #endif
         int width = im.cols * imageScale;
         int height = im.rows * imageScale;
         cv::resize(im, im, cv::Size(width, height));
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
+
         t_resize = std::chrono::duration_cast<
                        std::chrono::duration<double, std::milli> >(
                        t_End_Resize - t_Start_Resize)
                        .count();
-        SLAM.InsertResizeTime(t_resize);
+        SLAM->InsertResizeTime(t_resize);
 #endif
       }
 
@@ -192,33 +191,23 @@ int main(int argc, char *argv[]) {
         }
       }
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
       // Pass the image to the SLAM system
       // cout << "tframe = " << tframe << endl;
-      SLAM.TrackMonocular(im, tframe,
-                          vImuMeas);  // TODO change to monocular_inertial
+      SLAM->TrackMonocular(im, tframe,
+                           vImuMeas);  // TODO change to monocular_inertial
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
 #ifdef REGISTER_TIMES
       t_track =
           t_resize + std::chrono::duration_cast<
                          std::chrono::duration<double, std::milli> >(t2 - t1)
                          .count();
-      SLAM.InsertTrackTime(t_track);
+      SLAM->InsertTrackTime(t_track);
 #endif
 
       double ttrack =
@@ -241,22 +230,22 @@ int main(int argc, char *argv[]) {
     if (seq < num_seq - 1) {
       cout << "Changing the dataset" << endl;
 
-      SLAM.ChangeDataset();
+      SLAM->ChangeDataset();
     }
   }
 
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Save camera trajectory
   if (bFileName) {
     const string kf_file = "kf_" + string(argv[argc - 1]) + ".txt";
     const string f_file = "f_" + string(argv[argc - 1]) + ".txt";
-    SLAM.SaveTrajectoryEuRoC(f_file);
-    SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+    SLAM->SaveTrajectoryEuRoC(f_file);
+    SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
   } else {
-    SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
-    SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+    SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
+    SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
   }
 
   return 0;
diff --git a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
index 45f57fe18aa..971f8172653 100644
--- a/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
+++ b/Examples/Monocular-Inertial/mono_inertial_tum_vi.cc
@@ -121,9 +121,17 @@ int main(int argc, char **argv) {
 
   // 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);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::IMU_MONOCULAR, true, file_name);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   double t_resize = 0.f;
   double t_track = 0.f;
@@ -172,63 +180,44 @@ int main(int argc, char **argv) {
 
       if (imageScale != 1.f) {
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
 #endif
         int width = im.cols * imageScale;
         int height = im.rows * imageScale;
         cv::resize(im, im, cv::Size(width, height));
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
+
         t_resize = std::chrono::duration_cast<
                        std::chrono::duration<double, std::milli> >(
                        t_End_Resize - t_Start_Resize)
                        .count();
-        SLAM.InsertResizeTime(t_resize);
+        SLAM->InsertResizeTime(t_resize);
 #endif
       }
 
       // cout << "first imu: " << first_imu[seq] << endl;
       /*cout << "first imu time: " << fixed << vTimestampsImu[first_imu] <<
       endl; cout << "size vImu: " << vImuMeas.size() << endl;*/
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
       // Pass the image to the SLAM system
       // cout << "tframe = " << tframe << endl;
-      SLAM.TrackMonocular(im, tframe,
-                          vImuMeas);  // TODO change to monocular_inertial
+      SLAM->TrackMonocular(im, tframe,
+                           vImuMeas);  // TODO change to monocular_inertial
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
 #ifdef REGISTER_TIMES
       t_track =
           t_resize + std::chrono::duration_cast<
                          std::chrono::duration<double, std::milli> >(t2 - t1)
                          .count();
-      SLAM.InsertTrackTime(t_track);
+      SLAM->InsertTrackTime(t_track);
 #endif
 
       double ttrack =
@@ -251,13 +240,13 @@ int main(int argc, char **argv) {
     if (seq < num_seq - 1) {
       cout << "Changing the dataset" << endl;
 
-      SLAM.ChangeDataset();
+      SLAM->ChangeDataset();
     }
   }
 
   // cout << "ttrack_tot = " << ttrack_tot << std::endl;
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Tracking time statistics
 
@@ -266,11 +255,11 @@ int main(int argc, char **argv) {
   if (bFileName) {
     const string kf_file = "kf_" + string(argv[argc - 1]) + ".txt";
     const string f_file = "f_" + string(argv[argc - 1]) + ".txt";
-    SLAM.SaveTrajectoryEuRoC(f_file);
-    SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+    SLAM->SaveTrajectoryEuRoC(f_file);
+    SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
   } else {
-    SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
-    SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+    SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
+    SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
   }
 
   sort(vTimesTrack.begin(), vTimesTrack.end());
@@ -285,8 +274,8 @@ int main(int argc, char **argv) {
   /*const string kf_file =  "kf_" + ss.str() + ".txt";
   const string f_file =  "f_" + ss.str() + ".txt";
 
-  SLAM.SaveTrajectoryEuRoC(f_file);
-  SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);*/
+  SLAM->SaveTrajectoryEuRoC(f_file);
+  SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);*/
 
   return 0;
 }
diff --git a/Examples/Monocular/mono_euroc.cc b/Examples/Monocular/mono_euroc.cc
index 051c6b81741..1dca78b950d 100644
--- a/Examples/Monocular/mono_euroc.cc
+++ b/Examples/Monocular/mono_euroc.cc
@@ -84,10 +84,20 @@ 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);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::MONOCULAR, false);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   double t_resize = 0.f;
   double t_track = 0.f;
@@ -111,59 +121,40 @@ int main(int argc, char **argv) {
 
       if (imageScale != 1.f) {
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
 #endif
         int width = im.cols * imageScale;
         int height = im.rows * imageScale;
         cv::resize(im, im, cv::Size(width, height));
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
+
         t_resize = std::chrono::duration_cast<
                        std::chrono::duration<double, std::milli> >(
                        t_End_Resize - t_Start_Resize)
                        .count();
-        SLAM.InsertResizeTime(t_resize);
+        SLAM->InsertResizeTime(t_resize);
 #endif
       }
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
       // Pass the image to the SLAM system
       // cout << "tframe = " << tframe << endl;
-      SLAM.TrackMonocular(im, tframe);  // TODO change to monocular_inertial
+      SLAM->TrackMonocular(im, tframe);  // TODO change to monocular_inertial
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
 #ifdef REGISTER_TIMES
       t_track =
           t_resize + std::chrono::duration_cast<
                          std::chrono::duration<double, std::milli> >(t2 - t1)
                          .count();
-      SLAM.InsertTrackTime(t_track);
+      SLAM->InsertTrackTime(t_track);
 #endif
 
       double ttrack =
@@ -193,26 +184,26 @@ int main(int argc, char **argv) {
           "./SubMaps/kf_SubMap_" + std::to_string(seq) + ".txt";
       string f_file_submap =
           "./SubMaps/f_SubMap_" + std::to_string(seq) + ".txt";
-      SLAM.SaveTrajectoryEuRoC(f_file_submap);
-      SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file_submap);
+      SLAM->SaveTrajectoryEuRoC(f_file_submap);
+      SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file_submap);
 
       cout << "Changing the dataset" << endl;
 
-      SLAM.ChangeDataset();
+      SLAM->ChangeDataset();
     }
   }
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Save camera trajectory
   if (bFileName) {
     const string kf_file = "kf_" + string(argv[argc - 1]) + ".txt";
     const string f_file = "f_" + string(argv[argc - 1]) + ".txt";
-    SLAM.SaveTrajectoryEuRoC(f_file);
-    SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+    SLAM->SaveTrajectoryEuRoC(f_file);
+    SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
   } else {
-    SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
-    SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+    SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
+    SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
   }
 
   return 0;
diff --git a/Examples/Monocular/mono_kitti.cc b/Examples/Monocular/mono_kitti.cc
index 717aa4a395e..03193a8d823 100644
--- a/Examples/Monocular/mono_kitti.cc
+++ b/Examples/Monocular/mono_kitti.cc
@@ -51,8 +51,17 @@ int main(int argc, char **argv) {
 
   // 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, true);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::MONOCULAR, true);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   // Vector for tracking time statistics
   vector<float> vTimesTrack;
@@ -81,50 +90,31 @@ int main(int argc, char **argv) {
 
     if (imageScale != 1.f) {
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
       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();
-#endif
 #endif
       int width = im.cols * imageScale;
       int height = im.rows * imageScale;
       cv::resize(im, im, cv::Size(width, height));
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
       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();
-#endif
+
       t_resize = std::chrono::duration_cast<
                      std::chrono::duration<double, std::milli> >(t_End_Resize -
                                                                  t_Start_Resize)
                      .count();
-      SLAM.InsertResizeTime(t_resize);
+      SLAM->InsertResizeTime(t_resize);
 #endif
     }
 
-#ifdef COMPILEDWITHC14
     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();
-#endif
 
     // Pass the image to the SLAM system
-    SLAM.TrackMonocular(im, tframe, vector<ORB_SLAM3::IMU::Point>(),
-                        vstrImageFilenames[ni]);
+    SLAM->TrackMonocular(im, tframe, vector<ORB_SLAM3::IMU::Point>(),
+                         vstrImageFilenames[ni]);
 
-#ifdef COMPILEDWITHC14
     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();
-#endif
 
 #ifdef REGISTER_TIMES
     t_track =
@@ -132,7 +122,7 @@ int main(int argc, char **argv) {
         std::chrono::duration_cast<std::chrono::duration<double, std::milli> >(
             t2 - t1)
             .count();
-    SLAM.InsertTrackTime(t_track);
+    SLAM->InsertTrackTime(t_track);
 #endif
 
     double ttrack =
@@ -152,7 +142,7 @@ int main(int argc, char **argv) {
   }
 
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Tracking time statistics
   sort(vTimesTrack.begin(), vTimesTrack.end());
@@ -165,7 +155,7 @@ int main(int argc, char **argv) {
   cout << "mean tracking time: " << totaltime / nImages << endl;
 
   // Save camera trajectory
-  SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
+  SLAM->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
 
   return 0;
 }
diff --git a/Examples/Monocular/mono_tum.cc b/Examples/Monocular/mono_tum.cc
index 7cfcb26f319..012898a453b 100644
--- a/Examples/Monocular/mono_tum.cc
+++ b/Examples/Monocular/mono_tum.cc
@@ -51,8 +51,17 @@ int main(int argc, char **argv) {
 
   // 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, true);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::MONOCULAR, true);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   // Vector for tracking time statistics
   vector<float> vTimesTrack;
@@ -82,49 +91,30 @@ int main(int argc, char **argv) {
 
     if (imageScale != 1.f) {
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
       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();
-#endif
 #endif
       int width = im.cols * imageScale;
       int height = im.rows * imageScale;
       cv::resize(im, im, cv::Size(width, height));
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
       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();
-#endif
+
       t_resize = std::chrono::duration_cast<
                      std::chrono::duration<double, std::milli> >(t_End_Resize -
                                                                  t_Start_Resize)
                      .count();
-      SLAM.InsertResizeTime(t_resize);
+      SLAM->InsertResizeTime(t_resize);
 #endif
     }
 
-#ifdef COMPILEDWITHC14
     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();
-#endif
 
     // Pass the image to the SLAM system
-    SLAM.TrackMonocular(im, tframe);
+    SLAM->TrackMonocular(im, tframe);
 
-#ifdef COMPILEDWITHC14
     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();
-#endif
 
 #ifdef REGISTER_TIMES
     t_track =
@@ -132,7 +122,7 @@ int main(int argc, char **argv) {
         std::chrono::duration_cast<std::chrono::duration<double, std::milli> >(
             t2 - t1)
             .count();
-    SLAM.InsertTrackTime(t_track);
+    SLAM->InsertTrackTime(t_track);
 #endif
 
     double ttrack =
@@ -152,7 +142,7 @@ int main(int argc, char **argv) {
   }
 
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Tracking time statistics
   sort(vTimesTrack.begin(), vTimesTrack.end());
@@ -165,7 +155,7 @@ int main(int argc, char **argv) {
   cout << "mean tracking time: " << totaltime / nImages << endl;
 
   // Save camera trajectory
-  SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
+  SLAM->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
 
   return 0;
 }
diff --git a/Examples/Monocular/mono_tum_vi.cc b/Examples/Monocular/mono_tum_vi.cc
index cd02f268999..c5172547850 100644
--- a/Examples/Monocular/mono_tum_vi.cc
+++ b/Examples/Monocular/mono_tum_vi.cc
@@ -93,9 +93,17 @@ int main(int argc, char **argv) {
 
   // 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,
-                         0, file_name);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::MONOCULAR, false, file_name);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   double t_resize = 0.f;
   double t_track = 0.f;
@@ -113,30 +121,21 @@ int main(int argc, char **argv) {
 
       if (imageScale != 1.f) {
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
 #endif
         int width = im.cols * imageScale;
         int height = im.rows * imageScale;
         cv::resize(im, im, cv::Size(width, height));
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
+
         t_resize = std::chrono::duration_cast<
                        std::chrono::duration<double, std::milli> >(
                        t_End_Resize - t_Start_Resize)
                        .count();
-        SLAM.InsertResizeTime(t_resize);
+        SLAM->InsertResizeTime(t_resize);
 #endif
       }
 
@@ -152,31 +151,21 @@ int main(int argc, char **argv) {
              << endl;
         return 1;
       }
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
       // Pass the image to the SLAM system
-      SLAM.TrackMonocular(im, tframe);  // TODO change to monocular_inertial
+      SLAM->TrackMonocular(im, tframe);  // TODO change to monocular_inertial
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
 #ifdef REGISTER_TIMES
       t_track =
           t_resize + std::chrono::duration_cast<
                          std::chrono::duration<double, std::milli> >(t2 - t1)
                          .count();
-      SLAM.InsertTrackTime(t_track);
+      SLAM->InsertTrackTime(t_track);
 #endif
 
       double ttrack =
@@ -198,13 +187,13 @@ int main(int argc, char **argv) {
     if (seq < num_seq - 1) {
       cout << "Changing the dataset" << endl;
 
-      SLAM.ChangeDataset();
+      SLAM->ChangeDataset();
     }
   }
 
   // cout << "ttrack_tot = " << ttrack_tot << std::endl;
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Tracking time statistics
 
@@ -213,11 +202,11 @@ int main(int argc, char **argv) {
   if (bFileName) {
     const string kf_file = "kf_" + string(argv[argc - 1]) + ".txt";
     const string f_file = "f_" + string(argv[argc - 1]) + ".txt";
-    SLAM.SaveTrajectoryEuRoC(f_file);
-    SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+    SLAM->SaveTrajectoryEuRoC(f_file);
+    SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
   } else {
-    SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
-    SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+    SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
+    SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
   }
 
   sort(vTimesTrack.begin(), vTimesTrack.end());
diff --git a/Examples/REAMDME.md b/Examples/README.md
similarity index 100%
rename from Examples/REAMDME.md
rename to Examples/README.md
diff --git a/Examples/RGB-D-Inertial/RealSense_D435i.yaml b/Examples/RGB-D-Inertial/RealSense_D435i.yaml
deleted file mode 100755
index cfc1ffc6569..00000000000
--- a/Examples/RGB-D-Inertial/RealSense_D435i.yaml
+++ /dev/null
@@ -1,90 +0,0 @@
-%YAML:1.0
-
-#--------------------------------------------------------------------------------------------
-# Camera Parameters. Adjust them!
-#--------------------------------------------------------------------------------------------
-File.version: "1.0"
-
-Camera.type: "PinHole"
-
-# Right Camera calibration and distortion parameters (OpenCV)
-Camera1.fx: 617.201
-Camera1.fy: 617.362
-Camera1.cx: 324.637
-Camera1.cy: 242.462
-
-# distortion parameters
-Camera1.k1: 0.0
-Camera1.k2: 0.0
-Camera1.p1: 0.0
-Camera1.p2: 0.0
-
-# Camera resolution
-Camera.width: 640
-Camera.height: 480
-
-# Camera frames per second
-Camera.fps: 30
-
-# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
-Camera.RGB: 1
-
-Stereo.ThDepth: 40.0
-Stereo.b: 0.0745
-
-# Depth map values factor
-RGBD.DepthMapFactor: 1000.0
-
-# Transformation from body-frame (imu) to left camera
-IMU.T_b_c1: !!opencv-matrix
-   rows: 4
-   cols: 4
-   dt: f
-   data: [0.999903, -0.0138036, -0.00208099, -0.0202141,
-         0.0137985, 0.999902, -0.00243498, 0.00505961,
-         0.0021144, 0.00240603, 0.999995, 0.0114047,
-         0.0, 0.0, 0.0, 1.0]
-
-
-# Do not insert KFs when recently lost
-IMU.InsertKFsWhenLost: 0
-
-# IMU noise (Use those from VINS-mono)
-IMU.NoiseGyro: 1e-2 # 3 # 2.44e-4 #1e-3 # rad/s^0.5
-IMU.NoiseAcc: 1e-1 #2 # 1.47e-3 #1e-2 # m/s^1.5
-IMU.GyroWalk: 1e-6 # rad/s^1.5
-IMU.AccWalk: 1e-4 # m/s^2.5
-IMU.Frequency: 200.0
-
-#--------------------------------------------------------------------------------------------
-# ORB Parameters
-#--------------------------------------------------------------------------------------------
-# ORB Extractor: Number of features per image
-ORBextractor.nFeatures: 1250
-
-# ORB Extractor: Scale factor between levels in the scale pyramid
-ORBextractor.scaleFactor: 1.2
-
-# ORB Extractor: Number of levels in the scale pyramid
-ORBextractor.nLevels: 8
-
-# ORB Extractor: Fast threshold
-# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
-# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
-# You can lower these values if your images have low contrast
-ORBextractor.iniThFAST: 20
-ORBextractor.minThFAST: 7
-
-#--------------------------------------------------------------------------------------------
-# Viewer Parameters
-#--------------------------------------------------------------------------------------------
-Viewer.KeyFrameSize: 0.05
-Viewer.KeyFrameLineWidth: 1.0
-Viewer.GraphLineWidth: 0.9
-Viewer.PointSize: 2.0
-Viewer.CameraSize: 0.08
-Viewer.CameraLineWidth: 3.0
-Viewer.ViewpointX: 0.0
-Viewer.ViewpointY: -0.7
-Viewer.ViewpointZ: -3.5
-Viewer.ViewpointF: 500.0
diff --git a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc b/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
deleted file mode 100644
index 96e2fef48b7..00000000000
--- a/Examples/RGB-D-Inertial/rgbd_inertial_realsense_D435i.cc
+++ /dev/null
@@ -1,556 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
- * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
- * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
- * University of Zaragoza.
- *
- * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
- * the terms of the GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option) any later
- * version.
- *
- * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
- * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <System.h>
-#include <signal.h>
-#include <stdlib.h>
-
-#include <algorithm>
-#include <chrono>
-#include <condition_variable>
-#include <ctime>
-#include <fstream>
-#include <iostream>
-#include <librealsense2/rs.hpp>
-#include <opencv2/core/core.hpp>
-#include <sstream>
-
-#include "librealsense2/rsutil.h"
-
-using namespace std;
-
-bool b_continue_session;
-
-void exit_loop_handler(int s) {
-  cout << "Finishing session" << endl;
-  b_continue_session = false;
-}
-
-rs2_stream find_stream_to_align(
-    const std::vector<rs2::stream_profile>& streams);
-bool profile_changed(const std::vector<rs2::stream_profile>& current,
-                     const std::vector<rs2::stream_profile>& prev);
-
-void interpolateData(const std::vector<double>& vBase_times,
-                     std::vector<rs2_vector>& vInterp_data,
-                     std::vector<double>& vInterp_times,
-                     const rs2_vector& prev_data, const double& prev_time);
-
-rs2_vector interpolateMeasure(const double target_time,
-                              const rs2_vector current_data,
-                              const double current_time,
-                              const rs2_vector prev_data,
-                              const double prev_time);
-
-static rs2_option get_sensor_option(const rs2::sensor& sensor) {
-  // Sensors usually have several options to control their properties
-  //  such as Exposure, Brightness etc.
-
-  std::cout << "Sensor supports the following options:\n" << std::endl;
-
-  // The following loop shows how to iterate over all available options
-  // Starting from 0 until RS2_OPTION_COUNT (exclusive)
-  for (int i = 0; i < static_cast<int>(RS2_OPTION_COUNT); i++) {
-    rs2_option option_type = static_cast<rs2_option>(i);
-    // SDK enum types can be streamed to get a string that represents them
-    std::cout << "  " << i << ": " << option_type;
-
-    // To control an option, use the following api:
-
-    // First, verify that the sensor actually supports this option
-    if (sensor.supports(option_type)) {
-      std::cout << std::endl;
-
-      // Get a human readable description of the option
-      const char* description = sensor.get_option_description(option_type);
-      std::cout << "       Description   : " << description << std::endl;
-
-      // Get the current value of the option
-      float current_value = sensor.get_option(option_type);
-      std::cout << "       Current Value : " << current_value << std::endl;
-
-      // To change the value of an option, please follow the
-      // change_sensor_option() function
-    } else {
-      std::cout << " is not supported" << std::endl;
-    }
-  }
-
-  uint32_t selected_sensor_option = 0;
-  return static_cast<rs2_option>(selected_sensor_option);
-}
-
-int main(int argc, char** argv) {
-  if (argc < 3 || argc > 4) {
-    cerr << endl
-         << "Usage: ./mono_inertial_realsense_D435i path_to_vocabulary "
-            "path_to_settings (trajectory_file_name)"
-         << endl;
-    return 1;
-  }
-
-  string file_name;
-
-  if (argc == 4) {
-    file_name = string(argv[argc - 1]);
-  }
-
-  struct sigaction sigIntHandler;
-
-  sigIntHandler.sa_handler = exit_loop_handler;
-  sigemptyset(&sigIntHandler.sa_mask);
-  sigIntHandler.sa_flags = 0;
-
-  sigaction(SIGINT, &sigIntHandler, NULL);
-  b_continue_session = true;
-
-  double offset = 0;  // ms
-
-  rs2::context ctx;
-  rs2::device_list devices = ctx.query_devices();
-  rs2::device selected_device;
-  if (devices.size() == 0) {
-    std::cerr << "No device connected, please connect a RealSense device"
-              << std::endl;
-    return 0;
-  } else
-    selected_device = devices[0];
-
-  std::vector<rs2::sensor> sensors = selected_device.query_sensors();
-  int index = 0;
-  // We can now iterate the sensors and print their names
-  for (rs2::sensor sensor : sensors)
-    if (sensor.supports(RS2_CAMERA_INFO_NAME)) {
-      ++index;
-      if (index == 1) {
-        sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1);
-        // sensor.set_option(RS2_OPTION_AUTO_EXPOSURE_LIMIT,50000);
-        sensor.set_option(RS2_OPTION_EMITTER_ENABLED,
-                          1);  // emitter on for depth information
-      }
-      // std::cout << "  " << index << " : " <<
-      // sensor.get_info(RS2_CAMERA_INFO_NAME) << std::endl;
-      get_sensor_option(sensor);
-      if (index == 2) {
-        // RGB camera
-        sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1);
-
-        // sensor.set_option(RS2_OPTION_EXPOSURE,80.f);
-      }
-
-      if (index == 3) {
-        sensor.set_option(RS2_OPTION_ENABLE_MOTION_CORRECTION, 0);
-      }
-    }
-
-  // Declare RealSense pipeline, encapsulating the actual device and sensors
-  rs2::pipeline pipe;
-
-  // Create a configuration for configuring the pipeline with a non default
-  // profile
-  rs2::config cfg;
-
-  // RGB stream
-  cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGB8, 30);
-
-  // Depth stream
-  // cfg.enable_stream(RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 30);
-  cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
-
-  // IMU stream
-  cfg.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F);
-  cfg.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);
-
-  // IMU callback
-  std::mutex imu_mutex;
-  std::condition_variable cond_image_rec;
-
-  vector<double> v_accel_timestamp;
-  vector<rs2_vector> v_accel_data;
-  vector<double> v_gyro_timestamp;
-  vector<rs2_vector> v_gyro_data;
-
-  double prev_accel_timestamp = 0;
-  rs2_vector prev_accel_data;
-  double current_accel_timestamp = 0;
-  rs2_vector current_accel_data;
-  vector<double> v_accel_timestamp_sync;
-  vector<rs2_vector> v_accel_data_sync;
-
-  cv::Mat imCV, depthCV;
-  int width_img, height_img;
-  double timestamp_image = -1.0;
-  bool image_ready = false;
-  int count_im_buffer = 0;  // count dropped frames
-
-  // start and stop just to get necessary profile
-  rs2::pipeline_profile pipe_profile = pipe.start(cfg);
-  pipe.stop();
-
-  // Align depth and RGB frames
-  // Pipeline could choose a device that does not have a color stream
-  // If there is no color stream, choose to align depth to another stream
-  rs2_stream align_to = find_stream_to_align(pipe_profile.get_streams());
-
-  // Create a rs2::align object.
-  // rs2::align allows us to perform alignment of depth frames to others frames
-  // The "align_to" is the stream type to which we plan to align depth frames.
-  rs2::align align(align_to);
-  rs2::frameset fsSLAM;
-
-  auto imu_callback = [&](const rs2::frame& frame) {
-    std::unique_lock<std::mutex> lock(imu_mutex);
-
-    if (rs2::frameset fs = frame.as<rs2::frameset>()) {
-      count_im_buffer++;
-
-      double new_timestamp_image = fs.get_timestamp() * 1e-3;
-      if (abs(timestamp_image - new_timestamp_image) < 0.001) {
-        count_im_buffer--;
-        return;
-      }
-
-      if (profile_changed(pipe.get_active_profile().get_streams(),
-                          pipe_profile.get_streams())) {
-        // If the profile was changed, update the align object, and also get the
-        // new device's depth scale
-        pipe_profile = pipe.get_active_profile();
-        align_to = find_stream_to_align(pipe_profile.get_streams());
-        align = rs2::align(align_to);
-      }
-
-      // Align depth and rgb takes long time, move it out of the interruption to
-      // avoid losing IMU measurements
-      fsSLAM = fs;
-
-      timestamp_image = fs.get_timestamp() * 1e-3;
-      image_ready = true;
-
-      while (v_gyro_timestamp.size() > v_accel_timestamp_sync.size()) {
-        int index = v_accel_timestamp_sync.size();
-        double target_time = v_gyro_timestamp[index];
-
-        v_accel_data_sync.push_back(current_accel_data);
-        v_accel_timestamp_sync.push_back(target_time);
-      }
-
-      lock.unlock();
-      cond_image_rec.notify_all();
-    } else if (rs2::motion_frame m_frame = frame.as<rs2::motion_frame>()) {
-      if (m_frame.get_profile().stream_name() == "Gyro") {
-        // It runs at 200Hz
-        v_gyro_data.push_back(m_frame.get_motion_data());
-        v_gyro_timestamp.push_back((m_frame.get_timestamp() + offset) * 1e-3);
-      } else if (m_frame.get_profile().stream_name() == "Accel") {
-        // It runs at 60Hz
-        prev_accel_timestamp = current_accel_timestamp;
-        prev_accel_data = current_accel_data;
-
-        current_accel_data = m_frame.get_motion_data();
-        current_accel_timestamp = (m_frame.get_timestamp() + offset) * 1e-3;
-
-        while (v_gyro_timestamp.size() > v_accel_timestamp_sync.size()) {
-          int index = v_accel_timestamp_sync.size();
-          double target_time = v_gyro_timestamp[index];
-
-          rs2_vector interp_data = interpolateMeasure(
-              target_time, current_accel_data, current_accel_timestamp,
-              prev_accel_data, prev_accel_timestamp);
-
-          v_accel_data_sync.push_back(interp_data);
-          v_accel_timestamp_sync.push_back(target_time);
-        }
-      }
-    }
-  };
-
-  pipe_profile = pipe.start(cfg, imu_callback);
-
-  vector<ORB_SLAM3::IMU::Point> vImuMeas;
-  rs2::stream_profile cam_stream = pipe_profile.get_stream(RS2_STREAM_COLOR);
-
-  rs2::stream_profile imu_stream = pipe_profile.get_stream(RS2_STREAM_GYRO);
-  float* Rbc = cam_stream.get_extrinsics_to(imu_stream).rotation;
-  float* tbc = cam_stream.get_extrinsics_to(imu_stream).translation;
-  std::cout << "Tbc = " << std::endl;
-  for (int i = 0; i < 3; i++) {
-    for (int j = 0; j < 3; j++) std::cout << Rbc[i * 3 + j] << ", ";
-    std::cout << tbc[i] << "\n";
-  }
-
-  rs2_intrinsics intrinsics_cam =
-      cam_stream.as<rs2::video_stream_profile>().get_intrinsics();
-  width_img = intrinsics_cam.width;
-  height_img = intrinsics_cam.height;
-  std::cout << " fx = " << intrinsics_cam.fx << std::endl;
-  std::cout << " fy = " << intrinsics_cam.fy << std::endl;
-  std::cout << " cx = " << intrinsics_cam.ppx << std::endl;
-  std::cout << " cy = " << intrinsics_cam.ppy << std::endl;
-  std::cout << " height = " << intrinsics_cam.height << std::endl;
-  std::cout << " width = " << intrinsics_cam.width << std::endl;
-  std::cout << " Coeff = " << intrinsics_cam.coeffs[0] << ", "
-            << intrinsics_cam.coeffs[1] << ", " << intrinsics_cam.coeffs[2]
-            << ", " << intrinsics_cam.coeffs[3] << ", "
-            << intrinsics_cam.coeffs[4] << ", " << std::endl;
-  std::cout << " Model = " << intrinsics_cam.model << std::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_RGBD, true, 0,
-                         file_name);
-  float imageScale = SLAM.GetImageScale();
-
-  double timestamp;
-  cv::Mat im, depth;
-
-  // Clear IMU vectors
-  v_gyro_data.clear();
-  v_gyro_timestamp.clear();
-  v_accel_data_sync.clear();
-  v_accel_timestamp_sync.clear();
-
-  double t_resize = 0.f;
-  double t_track = 0.f;
-
-  while (!SLAM.isShutDown()) {
-    std::vector<rs2_vector> vGyro;
-    std::vector<double> vGyro_times;
-    std::vector<rs2_vector> vAccel;
-    std::vector<double> vAccel_times;
-    rs2::frameset fs;
-    {
-      std::unique_lock<std::mutex> lk(imu_mutex);
-      if (!image_ready) cond_image_rec.wait(lk);
-
-#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();
-#endif
-
-      fs = fsSLAM;
-
-      if (count_im_buffer > 1) cout << count_im_buffer - 1 << " dropped frs\n";
-      count_im_buffer = 0;
-
-      while (v_gyro_timestamp.size() > v_accel_timestamp_sync.size()) {
-        int index = v_accel_timestamp_sync.size();
-        double target_time = v_gyro_timestamp[index];
-
-        rs2_vector interp_data = interpolateMeasure(
-            target_time, current_accel_data, current_accel_timestamp,
-            prev_accel_data, prev_accel_timestamp);
-
-        v_accel_data_sync.push_back(interp_data);
-        v_accel_timestamp_sync.push_back(target_time);
-      }
-
-      // Copy the IMU data
-      vGyro = v_gyro_data;
-      vGyro_times = v_gyro_timestamp;
-      vAccel = v_accel_data_sync;
-      vAccel_times = v_accel_timestamp_sync;
-
-      // Image
-      timestamp = timestamp_image;
-
-      // Clear IMU vectors
-      v_gyro_data.clear();
-      v_gyro_timestamp.clear();
-      v_accel_data_sync.clear();
-      v_accel_timestamp_sync.clear();
-
-      image_ready = false;
-    }
-
-    // Perform alignment here
-    auto processed = align.process(fs);
-
-    // Trying to get both other and aligned depth frames
-    rs2::video_frame color_frame = processed.first(align_to);
-    rs2::depth_frame depth_frame = processed.get_depth_frame();
-
-    im = cv::Mat(cv::Size(width_img, height_img), CV_8UC3,
-                 (void*)(color_frame.get_data()), cv::Mat::AUTO_STEP);
-    depth = cv::Mat(cv::Size(width_img, height_img), CV_16U,
-                    (void*)(depth_frame.get_data()), cv::Mat::AUTO_STEP);
-
-    /*cv::Mat depthCV_8U;
-    depthCV.convertTo(depthCV_8U,CV_8U,0.01);
-    cv::imshow("depth image", depthCV_8U);*/
-
-    for (int i = 0; i < vGyro.size(); ++i) {
-      ORB_SLAM3::IMU::Point lastPoint(vAccel[i].x, vAccel[i].y, vAccel[i].z,
-                                      vGyro[i].x, vGyro[i].y, vGyro[i].z,
-                                      vGyro_times[i]);
-      vImuMeas.push_back(lastPoint);
-    }
-
-    if (imageScale != 1.f) {
-#ifdef REGISTER_TIMES
-#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();
-#endif
-#endif
-      int width = im.cols * imageScale;
-      int height = im.rows * imageScale;
-      cv::resize(im, im, cv::Size(width, height));
-      cv::resize(depth, depth, cv::Size(width, height));
-
-#ifdef REGISTER_TIMES
-#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();
-#endif
-      t_resize = std::chrono::duration_cast<
-                     std::chrono::duration<double, std::milli> >(t_End_Resize -
-                                                                 t_Start_Resize)
-                     .count();
-      SLAM.InsertResizeTime(t_resize);
-#endif
-    }
-
-#ifdef REGISTER_TIMES
-#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();
-#endif
-#endif
-    // Pass the image to the SLAM system
-    SLAM.TrackRGBD(im, depth, timestamp, vImuMeas);
-
-#ifdef REGISTER_TIMES
-#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();
-#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);
-#endif
-
-    // Clear the previous IMU measurements to load the new ones
-    vImuMeas.clear();
-  }
-  cout << "System shutdown!\n";
-}
-
-rs2_stream find_stream_to_align(
-    const std::vector<rs2::stream_profile>& streams) {
-  // Given a vector of streams, we try to find a depth stream and another stream
-  // to align depth with. We prioritize color streams to make the view look
-  // better. If color is not available, we take another stream that (other than
-  // depth)
-  rs2_stream align_to = RS2_STREAM_ANY;
-  bool depth_stream_found = false;
-  bool color_stream_found = false;
-  for (rs2::stream_profile sp : streams) {
-    rs2_stream profile_stream = sp.stream_type();
-    if (profile_stream != RS2_STREAM_DEPTH) {
-      if (!color_stream_found)  // Prefer color
-        align_to = profile_stream;
-
-      if (profile_stream == RS2_STREAM_COLOR) {
-        color_stream_found = true;
-      }
-    } else {
-      depth_stream_found = true;
-    }
-  }
-
-  if (!depth_stream_found)
-    throw std::runtime_error("No Depth stream available");
-
-  if (align_to == RS2_STREAM_ANY)
-    throw std::runtime_error("No stream found to align with Depth");
-
-  return align_to;
-}
-
-bool profile_changed(const std::vector<rs2::stream_profile>& current,
-                     const std::vector<rs2::stream_profile>& prev) {
-  for (auto&& sp : prev) {
-    // If previous profile is in current (maybe just added another)
-    auto itr = std::find_if(std::begin(current), std::end(current),
-                            [&sp](const rs2::stream_profile& current_sp) {
-                              return sp.unique_id() == current_sp.unique_id();
-                            });
-    if (itr ==
-        std::end(current))  // If it previous stream wasn't found in current
-    {
-      return true;
-    }
-  }
-  return false;
-}
-
-rs2_vector interpolateMeasure(const double target_time,
-                              const rs2_vector current_data,
-                              const double current_time,
-                              const rs2_vector prev_data,
-                              const double prev_time) {
-  // If there are not previous information, the current data is propagated
-  if (prev_time == 0) {
-    return current_data;
-  }
-
-  rs2_vector increment;
-  rs2_vector value_interp;
-
-  if (target_time > current_time) {
-    value_interp = current_data;
-  } else if (target_time > prev_time) {
-    increment.x = current_data.x - prev_data.x;
-    increment.y = current_data.y - prev_data.y;
-    increment.z = current_data.z - prev_data.z;
-
-    double factor = (target_time - prev_time) / (current_time - prev_time);
-
-    value_interp.x = prev_data.x + increment.x * factor;
-    value_interp.y = prev_data.y + increment.y * factor;
-    value_interp.z = prev_data.z + increment.z * factor;
-
-    // zero interpolation
-    value_interp = current_data;
-  } else {
-    value_interp = prev_data;
-  }
-
-  return value_interp;
-}
diff --git a/Examples/RGB-D/rgbd_tum.cc b/Examples/RGB-D/rgbd_tum.cc
index 1935c9e120d..978088062d0 100644
--- a/Examples/RGB-D/rgbd_tum.cc
+++ b/Examples/RGB-D/rgbd_tum.cc
@@ -63,8 +63,17 @@ int main(int argc, char **argv) {
 
   // 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::RGBD, true);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::RGBD, true);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   // Vector for tracking time statistics
   vector<float> vTimesTrack;
@@ -98,22 +107,12 @@ int main(int argc, char **argv) {
       cv::resize(imD, imD, cv::Size(width, height));
     }
 
-#ifdef COMPILEDWITHC14
     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();
-#endif
 
     // Pass the image to the SLAM system
-    SLAM.TrackRGBD(imRGB, imD, tframe);
+    SLAM->TrackRGBD(imRGB, imD, tframe);
 
-#ifdef COMPILEDWITHC14
     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();
-#endif
 
     double ttrack =
         std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1)
@@ -132,7 +131,7 @@ int main(int argc, char **argv) {
   }
 
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Tracking time statistics
   sort(vTimesTrack.begin(), vTimesTrack.end());
@@ -145,8 +144,8 @@ int main(int argc, char **argv) {
   cout << "mean tracking time: " << totaltime / nImages << endl;
 
   // Save camera trajectory
-  SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
-  SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
+  SLAM->SaveTrajectoryTUM("CameraTrajectory.txt");
+  SLAM->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
 
   return 0;
 }
diff --git a/Examples/ROS/ORB_SLAM3/Asus.yaml b/Examples/ROS/ORB_SLAM3/Asus.yaml
deleted file mode 100644
index 164261a42d7..00000000000
--- a/Examples/ROS/ORB_SLAM3/Asus.yaml
+++ /dev/null
@@ -1,69 +0,0 @@
-%YAML:1.0
-
-#--------------------------------------------------------------------------------------------
-# Camera Parameters. Adjust them!
-#--------------------------------------------------------------------------------------------
-Camera.type: "PinHole"
-
-# Camera calibration and distortion parameters (OpenCV)
-Camera.fx: 535.4
-Camera.fy: 539.2
-Camera.cx: 320.1
-Camera.cy: 247.6
-
-Camera.k1: 0.0
-Camera.k2: 0.0
-Camera.p1: 0.0
-Camera.p2: 0.0
-
-Camera.width: 640
-Camera.height: 480
-
-# Camera frames per second
-Camera.fps: 30.0
-
-# IR projector baseline times fx (aprox.)
-Camera.bf: 40.0
-
-# 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
-
-# Deptmap values factor
-DepthMapFactor: 1.0
-
-#--------------------------------------------------------------------------------------------
-# ORB Parameters
-#--------------------------------------------------------------------------------------------
-
-# ORB Extractor: Number of features per image
-ORBextractor.nFeatures: 1000
-
-# ORB Extractor: Scale factor between levels in the scale pyramid
-ORBextractor.scaleFactor: 1.2
-
-# ORB Extractor: Number of levels in the scale pyramid
-ORBextractor.nLevels: 8
-
-# ORB Extractor: Fast threshold
-# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
-# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
-# You can lower these values if your images have low contrast
-ORBextractor.iniThFAST: 20
-ORBextractor.minThFAST: 7
-
-#--------------------------------------------------------------------------------------------
-# Viewer Parameters
-#--------------------------------------------------------------------------------------------
-Viewer.KeyFrameSize: 0.05
-Viewer.KeyFrameLineWidth: 1
-Viewer.GraphLineWidth: 0.9
-Viewer.PointSize:2
-Viewer.CameraSize: 0.08
-Viewer.CameraLineWidth: 3
-Viewer.ViewpointX: 0
-Viewer.ViewpointY: -0.7
-Viewer.ViewpointZ: -1.8
-Viewer.ViewpointF: 500
diff --git a/Examples/ROS/ORB_SLAM3/CMakeLists.txt b/Examples/ROS/ORB_SLAM3/CMakeLists.txt
deleted file mode 100644
index f0d16eccc6d..00000000000
--- a/Examples/ROS/ORB_SLAM3/CMakeLists.txt
+++ /dev/null
@@ -1,118 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-rosbuild_init()
-
-IF(NOT ROS_BUILD_TYPE)
-  SET(ROS_BUILD_TYPE Release)
-ENDIF()
-
-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
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX14)
-   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
-   add_definitions(-DCOMPILEDWITHC14)
-   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.")
-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()
-
-MESSAGE("OPENCV VERSION:")
-MESSAGE(${OpenCV_VERSION})
-
-find_package(Eigen3 3.1.0 REQUIRED)
-find_package(Pangolin REQUIRED)
-
-include_directories(
-${PROJECT_SOURCE_DIR}
-${PROJECT_SOURCE_DIR}/../../../
-${PROJECT_SOURCE_DIR}/../../../include
-${PROJECT_SOURCE_DIR}/../../../include/CameraModels
-${PROJECT_SOURCE_DIR}/../../../Thirdparty/Sophus
-${Pangolin_INCLUDE_DIRS}
-)
-
-set(LIBS
-${OpenCV_LIBS}
-${EIGEN3_LIBS}
-${Pangolin_LIBRARIES}
-${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
-${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
-${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM3.so
--lboost_system
-)
-
-# Node for monocular camera
-rosbuild_add_executable(Mono
-src/ros_mono.cc
-)
-
-target_link_libraries(Mono
-${LIBS}
-)
-
-# Node for monocular camera (Augmented Reality Demo)
-rosbuild_add_executable(MonoAR
-src/AR/ros_mono_ar.cc
-src/AR/ViewerAR.h
-src/AR/ViewerAR.cc
-)
-
-target_link_libraries(MonoAR
-${LIBS}
-)
-
-# Node for stereo camera
-rosbuild_add_executable(Stereo
-src/ros_stereo.cc
-)
-
-target_link_libraries(Stereo
-${LIBS}
-)
-
-# Node for RGB-D camera
-rosbuild_add_executable(RGBD
-src/ros_rgbd.cc
-)
-
-target_link_libraries(RGBD
-${LIBS}
-)
-
-# Node for monocular-inertial camera
-rosbuild_add_executable(Mono_Inertial
-src/ros_mono_inertial.cc
-)
-
-target_link_libraries(Mono_Inertial
-${LIBS}
-)
-
-# Node for stereo-inertial camera
-rosbuild_add_executable(Stereo_Inertial
-src/ros_stereo_inertial.cc
-)
-
-target_link_libraries(Stereo_Inertial
-${LIBS}
-)
diff --git a/Examples/ROS/ORB_SLAM3/Tello.yaml b/Examples/ROS/ORB_SLAM3/Tello.yaml
deleted file mode 100644
index ca762a8e1ff..00000000000
--- a/Examples/ROS/ORB_SLAM3/Tello.yaml
+++ /dev/null
@@ -1,101 +0,0 @@
-%YAML:1.0
-
-# Camera calibration and distortion parameters (OpenCV)
-Camera.type: "PinHole"
-
-Camera.fx: 924.873180
-Camera.fy: 923.504522
-Camera.cx: 486.997346
-Camera.cy: 364.308527
-
-#   data: [ 924.873180, 0.000000  , 486.997346,
-#           0.000000  , 923.504522, 364.308527,
-#           0.000000  , 0.000000  , 1.000000  ]
-
-          # [ fx        0           cx]
-          # [ 0         fy          cy]
-          # [ 0         0           1 ]
-
-
-Camera.k1: -0.034749
-Camera.k2: 0.071514
-Camera.p1: 0.000363
-Camera.p2: 0.003131
-Camera.k3: 0.0
-
-# data: [-0.034749, 0.071514, 0.000363, 0.003131, 0.000000]
-
-
-Camera.width: 960
-Camera.height: 720
-
-# Camera frames per second
-# Camera.fps: 60.0
-Camera.fps: 30.0
-
-# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
-Camera.RGB: 1
-
-#--------------------------------------------------------------------------------------------
-# ORB Parameters
-#--------------------------------------------------------------------------------------------
-
-# ORB Extractor: Number of features per image
-ORBextractor.nFeatures: 10000
-
-# ORB Extractor: Scale factor between levels in the scale pyramid
-ORBextractor.scaleFactor: 1.15
-
-# ORB Extractor: Number of levels in the scale pyramid
-ORBextractor.nLevels: 10
-
-# ORB Extractor: Fast threshold
-# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
-# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
-# You can lower these values if your images have low contrast
-ORBextractor.iniThFAST: 20
-ORBextractor.minThFAST: 14
-
-
-# image_width: 960
-#hanged the resolution of the cameras so as to have the biggest resolution they can. The results seem similar for the camera matrix, but the distorti
-#image_height: 720
-# camera_name: narrow_stereo
-# camera_matrix:
-#   rows: 3
-#   cols: 3
-#   data: [ 924.873180, 0.000000  , 486.997346,
-#           0.000000  , 923.504522, 364.308527,
-#           0.000000  , 0.000000  , 1.000000  ]
-
-          # [ fx        0           cx]
-          # [ 0         fy          cy]
-          # [ 0         0           1 ]
-# distortion_model: plumb_bob
-# distortion_coefficients:
-#   rows: 1
-#   cols: 5
-#   data: [-0.034749, 0.071514, 0.000363, 0.003131, 0.000000]
-# rectification_matrix:
-#   rows: 3
-#   cols: 3
-#   data: [ 1.000000, 0.000000, 0.000000,
-#           0.000000, 1.000000, 0.000000,
-#           0.000000, 0.000000, 1.000000]
-# projection_matrix:
-#   rows: 3
-#   cols: 4
-#   data: [   921.967102, 0.000000  , 489.492281, 0.000000,
-#             0.000000  , 921.018890, 364.508536, 0.000000,
-#             0.000000  , 0.000000  , 1.000000  , 0.000000]
-
-Viewer.KeyFrameSize: 0.05
-Viewer.KeyFrameLineWidth: 1
-Viewer.GraphLineWidth: 0.9
-Viewer.PointSize:2
-Viewer.CameraSize: 0.08
-Viewer.CameraLineWidth: 3
-Viewer.ViewpointX: 0
-Viewer.ViewpointY: -0.7
-Viewer.ViewpointZ: -1.8
-Viewer.ViewpointF: 500
diff --git a/Examples/ROS/ORB_SLAM3/manifest.xml b/Examples/ROS/ORB_SLAM3/manifest.xml
deleted file mode 100644
index 6f424a4939d..00000000000
--- a/Examples/ROS/ORB_SLAM3/manifest.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-<package>
-  <description brief="ORB_SLAM3">
-     ORB_SLAM3
-  </description>
-  <author>Carlos Campos, Richard Elvira, Juan J. Gomez, Jose M.M. Montiel, Juan D. Tardos</author>
-  <license>GPLv3</license>
-  <depend package="roscpp"/>
-  <depend package="tf"/>
-  <depend package="sensor_msgs"/>
-  <depend package="image_transport"/>
-  <depend package="cv_bridge"/>
-</package>
diff --git a/Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.cc b/Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.cc
deleted file mode 100644
index a18dda3a186..00000000000
--- a/Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.cc
+++ /dev/null
@@ -1,629 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
- * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
- * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
- * University of Zaragoza.
- *
- * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
- * the terms of the GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option) any later
- * version.
- *
- * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
- * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include "ViewerAR.h"
-
-#include <Eigen/Dense>
-#include <cstdlib>
-#include <mutex>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/core.hpp>
-#include <opencv2/core/eigen.hpp>
-#include <opencv2/core/mat.inl.hpp>
-#include <opencv2/core/opengl.hpp>
-#include <opencv2/features2d.hpp>
-#include <opencv2/highgui.hpp>
-#include <opencv2/highgui/highgui.hpp>
-#include <opencv2/imgproc.hpp>
-#include <thread>
-
-using namespace std;
-
-namespace ORB_SLAM3 {
-
-const float eps = 1e-4;
-
-cv::Mat ExpSO3(const float &x, const float &y, const float &z) {
-  cv::Mat I = cv::Mat::eye(3, 3, CV_32F);
-  const float d2 = x * x + y * y + z * z;
-  const float d = sqrt(d2);
-  cv::Mat W = (cv::Mat_<float>(3, 3) << 0, -z, y, z, 0, -x, -y, x, 0);
-  if (d < eps)
-    return (I + W + 0.5f * W * W);
-  else
-    return (I + W * sin(d) / d + W * W * (1.0f - cos(d)) / d2);
-}
-
-cv::Mat ExpSO3(const cv::Mat &v) {
-  return ExpSO3(v.at<float>(0), v.at<float>(1), v.at<float>(2));
-}
-
-ViewerAR::ViewerAR() {}
-
-void ViewerAR::Run() {
-  int w, h, wui;
-
-  cv::Mat im, Tcw;
-  int status;
-  vector<cv::KeyPoint> vKeys;
-  vector<MapPoint *> vMPs;
-
-  while (1) {
-    GetImagePose(im, Tcw, status, vKeys, vMPs);
-    if (im.empty())
-      cv::waitKey(mT);
-    else {
-      w = im.cols;
-      h = im.rows;
-      break;
-    }
-  }
-
-  wui = 200;
-
-  pangolin::CreateWindowAndBind("Viewer", w + wui, h);
-
-  glEnable(GL_DEPTH_TEST);
-  glEnable(GL_BLEND);
-
-  pangolin::CreatePanel("menu").SetBounds(0.0, 1.0, 0.0,
-                                          pangolin::Attach::Pix(wui));
-  pangolin::Var<bool> menu_detectplane("menu.Insert Cube", false, false);
-  pangolin::Var<bool> menu_clear("menu.Clear All", false, false);
-  pangolin::Var<bool> menu_drawim("menu.Draw Image", true, true);
-  pangolin::Var<bool> menu_drawcube("menu.Draw Cube", true, true);
-  pangolin::Var<float> menu_cubesize("menu. Cube Size", 0.05, 0.01, 0.3);
-  pangolin::Var<bool> menu_drawgrid("menu.Draw Grid", true, true);
-  pangolin::Var<int> menu_ngrid("menu. Grid Elements", 3, 1, 10);
-  pangolin::Var<float> menu_sizegrid("menu. Element Size", 0.05, 0.01, 0.3);
-  pangolin::Var<bool> menu_drawpoints("menu.Draw Points", false, true);
-
-  pangolin::Var<bool> menu_LocalizationMode("menu.Localization Mode", false,
-                                            true);
-  bool bLocalizationMode = false;
-
-  pangolin::View &d_image =
-      pangolin::Display("image")
-          .SetBounds(0, 1.0f, pangolin::Attach::Pix(wui), 1.0f, (float)w / h)
-          .SetLock(pangolin::LockLeft, pangolin::LockTop);
-
-  pangolin::GlTexture imageTexture(w, h, GL_RGB, false, 0, GL_RGB,
-                                   GL_UNSIGNED_BYTE);
-
-  pangolin::OpenGlMatrixSpec P =
-      pangolin::ProjectionMatrixRDF_TopLeft(w, h, fx, fy, cx, cy, 0.001, 1000);
-
-  vector<Plane *> vpPlane;
-
-  while (1) {
-    if (menu_LocalizationMode && !bLocalizationMode) {
-      mpSystem->ActivateLocalizationMode();
-      bLocalizationMode = true;
-    } else if (!menu_LocalizationMode && bLocalizationMode) {
-      mpSystem->DeactivateLocalizationMode();
-      bLocalizationMode = false;
-    }
-
-    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
-    // Activate camera view
-    d_image.Activate();
-    glColor3f(1.0, 1.0, 1.0);
-
-    // Get last image and its computed pose from SLAM
-    GetImagePose(im, Tcw, status, vKeys, vMPs);
-
-    // Add text to image
-    PrintStatus(status, bLocalizationMode, im);
-
-    if (menu_drawpoints) DrawTrackedPoints(vKeys, vMPs, im);
-
-    // Draw image
-    if (menu_drawim) DrawImageTexture(imageTexture, im);
-
-    glClear(GL_DEPTH_BUFFER_BIT);
-
-    // Load camera projection
-    glMatrixMode(GL_PROJECTION);
-    P.Load();
-
-    glMatrixMode(GL_MODELVIEW);
-
-    // Load camera pose
-    LoadCameraPose(Tcw);
-
-    // Draw virtual things
-    if (status == 2) {
-      if (menu_clear) {
-        if (!vpPlane.empty()) {
-          for (size_t i = 0; i < vpPlane.size(); i++) {
-            delete vpPlane[i];
-          }
-          vpPlane.clear();
-          cout << "All cubes erased!" << endl;
-        }
-        menu_clear = false;
-      }
-      if (menu_detectplane) {
-        Plane *pPlane = DetectPlane(Tcw, vMPs, 50);
-        if (pPlane) {
-          cout << "New virtual cube inserted!" << endl;
-          vpPlane.push_back(pPlane);
-        } else {
-          cout << "No plane detected. Point the camera to a planar region."
-               << endl;
-        }
-        menu_detectplane = false;
-      }
-
-      if (!vpPlane.empty()) {
-        // Recompute plane if there has been a loop closure or global BA
-        // In localization mode, map is not updated so we do not need to
-        // recompute
-        bool bRecompute = false;
-        if (!bLocalizationMode) {
-          if (mpSystem->MapChanged()) {
-            cout << "Map changed. All virtual elements are recomputed!" << endl;
-            bRecompute = true;
-          }
-        }
-
-        for (size_t i = 0; i < vpPlane.size(); i++) {
-          Plane *pPlane = vpPlane[i];
-
-          if (pPlane) {
-            if (bRecompute) {
-              pPlane->Recompute();
-            }
-            glPushMatrix();
-            pPlane->glTpw.Multiply();
-
-            // Draw cube
-            if (menu_drawcube) {
-              DrawCube(menu_cubesize);
-            }
-
-            // Draw grid plane
-            if (menu_drawgrid) {
-              DrawPlane(menu_ngrid, menu_sizegrid);
-            }
-
-            glPopMatrix();
-          }
-        }
-      }
-    }
-
-    pangolin::FinishFrame();
-    usleep(mT * 1000);
-  }
-}
-
-void ViewerAR::SetImagePose(const cv::Mat &im, const cv::Mat &Tcw,
-                            const int &status,
-                            const vector<cv::KeyPoint> &vKeys,
-                            const vector<ORB_SLAM3::MapPoint *> &vMPs) {
-  unique_lock<mutex> lock(mMutexPoseImage);
-  mImage = im.clone();
-  mTcw = Tcw.clone();
-  mStatus = status;
-  mvKeys = vKeys;
-  mvMPs = vMPs;
-}
-
-void ViewerAR::GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status,
-                            std::vector<cv::KeyPoint> &vKeys,
-                            std::vector<MapPoint *> &vMPs) {
-  unique_lock<mutex> lock(mMutexPoseImage);
-  im = mImage.clone();
-  Tcw = mTcw.clone();
-  status = mStatus;
-  vKeys = mvKeys;
-  vMPs = mvMPs;
-}
-
-void ViewerAR::LoadCameraPose(const cv::Mat &Tcw) {
-  if (!Tcw.empty()) {
-    pangolin::OpenGlMatrix M;
-
-    M.m[0] = Tcw.at<float>(0, 0);
-    M.m[1] = Tcw.at<float>(1, 0);
-    M.m[2] = Tcw.at<float>(2, 0);
-    M.m[3] = 0.0;
-
-    M.m[4] = Tcw.at<float>(0, 1);
-    M.m[5] = Tcw.at<float>(1, 1);
-    M.m[6] = Tcw.at<float>(2, 1);
-    M.m[7] = 0.0;
-
-    M.m[8] = Tcw.at<float>(0, 2);
-    M.m[9] = Tcw.at<float>(1, 2);
-    M.m[10] = Tcw.at<float>(2, 2);
-    M.m[11] = 0.0;
-
-    M.m[12] = Tcw.at<float>(0, 3);
-    M.m[13] = Tcw.at<float>(1, 3);
-    M.m[14] = Tcw.at<float>(2, 3);
-    M.m[15] = 1.0;
-
-    M.Load();
-  }
-}
-
-void ViewerAR::PrintStatus(const int &status, const bool &bLocMode,
-                           cv::Mat &im) {
-  if (!bLocMode) {
-    switch (status) {
-      case 1: {
-        AddTextToImage("SLAM NOT INITIALIZED", im, 255, 0, 0);
-        break;
-      }
-      case 2: {
-        AddTextToImage("SLAM ON", im, 0, 255, 0);
-        break;
-      }
-      case 3: {
-        AddTextToImage("SLAM LOST", im, 255, 0, 0);
-        break;
-      }
-    }
-  } else {
-    switch (status) {
-      case 1: {
-        AddTextToImage("SLAM NOT INITIALIZED", im, 255, 0, 0);
-        break;
-      }
-      case 2: {
-        AddTextToImage("LOCALIZATION ON", im, 0, 255, 0);
-        break;
-      }
-      case 3: {
-        AddTextToImage("LOCALIZATION LOST", im, 255, 0, 0);
-        break;
-      }
-    }
-  }
-}
-
-void ViewerAR::AddTextToImage(const string &s, cv::Mat &im, const int r,
-                              const int g, const int b) {
-  int l = 10;
-  // imText.rowRange(im.rows-imText.rows,imText.rows) =
-  // cv::Mat::zeros(textSize.height+10,im.cols,im.type());
-  cv::putText(im, s, cv::Point(l, im.rows - l), cv::FONT_HERSHEY_PLAIN, 1.5,
-              cv::Scalar(255, 255, 255), 2, 8);
-  cv::putText(im, s, cv::Point(l - 1, im.rows - l), cv::FONT_HERSHEY_PLAIN, 1.5,
-              cv::Scalar(255, 255, 255), 2, 8);
-  cv::putText(im, s, cv::Point(l + 1, im.rows - l), cv::FONT_HERSHEY_PLAIN, 1.5,
-              cv::Scalar(255, 255, 255), 2, 8);
-  cv::putText(im, s, cv::Point(l - 1, im.rows - (l - 1)),
-              cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8);
-  cv::putText(im, s, cv::Point(l, im.rows - (l - 1)), cv::FONT_HERSHEY_PLAIN,
-              1.5, cv::Scalar(255, 255, 255), 2, 8);
-  cv::putText(im, s, cv::Point(l + 1, im.rows - (l - 1)),
-              cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8);
-  cv::putText(im, s, cv::Point(l - 1, im.rows - (l + 1)),
-              cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8);
-  cv::putText(im, s, cv::Point(l, im.rows - (l + 1)), cv::FONT_HERSHEY_PLAIN,
-              1.5, cv::Scalar(255, 255, 255), 2, 8);
-  cv::putText(im, s, cv::Point(l + 1, im.rows - (l + 1)),
-              cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255, 255, 255), 2, 8);
-
-  cv::putText(im, s, cv::Point(l, im.rows - l), cv::FONT_HERSHEY_PLAIN, 1.5,
-              cv::Scalar(r, g, b), 2, 8);
-}
-
-void ViewerAR::DrawImageTexture(pangolin::GlTexture &imageTexture,
-                                cv::Mat &im) {
-  if (!im.empty()) {
-    imageTexture.Upload(im.data, GL_RGB, GL_UNSIGNED_BYTE);
-    imageTexture.RenderToViewportFlipY();
-  }
-}
-
-void ViewerAR::DrawCube(const float &size, const float x, const float y,
-                        const float z) {
-  pangolin::OpenGlMatrix M =
-      pangolin::OpenGlMatrix::Translate(-x, -size - y, -z);
-  glPushMatrix();
-  M.Multiply();
-  pangolin::glDrawColouredCube(-size, size);
-  glPopMatrix();
-}
-
-void ViewerAR::DrawPlane(Plane *pPlane, int ndivs, float ndivsize) {
-  glPushMatrix();
-  pPlane->glTpw.Multiply();
-  DrawPlane(ndivs, ndivsize);
-  glPopMatrix();
-}
-
-void ViewerAR::DrawPlane(int ndivs, float ndivsize) {
-  // Plane parallel to x-z at origin with normal -y
-  const float minx = -ndivs * ndivsize;
-  const float minz = -ndivs * ndivsize;
-  const float maxx = ndivs * ndivsize;
-  const float maxz = ndivs * ndivsize;
-
-  glLineWidth(2);
-  glColor3f(0.7f, 0.7f, 1.0f);
-  glBegin(GL_LINES);
-
-  for (int n = 0; n <= 2 * ndivs; n++) {
-    glVertex3f(minx + ndivsize * n, 0, minz);
-    glVertex3f(minx + ndivsize * n, 0, maxz);
-    glVertex3f(minx, 0, minz + ndivsize * n);
-    glVertex3f(maxx, 0, minz + ndivsize * n);
-  }
-
-  glEnd();
-}
-
-void ViewerAR::DrawTrackedPoints(const std::vector<cv::KeyPoint> &vKeys,
-                                 const std::vector<MapPoint *> &vMPs,
-                                 cv::Mat &im) {
-  const int N = vKeys.size();
-
-  for (int i = 0; i < N; i++) {
-    if (vMPs[i]) {
-      cv::circle(im, vKeys[i].pt, 1, cv::Scalar(0, 255, 0), -1);
-    }
-  }
-}
-
-Plane *ViewerAR::DetectPlane(const cv::Mat Tcw,
-                             const std::vector<MapPoint *> &vMPs,
-                             const int iterations) {
-  // Retrieve 3D points
-  vector<cv::Mat> vPoints;
-  vPoints.reserve(vMPs.size());
-  vector<MapPoint *> vPointMP;
-  vPointMP.reserve(vMPs.size());
-
-  for (size_t i = 0; i < vMPs.size(); i++) {
-    MapPoint *pMP = vMPs[i];
-    if (pMP) {
-      if (pMP->Observations() > 5) {
-        cv::Mat WorldPos;
-        cv::eigen2cv(pMP->GetWorldPos(), WorldPos);
-        vPoints.push_back(WorldPos);
-        vPointMP.push_back(pMP);
-      }
-    }
-  }
-
-  const int N = vPoints.size();
-
-  if (N < 50) return NULL;
-
-  // Indices for minimum set selection
-  vector<size_t> vAllIndices;
-  vAllIndices.reserve(N);
-  vector<size_t> vAvailableIndices;
-
-  for (int i = 0; i < N; i++) {
-    vAllIndices.push_back(i);
-  }
-
-  float bestDist = 1e10;
-  vector<float> bestvDist;
-
-  // RANSAC
-  for (int n = 0; n < iterations; n++) {
-    vAvailableIndices = vAllIndices;
-
-    cv::Mat A(3, 4, CV_32F);
-    A.col(3) = cv::Mat::ones(3, 1, CV_32F);
-
-    // Get min set of points
-    for (short i = 0; i < 3; ++i) {
-      int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size() - 1);
-
-      int idx = vAvailableIndices[randi];
-
-      A.row(i).colRange(0, 3) = vPoints[idx].t();
-
-      vAvailableIndices[randi] = vAvailableIndices.back();
-      vAvailableIndices.pop_back();
-    }
-
-    cv::Mat u, w, vt;
-    cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
-
-    const float a = vt.at<float>(3, 0);
-    const float b = vt.at<float>(3, 1);
-    const float c = vt.at<float>(3, 2);
-    const float d = vt.at<float>(3, 3);
-
-    vector<float> vDistances(N, 0);
-
-    const float f = 1.0f / sqrt(a * a + b * b + c * c + d * d);
-
-    for (int i = 0; i < N; i++) {
-      vDistances[i] =
-          fabs(vPoints[i].at<float>(0) * a + vPoints[i].at<float>(1) * b +
-               vPoints[i].at<float>(2) * c + d) *
-          f;
-    }
-
-    vector<float> vSorted = vDistances;
-    sort(vSorted.begin(), vSorted.end());
-
-    int nth = max((int)(0.2 * N), 20);
-    const float medianDist = vSorted[nth];
-
-    if (medianDist < bestDist) {
-      bestDist = medianDist;
-      bestvDist = vDistances;
-    }
-  }
-
-  // Compute threshold inlier/outlier
-  const float th = 1.4 * bestDist;
-  vector<bool> vbInliers(N, false);
-  int nInliers = 0;
-  for (int i = 0; i < N; i++) {
-    if (bestvDist[i] < th) {
-      nInliers++;
-      vbInliers[i] = true;
-    }
-  }
-
-  vector<MapPoint *> vInlierMPs(nInliers, NULL);
-  int nin = 0;
-  for (int i = 0; i < N; i++) {
-    if (vbInliers[i]) {
-      vInlierMPs[nin] = vPointMP[i];
-      nin++;
-    }
-  }
-
-  return new Plane(vInlierMPs, Tcw);
-}
-
-Plane::Plane(const std::vector<MapPoint *> &vMPs, const cv::Mat &Tcw)
-    : mvMPs(vMPs), mTcw(Tcw.clone()) {
-  rang = -3.14f / 2 + ((float)rand() / RAND_MAX) * 3.14f;
-  Recompute();
-}
-
-void Plane::Recompute() {
-  const int N = mvMPs.size();
-
-  // Recompute plane with all points
-  cv::Mat A = cv::Mat(N, 4, CV_32F);
-  A.col(3) = cv::Mat::ones(N, 1, CV_32F);
-
-  o = cv::Mat::zeros(3, 1, CV_32F);
-
-  int nPoints = 0;
-  for (int i = 0; i < N; i++) {
-    MapPoint *pMP = mvMPs[i];
-    if (!pMP->isBad()) {
-      cv::Mat Xw;
-      cv::eigen2cv(pMP->GetWorldPos(), Xw);
-      o += Xw;
-      A.row(nPoints).colRange(0, 3) = Xw.t();
-      nPoints++;
-    }
-  }
-  A.resize(nPoints);
-
-  cv::Mat u, w, vt;
-  cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
-
-  float a = vt.at<float>(3, 0);
-  float b = vt.at<float>(3, 1);
-  float c = vt.at<float>(3, 2);
-
-  o = o * (1.0f / nPoints);
-  const float f = 1.0f / sqrt(a * a + b * b + c * c);
-
-  // Compute XC just the first time
-  if (XC.empty()) {
-    cv::Mat Oc =
-        -mTcw.colRange(0, 3).rowRange(0, 3).t() * mTcw.rowRange(0, 3).col(3);
-    XC = Oc - o;
-  }
-
-  if ((XC.at<float>(0) * a + XC.at<float>(1) * b + XC.at<float>(2) * c) > 0) {
-    a = -a;
-    b = -b;
-    c = -c;
-  }
-
-  const float nx = a * f;
-  const float ny = b * f;
-  const float nz = c * f;
-
-  n = (cv::Mat_<float>(3, 1) << nx, ny, nz);
-
-  cv::Mat up = (cv::Mat_<float>(3, 1) << 0.0f, 1.0f, 0.0f);
-
-  cv::Mat v = up.cross(n);
-  const float sa = cv::norm(v);
-  const float ca = up.dot(n);
-  const float ang = atan2(sa, ca);
-  Tpw = cv::Mat::eye(4, 4, CV_32F);
-
-  Tpw.rowRange(0, 3).colRange(0, 3) = ExpSO3(v * ang / sa) * ExpSO3(up * rang);
-  o.copyTo(Tpw.col(3).rowRange(0, 3));
-
-  glTpw.m[0] = Tpw.at<float>(0, 0);
-  glTpw.m[1] = Tpw.at<float>(1, 0);
-  glTpw.m[2] = Tpw.at<float>(2, 0);
-  glTpw.m[3] = 0.0;
-
-  glTpw.m[4] = Tpw.at<float>(0, 1);
-  glTpw.m[5] = Tpw.at<float>(1, 1);
-  glTpw.m[6] = Tpw.at<float>(2, 1);
-  glTpw.m[7] = 0.0;
-
-  glTpw.m[8] = Tpw.at<float>(0, 2);
-  glTpw.m[9] = Tpw.at<float>(1, 2);
-  glTpw.m[10] = Tpw.at<float>(2, 2);
-  glTpw.m[11] = 0.0;
-
-  glTpw.m[12] = Tpw.at<float>(0, 3);
-  glTpw.m[13] = Tpw.at<float>(1, 3);
-  glTpw.m[14] = Tpw.at<float>(2, 3);
-  glTpw.m[15] = 1.0;
-}
-
-Plane::Plane(const float &nx, const float &ny, const float &nz, const float &ox,
-             const float &oy, const float &oz) {
-  n = (cv::Mat_<float>(3, 1) << nx, ny, nz);
-  o = (cv::Mat_<float>(3, 1) << ox, oy, oz);
-
-  cv::Mat up = (cv::Mat_<float>(3, 1) << 0.0f, 1.0f, 0.0f);
-
-  cv::Mat v = up.cross(n);
-  const float s = cv::norm(v);
-  const float c = up.dot(n);
-  const float a = atan2(s, c);
-  Tpw = cv::Mat::eye(4, 4, CV_32F);
-  const float rang = -3.14f / 2 + ((float)rand() / RAND_MAX) * 3.14f;
-  cout << rang;
-  Tpw.rowRange(0, 3).colRange(0, 3) = ExpSO3(v * a / s) * ExpSO3(up * rang);
-  o.copyTo(Tpw.col(3).rowRange(0, 3));
-
-  glTpw.m[0] = Tpw.at<float>(0, 0);
-  glTpw.m[1] = Tpw.at<float>(1, 0);
-  glTpw.m[2] = Tpw.at<float>(2, 0);
-  glTpw.m[3] = 0.0;
-
-  glTpw.m[4] = Tpw.at<float>(0, 1);
-  glTpw.m[5] = Tpw.at<float>(1, 1);
-  glTpw.m[6] = Tpw.at<float>(2, 1);
-  glTpw.m[7] = 0.0;
-
-  glTpw.m[8] = Tpw.at<float>(0, 2);
-  glTpw.m[9] = Tpw.at<float>(1, 2);
-  glTpw.m[10] = Tpw.at<float>(2, 2);
-  glTpw.m[11] = 0.0;
-
-  glTpw.m[12] = Tpw.at<float>(0, 3);
-  glTpw.m[13] = Tpw.at<float>(1, 3);
-  glTpw.m[14] = Tpw.at<float>(2, 3);
-  glTpw.m[15] = 1.0;
-}
-
-}  // namespace ORB_SLAM3
diff --git a/Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.h b/Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.h
deleted file mode 100644
index 6a87efab00a..00000000000
--- a/Examples/ROS/ORB_SLAM3/src/AR/ViewerAR.h
+++ /dev/null
@@ -1,122 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
- * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
- * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
- * University of Zaragoza.
- *
- * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
- * the terms of the GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option) any later
- * version.
- *
- * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
- * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef VIEWERAR_H
-#define VIEWERAR_H
-
-#include <pangolin/pangolin.h>
-
-#include <mutex>
-#include <opencv2/core.hpp>
-#include <string>
-
-#include "../../../include/System.h"
-
-namespace ORB_SLAM3 {
-
-class Plane {
- public:
-  Plane(const std::vector<MapPoint *> &vMPs, const cv::Mat &Tcw);
-  Plane(const float &nx, const float &ny, const float &nz, const float &ox,
-        const float &oy, const float &oz);
-
-  void Recompute();
-
-  // normal
-  cv::Mat n;
-  // origin
-  cv::Mat o;
-  // arbitrary orientation along normal
-  float rang;
-  // transformation from world to the plane
-  cv::Mat Tpw;
-  pangolin::OpenGlMatrix glTpw;
-  // MapPoints that define the plane
-  std::vector<MapPoint *> mvMPs;
-  // camera pose when the plane was first observed (to compute normal direction)
-  cv::Mat mTcw, XC;
-};
-
-class ViewerAR {
- public:
-  ViewerAR();
-
-  void SetFPS(const float fps) {
-    mFPS = fps;
-    mT = 1e3 / fps;
-  }
-
-  void SetSLAM(ORB_SLAM3::System *pSystem) { mpSystem = pSystem; }
-
-  // Main thread function.
-  void Run();
-
-  void SetCameraCalibration(const float &fx_, const float &fy_,
-                            const float &cx_, const float &cy_) {
-    fx = fx_;
-    fy = fy_;
-    cx = cx_;
-    cy = cy_;
-  }
-
-  void SetImagePose(const cv::Mat &im, const cv::Mat &Tcw, const int &status,
-                    const std::vector<cv::KeyPoint> &vKeys,
-                    const std::vector<MapPoint *> &vMPs);
-
-  void GetImagePose(cv::Mat &im, cv::Mat &Tcw, int &status,
-                    std::vector<cv::KeyPoint> &vKeys,
-                    std::vector<MapPoint *> &vMPs);
-
- private:
-  // SLAM
-  ORB_SLAM3::System *mpSystem;
-
-  void PrintStatus(const int &status, const bool &bLocMode, cv::Mat &im);
-  void AddTextToImage(const std::string &s, cv::Mat &im, const int r = 0,
-                      const int g = 0, const int b = 0);
-  void LoadCameraPose(const cv::Mat &Tcw);
-  void DrawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im);
-  void DrawCube(const float &size, const float x = 0, const float y = 0,
-                const float z = 0);
-  void DrawPlane(int ndivs, float ndivsize);
-  void DrawPlane(Plane *pPlane, int ndivs, float ndivsize);
-  void DrawTrackedPoints(const std::vector<cv::KeyPoint> &vKeys,
-                         const std::vector<MapPoint *> &vMPs, cv::Mat &im);
-
-  Plane *DetectPlane(const cv::Mat Tcw, const std::vector<MapPoint *> &vMPs,
-                     const int iterations = 50);
-
-  // frame rate
-  float mFPS, mT;
-  float fx, fy, cx, cy;
-
-  // Last processed image and computed pose by the SLAM
-  std::mutex mMutexPoseImage;
-  cv::Mat mTcw;
-  cv::Mat mImage;
-  int mStatus;
-  std::vector<cv::KeyPoint> mvKeys;
-  std::vector<MapPoint *> mvMPs;
-};
-
-}  // namespace ORB_SLAM3
-
-#endif  // VIEWERAR_H
diff --git a/Examples/ROS/ORB_SLAM3/src/AR/ros_mono_ar.cc b/Examples/ROS/ORB_SLAM3/src/AR/ros_mono_ar.cc
deleted file mode 100644
index 928699100ac..00000000000
--- a/Examples/ROS/ORB_SLAM3/src/AR/ros_mono_ar.cc
+++ /dev/null
@@ -1,138 +0,0 @@
-#include <cv_bridge/cv_bridge.h>
-#include <ros/ros.h>
-
-#include <Eigen/Dense>
-#include <algorithm>
-#include <chrono>
-#include <fstream>
-#include <iostream>
-#include <opencv2/core.hpp>
-#include <opencv2/core/eigen.hpp>
-#include <opencv2/imgproc.hpp>
-#include <thread>
-
-#include "../../../include/System.h"
-#include "ViewerAR.h"
-
-using namespace std;
-
-ORB_SLAM3::ViewerAR viewerAR;
-bool bRGB = true;
-
-cv::Mat K;
-cv::Mat DistCoef;
-
-class ImageGrabber {
- public:
-  ImageGrabber(ORB_SLAM3::System* pSLAM) : mpSLAM(pSLAM) {}
-
-  void GrabImage(const sensor_msgs::ImageConstPtr& msg);
-
-  ORB_SLAM3::System* mpSLAM;
-};
-
-int main(int argc, char** argv) {
-  ros::init(argc, argv, "Mono");
-  ros::start();
-
-  if (argc != 3) {
-    cerr << endl
-         << "Usage: rosrun ORB_SLAM3 Mono path_to_vocabulary path_to_settings"
-         << endl;
-    ros::shutdown();
-    return 1;
-  }
-
-  // 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);
-
-  cout << endl << endl;
-  cout << "-----------------------" << endl;
-  cout << "Augmented Reality Demo" << endl;
-  cout << "1) Translate the camera to initialize SLAM." << endl;
-  cout << "2) Look at a planar region and translate the camera." << endl;
-  cout << "3) Press Insert Cube to place a virtual cube in the plane. " << endl;
-  cout << endl;
-  cout << "You can place several cubes in different planes." << endl;
-  cout << "-----------------------" << endl;
-  cout << endl;
-
-  viewerAR.SetSLAM(&SLAM);
-
-  ImageGrabber igb(&SLAM);
-
-  ros::NodeHandle nodeHandler;
-  ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1,
-                                              &ImageGrabber::GrabImage, &igb);
-
-  cv::FileStorage fSettings(argv[2], cv::FileStorage::READ);
-  bRGB = static_cast<bool>((int)fSettings["Camera.RGB"]);
-  float fps = fSettings["Camera.fps"];
-  viewerAR.SetFPS(fps);
-
-  float fx = fSettings["Camera.fx"];
-  float fy = fSettings["Camera.fy"];
-  float cx = fSettings["Camera.cx"];
-  float cy = fSettings["Camera.cy"];
-
-  viewerAR.SetCameraCalibration(fx, fy, cx, cy);
-
-  K = cv::Mat::eye(3, 3, CV_32F);
-  K.at<float>(0, 0) = fx;
-  K.at<float>(1, 1) = fy;
-  K.at<float>(0, 2) = cx;
-  K.at<float>(1, 2) = cy;
-
-  DistCoef = cv::Mat::zeros(5, 1, CV_32F);  // Use 5 for k3 support
-  DistCoef.at<float>(0) = fSettings["Camera.k1"];
-  DistCoef.at<float>(1) = fSettings["Camera.k2"];
-  DistCoef.at<float>(2) = fSettings["Camera.p1"];
-  DistCoef.at<float>(3) = fSettings["Camera.p2"];
-  const float k3 = fSettings["Camera.k3"];
-  DistCoef.at<float>(4) = k3;
-
-  std::thread tViewer(&ORB_SLAM3::ViewerAR::Run, &viewerAR);
-
-  ros::spin();
-
-  // Stop all threads
-  SLAM.Shutdown();
-
-  // Save camera trajectory
-  SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
-
-  ros::shutdown();
-
-  return 0;
-}
-
-void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) {
-  // Copy the ROS image message to cv::Mat.
-  cv_bridge::CvImageConstPtr cv_ptr;
-  try {
-    cv_ptr = cv_bridge::toCvShare(msg);
-  } catch (cv_bridge::Exception& e) {
-    ROS_ERROR("cv_bridge exception: %s", e.what());
-    return;
-  }
-  cv::Mat im = cv_ptr->image.clone();
-  cv::Mat imu;
-  cv::Mat Tcw;
-  Sophus::SE3f Tcw_SE3f =
-      mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec());
-  Eigen::Matrix4f Tcw_Matrix = Tcw_SE3f.matrix();
-  cv::eigen2cv(Tcw_Matrix, Tcw);
-  int state = mpSLAM->GetTrackingState();
-  vector<ORB_SLAM3::MapPoint*> vMPs = mpSLAM->GetTrackedMapPoints();
-  vector<cv::KeyPoint> vKeys = mpSLAM->GetTrackedKeyPointsUn();
-
-  cv::undistort(im, imu, K, DistCoef);
-
-  if (bRGB)
-    viewerAR.SetImagePose(imu, Tcw, state, vKeys, vMPs);
-  else {
-    cv::cvtColor(imu, imu, cv::COLOR_RGB2BGR);
-    viewerAR.SetImagePose(imu, Tcw, state, vKeys, vMPs);
-  }
-}
diff --git a/Examples/ROS/ORB_SLAM3/src/ros_mono.cc b/Examples/ROS/ORB_SLAM3/src/ros_mono.cc
deleted file mode 100644
index 0a6b0022d37..00000000000
--- a/Examples/ROS/ORB_SLAM3/src/ros_mono.cc
+++ /dev/null
@@ -1,89 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
- * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
- * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
- * University of Zaragoza.
- *
- * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
- * the terms of the GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option) any later
- * version.
- *
- * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
- * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <cv_bridge/cv_bridge.h>
-#include <ros/ros.h>
-#include <sensor_msgs/CompressedImage.h>
-#include <sensor_msgs/image_encodings.h>
-
-#include <iostream>
-#include <opencv2/core.hpp>
-
-#include "../../../include/System.h"
-
-using namespace std;
-
-class ImageGrabber {
- public:
-  ImageGrabber(ORB_SLAM3::System* pSLAM) : mpSLAM(pSLAM) {}
-
-  void GrabImage(const sensor_msgs::CompressedImageConstPtr& msg);
-
-  ORB_SLAM3::System* mpSLAM;
-};
-
-int main(int argc, char** argv) {
-  ros::init(argc, argv, "Mono");
-  ros::start();
-
-  if (argc != 3) {
-    cerr << endl
-         << "Usage: rosrun ORB_SLAM3 Mono path_to_vocabulary path_to_settings"
-         << endl;
-    ros::shutdown();
-    return 1;
-  }
-
-  // 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, true);
-
-  ImageGrabber igb(&SLAM);
-
-  ros::NodeHandle nodeHandler;
-  ros::Subscriber sub = nodeHandler.subscribe("/tello/image_raw/compressed", 1,
-                                              &ImageGrabber::GrabImage, &igb);
-
-  ros::spin();
-
-  // Stop all threads
-  SLAM.Shutdown();
-
-  // Save camera trajectory
-  SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
-
-  ros::shutdown();
-
-  return 0;
-}
-
-void ImageGrabber::GrabImage(const sensor_msgs::CompressedImageConstPtr& msg) {
-  // Copy the ros image message to cv::Mat.
-  cv_bridge::CvImageConstPtr cv_ptr;
-  try {
-    cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
-  } catch (cv_bridge::Exception& e) {
-    ROS_ERROR("cv_bridge exception: %s", e.what());
-    return;
-  }
-
-  mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec());
-}
diff --git a/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc b/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc
deleted file mode 100644
index 1f89d001a0b..00000000000
--- a/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc
+++ /dev/null
@@ -1,184 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
- * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
- * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
- * University of Zaragoza.
- *
- * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
- * the terms of the GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option) any later
- * version.
- *
- * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
- * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <cv_bridge/cv_bridge.h>
-#include <ros/ros.h>
-#include <sensor_msgs/Imu.h>
-
-#include <algorithm>
-#include <chrono>
-#include <fstream>
-#include <iostream>
-#include <mutex>
-#include <opencv2/core.hpp>  // Updated include header
-#include <queue>
-#include <thread>
-#include <vector>
-
-#include "../../../include/System.h"
-#include "../include/ImuTypes.h"
-
-using namespace std;
-
-class ImuGrabber {
- public:
-  ImuGrabber() {};
-  void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);
-
-  queue<sensor_msgs::ImuConstPtr> imuBuf;
-  std::mutex mBufMutex;
-};
-
-class ImageGrabber {
- public:
-  ImageGrabber(ORB_SLAM3::System *pSLAM, ImuGrabber *pImuGb, const bool bClahe)
-      : mpSLAM(pSLAM), mpImuGb(pImuGb), mbClahe(bClahe) {}
-
-  void GrabImage(const sensor_msgs::ImageConstPtr &msg);
-  cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);
-  void SyncWithImu();
-
-  queue<sensor_msgs::ImageConstPtr> img0Buf;
-  std::mutex mBufMutex;
-
-  ORB_SLAM3::System *mpSLAM;
-  ImuGrabber *mpImuGb;
-
-  const bool mbClahe;
-  cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));
-};
-
-int main(int argc, char **argv) {
-  ros::init(argc, argv, "Mono_Inertial");
-  ros::NodeHandle n("~");
-  ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
-                                 ros::console::levels::Info);
-  bool bEqual = false;
-  if (argc < 3 || argc > 4) {
-    cerr << endl
-         << "Usage: rosrun ORB_SLAM3 Mono_Inertial path_to_vocabulary "
-            "path_to_settings [do_equalize]"
-         << endl;
-    ros::shutdown();
-    return 1;
-  }
-
-  if (argc == 4) {
-    std::string sbEqual(argv[3]);
-    if (sbEqual == "true") bEqual = true;
-  }
-
-  // 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);
-
-  ImuGrabber imugb;
-  ImageGrabber igb(&SLAM, &imugb, bEqual);
-
-  // Maximum delay, 5 seconds
-  ros::Subscriber sub_imu =
-      n.subscribe("/tello/imu", 1000, &ImuGrabber::GrabImu, &imugb);
-  ros::Subscriber sub_img0 =
-      n.subscribe("/tello/image_raw", 100, &ImageGrabber::GrabImage, &igb);
-
-  std::thread sync_thread(&ImageGrabber::SyncWithImu, &igb);
-
-  ros::spin();
-
-  return 0;
-}
-
-// ... (rest of the code remains the same)
-
-void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &img_msg) {
-  mBufMutex.lock();
-  if (!img0Buf.empty()) img0Buf.pop();
-  img0Buf.push(img_msg);
-  mBufMutex.unlock();
-}
-
-cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg) {
-  // Copy the ros image message to cv::Mat.
-  cv_bridge::CvImageConstPtr cv_ptr;
-  try {
-    cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8);
-  } catch (cv_bridge::Exception &e) {
-    ROS_ERROR("cv_bridge exception: %s", e.what());
-  }
-
-  if (cv_ptr->image.type() == 0) {
-    return cv_ptr->image.clone();
-  } else {
-    std::cout << "Error type" << std::endl;
-    return cv_ptr->image.clone();
-  }
-}
-
-void ImageGrabber::SyncWithImu() {
-  while (1) {
-    cv::Mat im;
-    double tIm = 0;
-    if (!img0Buf.empty() && !mpImuGb->imuBuf.empty()) {
-      tIm = img0Buf.front()->header.stamp.toSec();
-      if (tIm > mpImuGb->imuBuf.back()->header.stamp.toSec()) continue;
-      {
-        this->mBufMutex.lock();
-        im = GetImage(img0Buf.front());
-        img0Buf.pop();
-        this->mBufMutex.unlock();
-      }
-
-      vector<ORB_SLAM3::IMU::Point> vImuMeas;
-      mpImuGb->mBufMutex.lock();
-      if (!mpImuGb->imuBuf.empty()) {
-        // Load imu measurements from buffer
-        vImuMeas.clear();
-        while (!mpImuGb->imuBuf.empty() &&
-               mpImuGb->imuBuf.front()->header.stamp.toSec() <= tIm) {
-          double t = mpImuGb->imuBuf.front()->header.stamp.toSec();
-          cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x,
-                          mpImuGb->imuBuf.front()->linear_acceleration.y,
-                          mpImuGb->imuBuf.front()->linear_acceleration.z);
-          cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x,
-                          mpImuGb->imuBuf.front()->angular_velocity.y,
-                          mpImuGb->imuBuf.front()->angular_velocity.z);
-          vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t));
-          mpImuGb->imuBuf.pop();
-        }
-      }
-      mpImuGb->mBufMutex.unlock();
-      if (mbClahe) mClahe->apply(im, im);
-
-      mpSLAM->TrackMonocular(im, tIm, vImuMeas);
-    }
-
-    std::chrono::milliseconds tSleep(1);
-    std::this_thread::sleep_for(tSleep);
-  }
-}
-
-void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg) {
-  mBufMutex.lock();
-  imuBuf.push(imu_msg);
-  mBufMutex.unlock();
-  return;
-}
diff --git a/Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc b/Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc
deleted file mode 100644
index beb10c4d01e..00000000000
--- a/Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc
+++ /dev/null
@@ -1,111 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
- * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
- * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
- * University of Zaragoza.
- *
- * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
- * the terms of the GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option) any later
- * version.
- *
- * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
- * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <cv_bridge/cv_bridge.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/synchronizer.h>
-#include <ros/ros.h>
-#include <sensor_msgs/Image.h>
-
-#include <iostream>
-#include <opencv2/core.hpp>
-
-#include "../../../include/System.h"
-
-using namespace std;
-
-class ImageGrabber {
- public:
-  ImageGrabber(ORB_SLAM3::System* pSLAM) : mpSLAM(pSLAM) {}
-
-  void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,
-                const sensor_msgs::ImageConstPtr& msgD);
-
-  ORB_SLAM3::System* mpSLAM;
-};
-
-int main(int argc, char** argv) {
-  ros::init(argc, argv, "RGBD");
-  ros::start();
-
-  if (argc != 3) {
-    cerr << endl
-         << "Usage: rosrun ORB_SLAM3 RGBD path_to_vocabulary path_to_settings"
-         << endl;
-    ros::shutdown();
-    return 1;
-  }
-
-  // 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::RGBD, true);
-
-  ImageGrabber igb(&SLAM);
-
-  ros::NodeHandle nh;
-
-  message_filters::Subscriber<sensor_msgs::Image> rgb_sub(
-      nh, "/camera/rgb/image_raw", 100);
-  message_filters::Subscriber<sensor_msgs::Image> depth_sub(
-      nh, "camera/depth_registered/image_raw", 100);
-  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
-                                                          sensor_msgs::Image>
-      sync_pol;
-  message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,
-                                               depth_sub);
-  sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD, &igb, _1, _2));
-
-  ros::spin();
-
-  // Stop all threads
-  SLAM.Shutdown();
-
-  // Save camera trajectory
-  SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
-
-  ros::shutdown();
-
-  return 0;
-}
-
-void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,
-                            const sensor_msgs::ImageConstPtr& msgD) {
-  // Copy the ros image messages to cv::Mat.
-  cv_bridge::CvImageConstPtr cv_ptrRGB;
-  try {
-    cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
-  } catch (cv_bridge::Exception& e) {
-    ROS_ERROR("cv_bridge exception for RGB: %s", e.what());
-    return;
-  }
-
-  cv_bridge::CvImageConstPtr cv_ptrD;
-  try {
-    cv_ptrD = cv_bridge::toCvShare(msgD);
-  } catch (cv_bridge::Exception& e) {
-    ROS_ERROR("cv_bridge exception for depth: %s", e.what());
-    return;
-  }
-
-  mpSLAM->TrackRGBD(cv_ptrRGB->image, cv_ptrD->image,
-                    cv_ptrRGB->header.stamp.toSec());
-}
diff --git a/Examples/ROS/ORB_SLAM3/src/ros_stereo.cc b/Examples/ROS/ORB_SLAM3/src/ros_stereo.cc
deleted file mode 100644
index b6ada1ec533..00000000000
--- a/Examples/ROS/ORB_SLAM3/src/ros_stereo.cc
+++ /dev/null
@@ -1,172 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
- * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
- * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
- * University of Zaragoza.
- *
- * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
- * the terms of the GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option) any later
- * version.
- *
- * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
- * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <cv_bridge/cv_bridge.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/time_synchronizer.h>
-#include <ros/ros.h>
-
-#include <algorithm>
-#include <chrono>
-#include <fstream>
-#include <iostream>
-#include <opencv2/core.hpp>
-#include <opencv2/highgui.hpp>  // For cv::imshow, cv::waitKey
-#include <opencv2/imgproc.hpp>  // For cv::remap
-
-#include "../../../include/System.h"
-
-using namespace std;
-
-class ImageGrabber {
- public:
-  ImageGrabber(ORB_SLAM3::System* pSLAM) : mpSLAM(pSLAM) {}
-
-  void GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,
-                  const sensor_msgs::ImageConstPtr& msgRight);
-
-  ORB_SLAM3::System* mpSLAM;
-  bool do_rectify;
-  cv::Mat M1l, M2l, M1r, M2r;
-};
-
-int main(int argc, char** argv) {
-  ros::init(argc, argv, "RGBD");
-  ros::start();
-
-  if (argc != 4) {
-    cerr << endl
-         << "Usage: rosrun ORB_SLAM3 Stereo path_to_vocabulary "
-            "path_to_settings do_rectify"
-         << endl;
-    ros::shutdown();
-    return 1;
-  }
-
-  // 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::STEREO, true);
-
-  ImageGrabber igb(&SLAM);
-
-  stringstream ss(argv[3]);
-  ss >> boolalpha >> igb.do_rectify;
-
-  if (igb.do_rectify) {
-    // Load settings related to stereo calibration
-    cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
-    if (!fsSettings.isOpened()) {
-      cerr << "ERROR: Wrong path to settings" << endl;
-      return -1;
-    }
-
-    cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
-    fsSettings["LEFT.K"] >> K_l;
-    fsSettings["RIGHT.K"] >> K_r;
-
-    fsSettings["LEFT.P"] >> P_l;
-    fsSettings["RIGHT.P"] >> P_r;
-
-    fsSettings["LEFT.R"] >> R_l;
-    fsSettings["RIGHT.R"] >> R_r;
-
-    fsSettings["LEFT.D"] >> D_l;
-    fsSettings["RIGHT.D"] >> D_r;
-
-    int rows_l = static_cast<int>(fsSettings["LEFT.height"]);
-    int cols_l = static_cast<int>(fsSettings["LEFT.width"]);
-    int rows_r = static_cast<int>(fsSettings["RIGHT.height"]);
-    int cols_r = static_cast<int>(fsSettings["RIGHT.width"]);
-
-    if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() ||
-        R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
-        rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) {
-      cerr << "ERROR: Calibration parameters to rectify stereo are missing!"
-           << endl;
-      return -1;
-    }
-
-    cv::initUndistortRectifyMap(
-        K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3),
-        cv::Size(cols_l, rows_l), CV_32F, igb.M1l, igb.M2l);
-    cv::initUndistortRectifyMap(
-        K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3),
-        cv::Size(cols_r, rows_r), CV_32F, igb.M1r, igb.M2r);
-  }
-
-  ros::NodeHandle nh;
-
-  message_filters::Subscriber<sensor_msgs::Image> left_sub(
-      nh, "/camera/left/image_raw", 1);
-  message_filters::Subscriber<sensor_msgs::Image> right_sub(
-      nh, "/camera/right/image_raw", 1);
-  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image,
-                                                          sensor_msgs::Image>
-      sync_pol;
-  message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub,
-                                               right_sub);
-  sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo, &igb, _1, _2));
-
-  ros::spin();
-
-  // Stop all threads
-  SLAM.Shutdown();
-
-  // Save camera trajectory
-  SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt");
-  SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt");
-  SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt");
-
-  ros::shutdown();
-
-  return 0;
-}
-
-void ImageGrabber::GrabStereo(const sensor_msgs::ImageConstPtr& msgLeft,
-                              const sensor_msgs::ImageConstPtr& msgRight) {
-  // Copy the ros image message to cv::Mat.
-  cv_bridge::CvImageConstPtr cv_ptrLeft;
-  try {
-    cv_ptrLeft = cv_bridge::toCvShare(msgLeft);
-  } catch (cv_bridge::Exception& e) {
-    ROS_ERROR("cv_bridge exception: %s", e.what());
-    return;
-  }
-
-  cv_bridge::CvImageConstPtr cv_ptrRight;
-  try {
-    cv_ptrRight = cv_bridge::toCvShare(msgRight);
-  } catch (cv_bridge::Exception& e) {
-    ROS_ERROR("cv_bridge exception: %s", e.what());
-    return;
-  }
-
-  if (do_rectify) {
-    cv::Mat imLeft, imRight;
-    cv::remap(cv_ptrLeft->image, imLeft, M1l, M2l, cv::INTER_LINEAR);
-    cv::remap(cv_ptrRight->image, imRight, M1r, M2r, cv::INTER_LINEAR);
-    mpSLAM->TrackStereo(imLeft, imRight, cv_ptrLeft->header.stamp.toSec());
-  } else {
-    mpSLAM->TrackStereo(cv_ptrLeft->image, cv_ptrRight->image,
-                        cv_ptrLeft->header.stamp.toSec());
-  }
-}
diff --git a/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc b/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc
deleted file mode 100644
index 7b38268af19..00000000000
--- a/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc
+++ /dev/null
@@ -1,270 +0,0 @@
-/**
- * This file is part of ORB-SLAM3
- *
- * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
- * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
- * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
- * University of Zaragoza.
- *
- * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
- * the terms of the GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option) any later
- * version.
- *
- * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
- * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
- * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <cv_bridge/cv_bridge.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <message_filters/synchronizer.h>
-#include <ros/ros.h>
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/Imu.h>
-
-#include <iostream>
-#include <opencv2/core.hpp>
-
-#include "../../../include/System.h"
-#include "../include/ImuTypes.h"
-
-using namespace std;
-
-class ImuGrabber {
- public:
-  ImuGrabber() {}
-  void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);
-
-  queue<sensor_msgs::ImuConstPtr> imuBuf;
-  std::mutex mBufMutex;
-};
-
-class ImageGrabber {
- public:
-  ImageGrabber(ORB_SLAM3::System *pSLAM, ImuGrabber *pImuGb, const bool bRect,
-               const bool bClahe)
-      : mpSLAM(pSLAM), mpImuGb(pImuGb), do_rectify(bRect), mbClahe(bClahe) {}
-
-  void GrabImageLeft(const sensor_msgs::ImageConstPtr &msg);
-  void GrabImageRight(const sensor_msgs::ImageConstPtr &msg);
-  cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);
-  void SyncWithImu();
-
-  queue<sensor_msgs::ImageConstPtr> imgLeftBuf, imgRightBuf;
-  std::mutex mBufMutexLeft, mBufMutexRight;
-
-  ORB_SLAM3::System *mpSLAM;
-  ImuGrabber *mpImuGb;
-
-  const bool do_rectify;
-  cv::Mat M1l, M2l, M1r, M2r;
-
-  const bool mbClahe;
-  cv::Ptr<cv::CLAHE> mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));
-};
-
-int main(int argc, char **argv) {
-  ros::init(argc, argv, "Stereo_Inertial");
-  ros::NodeHandle n("~");
-  ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
-                                 ros::console::levels::Info);
-  bool bEqual = false;
-  if (argc < 4 || argc > 5) {
-    cerr << endl
-         << "Usage: rosrun ORB_SLAM3 Stereo_Inertial path_to_vocabulary "
-            "path_to_settings do_rectify [do_equalize]"
-         << endl;
-    ros::shutdown();
-    return 1;
-  }
-
-  std::string sbRect(argv[3]);
-  if (argc == 5) {
-    std::string sbEqual(argv[4]);
-    if (sbEqual == "true") bEqual = true;
-  }
-
-  // Create SLAM system. It initializes all system threads and gets ready to
-  // process frames.
-  ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::IMU_STEREO, true);
-
-  ImuGrabber imugb;
-  ImageGrabber igb(&SLAM, &imugb, sbRect == "true", bEqual);
-
-  if (igb.do_rectify) {
-    // Load settings related to stereo calibration
-    cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
-    if (!fsSettings.isOpened()) {
-      cerr << "ERROR: Wrong path to settings" << endl;
-      return -1;
-    }
-
-    cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
-    fsSettings["LEFT.K"] >> K_l;
-    fsSettings["RIGHT.K"] >> K_r;
-
-    fsSettings["LEFT.P"] >> P_l;
-    fsSettings["RIGHT.P"] >> P_r;
-
-    fsSettings["LEFT.R"] >> R_l;
-    fsSettings["RIGHT.R"] >> R_r;
-
-    fsSettings["LEFT.D"] >> D_l;
-    fsSettings["RIGHT.D"] >> D_r;
-
-    int rows_l = fsSettings["LEFT.height"];
-    int cols_l = fsSettings["LEFT.width"];
-    int rows_r = fsSettings["RIGHT.height"];
-    int cols_r = fsSettings["RIGHT.width"];
-
-    if (K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() ||
-        R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
-        rows_l == 0 || rows_r == 0 || cols_l == 0 || cols_r == 0) {
-      cerr << "ERROR: Calibration parameters to rectify stereo are missing!"
-           << endl;
-      return -1;
-    }
-
-    cv::initUndistortRectifyMap(
-        K_l, D_l, R_l, P_l.rowRange(0, 3).colRange(0, 3),
-        cv::Size(cols_l, rows_l), CV_32F, igb.M1l, igb.M2l);
-    cv::initUndistortRectifyMap(
-        K_r, D_r, R_r, P_r.rowRange(0, 3).colRange(0, 3),
-        cv::Size(cols_r, rows_r), CV_32F, igb.M1r, igb.M2r);
-  }
-
-  // Maximum delay, 5 seconds
-  ros::Subscriber sub_imu =
-      n.subscribe("/tello/imu", 1000, &ImuGrabber::GrabImu, &imugb);
-  ros::Subscriber sub_img_left = n.subscribe(
-      "/camera/left/image_raw", 100, &ImageGrabber::GrabImageLeft, &igb);
-  ros::Subscriber sub_img_right = n.subscribe(
-      "/camera/right/image_raw", 100, &ImageGrabber::GrabImageRight, &igb);
-
-  std::thread sync_thread(&ImageGrabber::SyncWithImu, &igb);
-
-  ros::spin();
-
-  return 0;
-}
-
-void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg) {
-  mBufMutexLeft.lock();
-  if (!imgLeftBuf.empty()) imgLeftBuf.pop();
-  imgLeftBuf.push(img_msg);
-  mBufMutexLeft.unlock();
-}
-
-void ImageGrabber::GrabImageRight(const sensor_msgs::ImageConstPtr &img_msg) {
-  mBufMutexRight.lock();
-  if (!imgRightBuf.empty()) imgRightBuf.pop();
-  imgRightBuf.push(img_msg);
-  mBufMutexRight.unlock();
-}
-
-cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg) {
-  // Copy the ros image message to cv::Mat.
-  cv_bridge::CvImageConstPtr cv_ptr;
-  try {
-    cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8);
-  } catch (cv_bridge::Exception &e) {
-    ROS_ERROR("cv_bridge exception: %s", e.what());
-  }
-
-  if (cv_ptr->image.type() == CV_8UC1) {
-    return cv_ptr->image.clone();
-  } else {
-    std::cout << "Error type" << std::endl;
-    return cv_ptr->image.clone();
-  }
-}
-
-void ImageGrabber::SyncWithImu() {
-  const double maxTimeDiff = 0.01;
-  while (1) {
-    cv::Mat imLeft, imRight;
-    double tImLeft = 0, tImRight = 0;
-    if (!imgLeftBuf.empty() && !imgRightBuf.empty() &&
-        !mpImuGb->imuBuf.empty()) {
-      tImLeft = imgLeftBuf.front()->header.stamp.toSec();
-      tImRight = imgRightBuf.front()->header.stamp.toSec();
-
-      this->mBufMutexRight.lock();
-      while ((tImLeft - tImRight) > maxTimeDiff && imgRightBuf.size() > 1) {
-        imgRightBuf.pop();
-        tImRight = imgRightBuf.front()->header.stamp.toSec();
-      }
-      this->mBufMutexRight.unlock();
-
-      this->mBufMutexLeft.lock();
-      while ((tImRight - tImLeft) > maxTimeDiff && imgLeftBuf.size() > 1) {
-        imgLeftBuf.pop();
-        tImLeft = imgLeftBuf.front()->header.stamp.toSec();
-      }
-      this->mBufMutexLeft.unlock();
-
-      if ((tImLeft - tImRight) > maxTimeDiff ||
-          (tImRight - tImLeft) > maxTimeDiff) {
-        // std::cout << "big time difference" << std::endl;
-        continue;
-      }
-      if (tImLeft > mpImuGb->imuBuf.back()->header.stamp.toSec()) continue;
-
-      this->mBufMutexLeft.lock();
-      imLeft = GetImage(imgLeftBuf.front());
-      imgLeftBuf.pop();
-      this->mBufMutexLeft.unlock();
-
-      this->mBufMutexRight.lock();
-      imRight = GetImage(imgRightBuf.front());
-      imgRightBuf.pop();
-      this->mBufMutexRight.unlock();
-
-      vector<ORB_SLAM3::IMU::Point> vImuMeas;
-      mpImuGb->mBufMutex.lock();
-      if (!mpImuGb->imuBuf.empty()) {
-        // Load imu measurements from buffer
-        vImuMeas.clear();
-        while (!mpImuGb->imuBuf.empty() &&
-               mpImuGb->imuBuf.front()->header.stamp.toSec() <= tImLeft) {
-          double t = mpImuGb->imuBuf.front()->header.stamp.toSec();
-          cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x,
-                          mpImuGb->imuBuf.front()->linear_acceleration.y,
-                          mpImuGb->imuBuf.front()->linear_acceleration.z);
-          cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x,
-                          mpImuGb->imuBuf.front()->angular_velocity.y,
-                          mpImuGb->imuBuf.front()->angular_velocity.z);
-          vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc, gyr, t));
-          mpImuGb->imuBuf.pop();
-        }
-      }
-      mpImuGb->mBufMutex.unlock();
-      if (mbClahe) {
-        mClahe->apply(imLeft, imLeft);
-        mClahe->apply(imRight, imRight);
-      }
-
-      if (do_rectify) {
-        cv::remap(imLeft, imLeft, M1l, M2l, cv::INTER_LINEAR);
-        cv::remap(imRight, imRight, M1r, M2r, cv::INTER_LINEAR);
-      }
-
-      mpSLAM->TrackStereo(imLeft, imRight, tImLeft, vImuMeas);
-
-      std::chrono::milliseconds tSleep(1);
-      std::this_thread::sleep_for(tSleep);
-    }
-  }
-}
-
-void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg) {
-  mBufMutex.lock();
-  imuBuf.push(imu_msg);
-  mBufMutex.unlock();
-  return;
-}
diff --git a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc
index 38d7a318e20..a9235211041 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_euroc.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_euroc.cc
@@ -133,8 +133,15 @@ int main(int argc, char **argv) {
 
   // Create SLAM system. It initializes all system threads and gets ready to
   // process frames.
-  ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::IMU_STEREO,
-                         false);
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::IMU_STEREO, false);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
 
   cv::Mat imLeft, imRight;
   for (seq = 0; seq < num_seq; seq++) {
@@ -184,31 +191,21 @@ int main(int argc, char **argv) {
           first_imu[seq]++;
         }
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
       // Pass the images to the SLAM system
-      SLAM.TrackStereo(imLeft, imRight, tframe, vImuMeas);
+      SLAM->TrackStereo(imLeft, imRight, tframe, vImuMeas);
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
 #ifdef REGISTER_TIMES
       t_track = t_rect + t_resize +
                 std::chrono::duration_cast<
                     std::chrono::duration<double, std::milli> >(t2 - t1)
                     .count();
-      SLAM.InsertTrackTime(t_track);
+      SLAM->InsertTrackTime(t_track);
 #endif
 
       double ttrack =
@@ -230,21 +227,21 @@ int main(int argc, char **argv) {
     if (seq < num_seq - 1) {
       cout << "Changing the dataset" << endl;
 
-      SLAM.ChangeDataset();
+      SLAM->ChangeDataset();
     }
   }
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Save camera trajectory
   if (bFileName) {
     const string kf_file = "kf_" + string(argv[argc - 1]) + ".txt";
     const string f_file = "f_" + string(argv[argc - 1]) + ".txt";
-    SLAM.SaveTrajectoryEuRoC(f_file);
-    SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+    SLAM->SaveTrajectoryEuRoC(f_file);
+    SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
   } else {
-    SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
-    SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+    SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
+    SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
   }
 
   return 0;
diff --git a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc
index f80baf26102..f473e82f97e 100644
--- a/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc
+++ b/Examples/Stereo-Inertial/stereo_inertial_tum_vi.cc
@@ -129,9 +129,17 @@ int main(int argc, char **argv) {
 
   // Create SLAM system. It initializes all system threads and gets ready to
   // process frames.
-  ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::IMU_STEREO, true,
-                         0, file_name);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::IMU_STEREO, true, file_name);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   double t_resize = 0.f;
   double t_track = 0.f;
@@ -152,31 +160,22 @@ int main(int argc, char **argv) {
 
       if (imageScale != 1.f) {
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
 #endif
         int width = imLeft.cols * imageScale;
         int height = imLeft.rows * imageScale;
         cv::resize(imLeft, imLeft, cv::Size(width, height));
         cv::resize(imRight, imRight, cv::Size(width, height));
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
+
         t_resize = std::chrono::duration_cast<
                        std::chrono::duration<double, std::milli> >(
                        t_End_Resize - t_Start_Resize)
                        .count();
-        SLAM.InsertResizeTime(t_resize);
+        SLAM->InsertResizeTime(t_resize);
 #endif
       }
 
@@ -215,31 +214,21 @@ int main(int argc, char **argv) {
       cout << "first imu time: " << fixed << vTimestampsImu[seq][0] << endl;
       cout << "size vImu: " << vImuMeas.size() << endl;*/
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
       // Pass the image to the SLAM system
-      SLAM.TrackStereo(imLeft, imRight, tframe, vImuMeas);
+      SLAM->TrackStereo(imLeft, imRight, tframe, vImuMeas);
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
 #ifdef REGISTER_TIMES
       t_track =
           t_resize + std::chrono::duration_cast<
                          std::chrono::duration<double, std::milli> >(t2 - t1)
                          .count();
-      SLAM.InsertTrackTime(t_track);
+      SLAM->InsertTrackTime(t_track);
 #endif
 
       double ttrack =
@@ -262,12 +251,12 @@ int main(int argc, char **argv) {
     if (seq < num_seq - 1) {
       cout << "Changing the dataset" << endl;
 
-      SLAM.ChangeDataset();
+      SLAM->ChangeDataset();
     }
   }
 
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Tracking time statistics
 
@@ -281,11 +270,11 @@ int main(int argc, char **argv) {
   if (bFileName) {
     const string kf_file = "kf_" + string(argv[argc - 1]) + ".txt";
     const string f_file = "f_" + string(argv[argc - 1]) + ".txt";
-    SLAM.SaveTrajectoryEuRoC(f_file);
-    SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+    SLAM->SaveTrajectoryEuRoC(f_file);
+    SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
   } else {
-    SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
-    SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+    SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
+    SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
   }
 
   sort(vTimesTrack.begin(), vTimesTrack.end());
diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc
index 4d5b8f2732d..a4631da1a70 100644
--- a/Examples/Stereo/stereo_euroc.cc
+++ b/Examples/Stereo/stereo_euroc.cc
@@ -95,7 +95,17 @@ int main(int argc, char **argv) {
 
   // 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::STEREO, true);
+  // Create SLAM system. It initializes all system threads and gets ready to
+  // process frames.
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::STEREO, true);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
 
   cv::Mat imLeft, imRight;
   for (seq = 0; seq < num_seq; seq++) {
@@ -128,32 +138,23 @@ int main(int argc, char **argv) {
 
       double tframe = vTimestampsCam[seq][ni];
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
       // Pass the images to the SLAM system
-      SLAM.TrackStereo(imLeft, imRight, tframe, vector<ORB_SLAM3::IMU::Point>(),
-                       vstrImageLeft[seq][ni]);
+      SLAM->TrackStereo(imLeft, imRight, tframe,
+                        vector<ORB_SLAM3::IMU::Point>(),
+                        vstrImageLeft[seq][ni]);
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
 #ifdef REGISTER_TIMES
       t_track = t_resize + t_rect +
                 std::chrono::duration_cast<
                     std::chrono::duration<double, std::milli> >(t2 - t1)
                     .count();
-      SLAM.InsertTrackTime(t_track);
+      SLAM->InsertTrackTime(t_track);
 #endif
 
       double ttrack =
@@ -175,21 +176,21 @@ int main(int argc, char **argv) {
     if (seq < num_seq - 1) {
       cout << "Changing the dataset" << endl;
 
-      SLAM.ChangeDataset();
+      SLAM->ChangeDataset();
     }
   }
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Save camera trajectory
   if (bFileName) {
     const string kf_file = "kf_" + string(argv[argc - 1]) + ".txt";
     const string f_file = "f_" + string(argv[argc - 1]) + ".txt";
-    SLAM.SaveTrajectoryEuRoC(f_file);
-    SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+    SLAM->SaveTrajectoryEuRoC(f_file);
+    SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
   } else {
-    SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
-    SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+    SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
+    SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
   }
 
   return 0;
diff --git a/Examples/Stereo/stereo_kitti.cc b/Examples/Stereo/stereo_kitti.cc
index fa5eb30d03b..69c026b4ff4 100644
--- a/Examples/Stereo/stereo_kitti.cc
+++ b/Examples/Stereo/stereo_kitti.cc
@@ -52,8 +52,17 @@ int main(int argc, char **argv) {
 
   // 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::STEREO, true);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::STEREO, true);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   // Vector for tracking time statistics
   vector<float> vTimesTrack;
@@ -84,58 +93,41 @@ int main(int argc, char **argv) {
 
     if (imageScale != 1.f) {
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
       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();
-#endif
 #endif
+
       int width = imLeft.cols * imageScale;
       int height = imLeft.rows * imageScale;
       cv::resize(imLeft, imLeft, cv::Size(width, height));
       cv::resize(imRight, imRight, cv::Size(width, height));
+
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
       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();
-#endif
+
       t_resize = std::chrono::duration_cast<
                      std::chrono::duration<double, std::milli> >(t_End_Resize -
                                                                  t_Start_Resize)
                      .count();
-      SLAM.InsertResizeTime(t_resize);
-#endif
+      SLAM->InsertResizeTime(t_resize);
     }
 
-#ifdef COMPILEDWITHC14
     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();
 #endif
 
     // Pass the images to the SLAM system
-    SLAM.TrackStereo(imLeft, imRight, tframe);
+    SLAM->TrackStereo(imLeft, imRight, tframe);
 
-#ifdef COMPILEDWITHC14
+#ifdef REGISTER_TIMES
     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();
-#endif
 
-#ifdef REGISTER_TIMES
     t_track =
         t_resize +
         std::chrono::duration_cast<std::chrono::duration<double, std::milli> >(
             t2 - t1)
             .count();
-    SLAM.InsertTrackTime(t_track);
+    SLAM->InsertTrackTime(t_track);
 #endif
 
     double ttrack =
@@ -155,7 +147,7 @@ int main(int argc, char **argv) {
   }
 
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Tracking time statistics
   sort(vTimesTrack.begin(), vTimesTrack.end());
@@ -168,7 +160,7 @@ int main(int argc, char **argv) {
   cout << "mean tracking time: " << totaltime / nImages << endl;
 
   // Save camera trajectory
-  SLAM.SaveTrajectoryKITTI("CameraTrajectory.txt");
+  SLAM->SaveTrajectoryKITTI("CameraTrajectory.txt");
 
   return 0;
 }
diff --git a/Examples/Stereo/stereo_tum_vi.cc b/Examples/Stereo/stereo_tum_vi.cc
index 767f7c78c74..cafe6c93730 100644
--- a/Examples/Stereo/stereo_tum_vi.cc
+++ b/Examples/Stereo/stereo_tum_vi.cc
@@ -97,8 +97,17 @@ int main(int argc, char **argv) {
 
   // 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::STEREO, true);
-  float imageScale = SLAM.GetImageScale();
+  auto exSLAM = ORB_SLAM3::SystemFactory::create(
+      argv[1], argv[2], ORB_SLAM3::SensorType::STEREO, true);
+
+  if (!exSLAM) {
+    cerr << "Failure to initialize ORBSLAM3: " << exSLAM.error().msg() << endl;
+    exit(-1);
+  }
+
+  auto SLAM = exSLAM.value();
+
+  float imageScale = SLAM->GetImageScale();
 
   cout << endl << "-------" << endl;
   cout.precision(17);
@@ -122,31 +131,22 @@ int main(int argc, char **argv) {
 
       if (imageScale != 1.f) {
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
 #endif
         int width = imLeft.cols * imageScale;
         int height = imLeft.rows * imageScale;
         cv::resize(imLeft, imLeft, cv::Size(width, height));
         cv::resize(imRight, imRight, cv::Size(width, height));
 #ifdef REGISTER_TIMES
-#ifdef COMPILEDWITHC14
         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();
-#endif
+
         t_resize = std::chrono::duration_cast<
                        std::chrono::duration<double, std::milli> >(
                        t_End_Resize - t_Start_Resize)
                        .count();
-        SLAM.InsertResizeTime(t_resize);
+        SLAM->InsertResizeTime(t_resize);
 #endif
       }
 
@@ -163,31 +163,21 @@ int main(int argc, char **argv) {
         return 1;
       }
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
       // Pass the image to the SLAM system
-      SLAM.TrackStereo(imLeft, imRight, tframe);
+      SLAM->TrackStereo(imLeft, imRight, tframe);
 
-#ifdef COMPILEDWITHC14
       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();
-#endif
 
 #ifdef REGISTER_TIMES
       t_track =
           t_resize + std::chrono::duration_cast<
                          std::chrono::duration<double, std::milli> >(t2 - t1)
                          .count();
-      SLAM.InsertTrackTime(t_track);
+      SLAM->InsertTrackTime(t_track);
 #endif
 
       double ttrack =
@@ -210,12 +200,12 @@ int main(int argc, char **argv) {
     if (seq < num_seq - 1) {
       cout << "Changing the dataset" << endl;
 
-      SLAM.ChangeDataset();
+      SLAM->ChangeDataset();
     }
   }
 
   // Stop all threads
-  SLAM.Shutdown();
+  SLAM->Shutdown();
 
   // Tracking time statistics
 
@@ -229,11 +219,11 @@ int main(int argc, char **argv) {
   if (bFileName) {
     const string kf_file = "kf_" + string(argv[argc - 1]) + ".txt";
     const string f_file = "f_" + string(argv[argc - 1]) + ".txt";
-    SLAM.SaveTrajectoryEuRoC(f_file);
-    SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+    SLAM->SaveTrajectoryEuRoC(f_file);
+    SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
   } else {
-    SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
-    SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+    SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
+    SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
   }
 
   sort(vTimesTrack.begin(), vTimesTrack.end());
diff --git a/README.md b/README.md
index bb0cd830c64..6534edb898d 100644
--- a/README.md
+++ b/README.md
@@ -3,18 +3,18 @@
 
 Relative to the original code, this repo contains multiple updates:
 
-* I removed the "ThirdParty" copies of "Sophus" and "g2o" in lieu of packages which can be installed "rosdep".  Due to API changes, this necessitated some syntactically invasive (but functionally equivalent) changes.
+* I removed the "ThirdParty" copies of "Sophus" and "g2o" in lieu of packages which can be installed "rosdep" (or `apt`).  Due to API changes, this necessitated some syntactically invasive (but functionally equivalent) changes.
 * This branch contains preliminary migration to [spdlog](https://github.com/gabime/spdlog) as a more controllable logging backend.  This is a slow-motion migration to better manage text output from ORBSLAM3.
 * As I dug further into the code, I got more opinionated.  I also added [pre-commit](.pre-commit-config.yaml), which introduced significant textual changes.   No going back!
 * [`Thirdparty/tl/`](Thirdparty/tl/) includes a copy of [TartanLlama's expected](https://github.com/TartanLlama/expected) which is released under the [CC0-1.0 (Public doamin) license](http://creativecommons.org/publicdomain/zero/1.0/)
 
 > [!WARNING]
-> I _am not_ testing this repo outside of ROS2.   I am *only* checking [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2) in a ROS2 / colcon environment.
+> I _am not_ testing this repo outside of ROS2.   I am *only* checking [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2) in a ROS2 / colcon environment.   I do not expect this to build with `cmake` nor do I expect the original `Examples/` to run.
 
 -----
 -----
 
-Author's original README follows below.
+The author's original README follows below.
 
 
 # ORB-SLAM3
@@ -79,13 +79,16 @@ We use the new thread and chrono functionalities of C++11.
 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**.
+We use [OpenCV](http://opencv.org) to manipulate images and features. Dowload and install instructions can be found at: http://opencv.org. **Required at least 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.
+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.
+
+**Modified g2o has been removed, use the system version instead.**
+
 
 ## Python
 Required to calculate the alignment of the trajectory with the ground truth. **Required Numpy module**.
@@ -96,7 +99,9 @@ Required to calculate the alignment of the trajectory with the ground truth. **R
 
 ## 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.
+~~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.~~
+
+ROS1 support has been removed.  See [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2) for ROS2 support.
 
 # 3. Building ORB-SLAM3 library and examples
 
diff --git a/include/Expected.h b/include/Expected.h
new file mode 100644
index 00000000000..ff181c252ca
--- /dev/null
+++ b/include/Expected.h
@@ -0,0 +1,56 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
+ * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+ * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
+ * University of Zaragoza.
+ *
+ * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
+ * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
+ * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <fmt/core.h>
+
+#include <string>
+#include <utility>
+
+#include "Thirdparty/tl/expected.hpp"
+
+namespace ORB_SLAM3 {
+
+class ExpectedError {
+ public:
+  ExpectedError() = delete;
+  ExpectedError(const ExpectedError &) = default;
+
+  explicit ExpectedError(const std::string &errmsg) : err_(errmsg) {}
+
+  // \todo{} I think this isn't very efficient, results in an extra copy?
+  //         Although it only happens in "error" conditions.
+  template <typename... Args>
+  static ExpectedError fmt(fmt::format_string<Args...> rt_fmt_str,
+                           Args &&...args) {
+    std::string str;
+    auto it = std::back_inserter(str);
+    fmt::format_to(it, rt_fmt_str, std::forward<Args>(args)...);
+    return ExpectedError(str);
+  }
+
+  const std::string msg() const { return err_; }
+
+  std::string err_;
+};
+
+}  // namespace ORB_SLAM3
diff --git a/include/Frame.h b/include/Frame.h
index 0ce8fde0a7c..04df683fa11 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -334,7 +334,7 @@ class Frame : public std::enable_shared_from_this<Frame> {
 
   bool mbImuPreintegrated;
 
-  std::mutex *mpMutexImu;
+  std::shared_ptr<std::mutex> mpMutexImu;
 
  public:
   std::shared_ptr<GeometricCamera> mpCamera, mpCamera2;
diff --git a/include/ImuTypes.h b/include/ImuTypes.h
index 6569f8d1398..0a4c696a973 100644
--- a/include/ImuTypes.h
+++ b/include/ImuTypes.h
@@ -214,7 +214,7 @@ class Preintegrated {
 
   void printMeasurements() const {
     std::cout << "pint meas:\n";
-    for (int i = 0; i < mvMeasurements.size(); i++) {
+    for (size_t i = 0; i < mvMeasurements.size(); i++) {
       std::cout << "meas " << mvMeasurements[i].t << std::endl;
     }
     std::cout << "end pint meas:\n";
diff --git a/include/KeyFrameDatabase.h b/include/KeyFrameDatabase.h
index 239f44e9425..3d4799cc766 100644
--- a/include/KeyFrameDatabase.h
+++ b/include/KeyFrameDatabase.h
@@ -77,7 +77,7 @@ class KeyFrameDatabase {
   void DetectNBestCandidates(const std::shared_ptr<KeyFrame> &pKF,
                              vector<std::shared_ptr<KeyFrame>> &vpLoopCand,
                              vector<std::shared_ptr<KeyFrame>> &vpMergeCand,
-                             int nNumCandidates);
+                             size_t nNumCandidates);
 
   // Relocalization
   std::vector<std::shared_ptr<KeyFrame>> DetectRelocalizationCandidates(
diff --git a/include/Map.h b/include/Map.h
index c4d20656f2c..e0a8fe775e2 100644
--- a/include/Map.h
+++ b/include/Map.h
@@ -199,9 +199,6 @@ class Map : public std::enable_shared_from_this<Map> {
   // Index related to a big change in the map (loop closure, global BA)
   int mnBigChangeIdx;
 
-  // View of the map in aerial sight (for the AtlasViewer)
-  GLubyte* mThumbnail;
-
   bool mIsInUse;
   bool mHasTumbnail;
   bool mbBad = false;
diff --git a/include/ORBmatcher.h b/include/ORBmatcher.h
index 2ac3c42a706..8b000265aff 100644
--- a/include/ORBmatcher.h
+++ b/include/ORBmatcher.h
@@ -133,7 +133,7 @@ class ORBmatcher {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  protected:
-  float RadiusByViewingCos(const float &viewCos);
+  [[nodiscard]] float RadiusByViewingCos(float viewCos) const;
 
   void ComputeThreeMaxima(std::vector<int> *histo, const int L, int &ind1,
                           int &ind2, int &ind3);
diff --git a/include/Settings.h b/include/Settings.h
index b743d81a5ca..12259fae247 100644
--- a/include/Settings.h
+++ b/include/Settings.h
@@ -23,7 +23,7 @@
 
 // Flag to activate the measurement of time in each process (track,localmap,
 // place recognition).
-// #define REGISTER_TIMES
+#define REGISTER_TIMES
 
 #include <unistd.h>
 
@@ -35,7 +35,7 @@
 #include <vector>
 
 #include "CameraModels/GeometricCamera.h"
-#include "Thirdparty/tl/expected.hpp"
+#include "Expected.h"
 #include "Types.h"
 
 namespace ORB_SLAM3 {
@@ -47,7 +47,7 @@ class Settings;
 
 class SettingsLoader {
  public:
-  typedef tl::expected<std::shared_ptr<Settings>, bool> Expected;
+  typedef tl::expected<std::shared_ptr<Settings>, ExpectedError> Expected;
   static Expected load(const std::string& configFile, const SensorType sensor);
 
   explicit SettingsLoader(const SensorType sensor);
diff --git a/include/System.h b/include/System.h
index 251eacd9ce8..f6853812a92 100644
--- a/include/System.h
+++ b/include/System.h
@@ -32,6 +32,7 @@
 #include <vector>
 
 #include "Atlas.h"
+#include "Expected.h"
 #include "FrameDrawer.h"
 #include "ImuTypes.h"
 #include "KeyFrameDatabase.h"
@@ -41,7 +42,6 @@
 #include "MapDrawer.h"
 #include "ORBVocabulary.h"
 #include "Settings.h"
-#include "Thirdparty/tl/expected.hpp"
 #include "Tracking.h"
 #include "Viewer.h"
 
@@ -58,18 +58,30 @@ class Settings;
 
 class SystemFactory {
  public:
-  typedef tl::expected<std::shared_ptr<System>, bool> Expected;
+  typedef tl::expected<std::shared_ptr<System>, ExpectedError> Expected;
 
-  static Expected create(const std::shared_ptr<Settings> &settings);
+  static Expected create(const std::shared_ptr<Settings> &settings,
+                         bool initFr = false,
+                         const string &strSequence = std::string());
 
+  static Expected create(const std::string &configFile, const SensorType sensor,
+                         bool initFr = false,
+                         const string &strSequence = std::string());
+
+  // Provided for compatibility with old API
   static Expected create(const std::string &configFile,
-                         const SensorType sensor);
+                         const std::string &vocabFile, const SensorType sensor,
+                         bool initFr = false,
+                         const string &strSequence = std::string());
 };
 
+// System should be created using  SystemFactory::create()
+//
+// It will validate settings and catch errors on startup
 class System : public std::enable_shared_from_this<System> {
  public:
   friend SystemFactory::Expected SystemFactory::create(
-      const std::shared_ptr<Settings> &settings);
+      const std::shared_ptr<Settings> &, bool, const string &);
 
   // File type
   enum FileType {
diff --git a/src/Frame.cc b/src/Frame.cc
index 44259678f39..146f5de95a8 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -265,7 +265,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
     mVw.setZero();
   }
 
-  mpMutexImu = new std::mutex();
+  mpMutexImu = std::make_shared<std::mutex>();
 
   // Set no stereo fisheye information
   Nleft = -1;
@@ -378,7 +378,7 @@ Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth,
     mVw.setZero();
   }
 
-  mpMutexImu = new std::mutex();
+  mpMutexImu = std::make_shared<std::mutex>();
 
   // Set no stereo fisheye information
   Nleft = -1;
@@ -508,7 +508,7 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
     mVw.setZero();
   }
 
-  mpMutexImu = new std::mutex();
+  mpMutexImu = std::make_shared<std::mutex>();
 }
 
 void Frame::AssignFeaturesToGrid() {
@@ -1238,7 +1238,7 @@ Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight,
 
   AssignFeaturesToGrid();
 
-  mpMutexImu = new std::mutex();
+  mpMutexImu = std::make_shared<std::mutex>();
 
   UndistortKeyPoints();
 }
diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc
index b298940344b..949c73d46f2 100644
--- a/src/G2oTypes.cc
+++ b/src/G2oTypes.cc
@@ -161,7 +161,7 @@ void ImuCamPose::SetParam(const std::vector<Eigen::Matrix3d>& _Rcw,
   Rcb.resize(num_cams);
   tcb.resize(num_cams);
 
-  for (int i = 0; i < tcb.size(); i++) {
+  for (size_t i = 0; i < tcb.size(); i++) {
     Rcb[i] = Rbc[i].transpose();
     tcb[i] = -Rcb[i] * tbc[i];
   }
@@ -212,7 +212,7 @@ void ImuCamPose::Update(const double* pu) {
   const Eigen::Matrix3d Rbw = Rwb.transpose();
   const Eigen::Vector3d tbw = -Rbw * twb;
 
-  for (int i = 0; i < pCamera.size(); i++) {
+  for (size_t i = 0; i < pCamera.size(); i++) {
     Rcw[i] = Rcb[i] * Rbw;
     tcw[i] = Rcb[i] * tbw + tcb[i];
   }
@@ -244,7 +244,7 @@ void ImuCamPose::UpdateW(const double* pu) {
   const Eigen::Matrix3d Rbw = Rwb.transpose();
   const Eigen::Vector3d tbw = -Rbw * twb;
 
-  for (int i = 0; i < pCamera.size(); i++) {
+  for (size_t i = 0; i < pCamera.size(); i++) {
     Rcw[i] = Rcb[i] * Rbw;
     tcw[i] = Rcb[i] * tbw + tcb[i];
   }
@@ -269,8 +269,8 @@ bool VertexPose::read(std::istream& is) {
   std::vector<Eigen::Matrix<double, 3, 3> > Rbc;
   std::vector<Eigen::Matrix<double, 3, 1> > tbc;
 
-  const int num_cams = _estimate.Rbc.size();
-  for (int idx = 0; idx < num_cams; idx++) {
+  const size_t num_cams = _estimate.Rbc.size();
+  for (size_t idx = 0; idx < num_cams; idx++) {
     for (int i = 0; i < 3; i++) {
       for (int j = 0; j < 3; j++) is >> Rcw[idx](i, j);
     }
diff --git a/src/KeyFrameDatabase.cc b/src/KeyFrameDatabase.cc
index 8197b198cf1..a1326517630 100644
--- a/src/KeyFrameDatabase.cc
+++ b/src/KeyFrameDatabase.cc
@@ -571,7 +571,7 @@ bool compFirst(const pair<float, std::shared_ptr<KeyFrame>>& a,
 void KeyFrameDatabase::DetectNBestCandidates(
     const std::shared_ptr<KeyFrame>& pKF,
     vector<std::shared_ptr<KeyFrame>>& vpLoopCand,
-    vector<std::shared_ptr<KeyFrame>>& vpMergeCand, int nNumCandidates) {
+    vector<std::shared_ptr<KeyFrame>>& vpMergeCand, size_t nNumCandidates) {
   list<std::shared_ptr<KeyFrame>> lKFsSharingWords;
   set<std::shared_ptr<KeyFrame>> spConnectedKF;
 
@@ -665,7 +665,7 @@ void KeyFrameDatabase::DetectNBestCandidates(
   vpLoopCand.reserve(nNumCandidates);
   vpMergeCand.reserve(nNumCandidates);
   set<std::shared_ptr<KeyFrame>> spAlreadyAddedKF;
-  int i = 0;
+  size_t i = 0;
   list<pair<float, std::shared_ptr<KeyFrame>>>::iterator it =
       lAccScoreAndMatch.begin();
   while (i < lAccScoreAndMatch.size() &&
diff --git a/src/Map.cc b/src/Map.cc
index 6fdcc569364..a85e00b5987 100644
--- a/src/Map.cc
+++ b/src/Map.cc
@@ -39,7 +39,6 @@ Map::Map()
       mbImuInitialized(false),
       mnMapChange(0),
       mpFirstRegionKF(nullptr),
-      mThumbnail(nullptr),
       mbFail(false),
       mIsInUse(false),
       mHasTumbnail(false),
@@ -60,7 +59,6 @@ Map::Map(int initKFid)
       mbBad(false),
       mbImuInitialized(false),
       mpFirstRegionKF(nullptr),
-      mThumbnail(nullptr),
       mnMapChange(0),
       mbFail(false),
       mnMapChangeNotified(0),
@@ -77,9 +75,6 @@ Map::~Map() {
   // TODO: erase all keyframes from memory
   mspKeyFrames.clear();
 
-  if (mThumbnail) delete mThumbnail;
-  mThumbnail = static_cast<GLubyte*>(NULL);
-
   mvpReferenceMapPoints.clear();
   mvpKeyFrameOrigins.clear();
 }
diff --git a/src/ORBmatcher.cc b/src/ORBmatcher.cc
index abbb3cb040f..85303e6647b 100644
--- a/src/ORBmatcher.cc
+++ b/src/ORBmatcher.cc
@@ -209,7 +209,7 @@ int ORBmatcher::SearchByProjection(const std::shared_ptr<Frame> &F,
   return nmatches;
 }
 
-float ORBmatcher::RadiusByViewingCos(const float &viewCos) {
+float ORBmatcher::RadiusByViewingCos(float viewCos) const {
   if (viewCos > 0.998)
     return 2.5;
   else
diff --git a/src/Optimizer.cc b/src/Optimizer.cc
index 0e46f662b91..6970d897059 100644
--- a/src/Optimizer.cc
+++ b/src/Optimizer.cc
@@ -31,6 +31,7 @@
 #include <mutex>
 #include <set>
 #include <tuple>
+#include <unordered_set>
 #include <unsupported/Eigen/MatrixFunctions>
 #include <utility>
 #include <vector>
@@ -65,10 +66,9 @@ void Optimizer::BundleAdjustment(const vector<std::shared_ptr<KeyFrame>>& vpKFs,
                                  const vector<MapPoint*>& vpMP, int nIterations,
                                  bool* pbStopFlag, const unsigned long nLoopKF,
                                  const bool bRobust) {
-  vector<bool> vbNotIncludedMP;
-  vbNotIncludedMP.resize(vpMP.size());
+  std::unordered_set<MapPoint*> vbNotIncludedMP;
 
-  std::shared_ptr<Map> pMap = vpKFs[0]->GetMap();
+  auto pMap = vpKFs[0]->GetMap();
 
   g2o::SparseOptimizer optimizer;
   // g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
@@ -123,7 +123,7 @@ void Optimizer::BundleAdjustment(const vector<std::shared_ptr<KeyFrame>>& vpKFs,
 
   // Set KeyFrame vertices
 
-  for (auto pKF : vpKFs) {
+  for (auto const& pKF : vpKFs) {
     if (pKF->isBad()) continue;
     g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
     Sophus::SE3<float> Tcw = pKF->GetPose();
@@ -139,8 +139,7 @@ void Optimizer::BundleAdjustment(const vector<std::shared_ptr<KeyFrame>>& vpKFs,
   const float thHuber3D = sqrt(7.815);
 
   // Set MapPoint vertices
-  for (size_t i = 0; i < vpMP.size(); i++) {
-    MapPoint* pMP = vpMP[i];
+  for (auto pMP : vpMP) {
     if (pMP->isBad()) continue;
     g2o::VertexPointXYZ* vPoint = new g2o::VertexPointXYZ();
     vPoint->setEstimate(pMP->GetWorldPos().cast<double>());
@@ -273,9 +272,7 @@ void Optimizer::BundleAdjustment(const vector<std::shared_ptr<KeyFrame>>& vpKFs,
 
     if (nEdges == 0) {
       optimizer.removeVertex(vPoint);
-      vbNotIncludedMP[i] = true;
-    } else {
-      vbNotIncludedMP[i] = false;
+      vbNotIncludedMP.insert(pMP);
     }
   }
 
@@ -304,7 +301,7 @@ void Optimizer::BundleAdjustment(const vector<std::shared_ptr<KeyFrame>>& vpKFs,
       Sophus::SE3f mTwc = pKF->GetPoseInverse();
       Sophus::SE3f mTcGBA_c = pKF->mTcwGBA * mTwc;
       Eigen::Vector3f vector_dist = mTcGBA_c.translation();
-      double dist = vector_dist.norm();
+      const double dist = vector_dist.norm();
       if (dist > 1) {
         int numMonoBadPoints = 0, numMonoOptPoints = 0;
         int numStereoBadPoints = 0, numStereoOptPoints = 0;
@@ -315,10 +312,7 @@ void Optimizer::BundleAdjustment(const vector<std::shared_ptr<KeyFrame>>& vpKFs,
           MapPoint* pMP = vpMapPointEdgeMono[i2];
           std::shared_ptr<KeyFrame> pKFedge = vpEdgeKFMono[i2];
 
-          if (pKF != pKFedge) {
-            continue;
-          }
-
+          if (pKF != pKFedge) continue;
           if (pMP->isBad()) continue;
 
           if (e->chi2() > 5.991 || !e->isDepthPositive()) {
@@ -335,10 +329,7 @@ void Optimizer::BundleAdjustment(const vector<std::shared_ptr<KeyFrame>>& vpKFs,
           MapPoint* pMP = vpMapPointEdgeStereo[i2];
           std::shared_ptr<KeyFrame> pKFedge = vpEdgeKFMono[i2];
 
-          if (pKF != pKFedge) {
-            continue;
-          }
-
+          if (pKF != pKFedge) continue;
           if (pMP->isBad()) continue;
 
           if (e->chi2() > 7.815 || !e->isDepthPositive()) {
@@ -353,10 +344,8 @@ void Optimizer::BundleAdjustment(const vector<std::shared_ptr<KeyFrame>>& vpKFs,
   }
 
   // Points
-  for (size_t i = 0; i < vpMP.size(); i++) {
-    if (vbNotIncludedMP[i]) continue;
-
-    MapPoint* pMP = vpMP[i];
+  for (auto pMP : vpMP) {
+    if (vbNotIncludedMP.find(pMP) != vbNotIncludedMP.end()) continue;
 
     if (pMP->isBad()) continue;
     g2o::VertexPointXYZ* vPoint = static_cast<g2o::VertexPointXYZ*>(
@@ -573,10 +562,9 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
 
   const unsigned long iniMPid = maxKFid * 5;
 
-  vector<bool> vbNotIncludedMP(vpMPs.size(), false);
+  std::unordered_set<MapPoint*> vbNotIncludedMP;
 
-  for (size_t i = 0; i < vpMPs.size(); i++) {
-    MapPoint* pMP = vpMPs[i];
+  for (auto pMP : vpMPs) {
     g2o::VertexPointXYZ* vPoint = new g2o::VertexPointXYZ();
     vPoint->setEstimate(pMP->GetWorldPos().cast<double>());
     unsigned long id = pMP->mnId + iniMPid + 1;
@@ -656,7 +644,7 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
 
         if (pKFi->mpCamera2) {
           // Monocular right observation
-          int rightIndex = get<1>(vObs);
+          size_t rightIndex = get<1>(vObs);
 
           if (rightIndex != -1 && rightIndex < pKFi->mvKeysRight.size()) {
             rightIndex -= pKFi->NLeft;
@@ -692,7 +680,7 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
 
     if (bAllFixed) {
       optimizer.removeVertex(vPoint);
-      vbNotIncludedMP[i] = true;
+      vbNotIncludedMP.insert(pMP);
     }
   }
 
@@ -748,10 +736,9 @@ void Optimizer::FullInertialBA(const std::shared_ptr<Map>& pMap, int its,
   }
 
   // Points
-  for (size_t i = 0; i < vpMPs.size(); i++) {
-    if (vbNotIncludedMP[i]) continue;
+  for (auto pMP : vpMPs) {
+    if (vbNotIncludedMP.find(pMP) != vbNotIncludedMP.end()) continue;
 
-    MapPoint* pMP = vpMPs[i];
     g2o::VertexPointXYZ* vPoint = static_cast<g2o::VertexPointXYZ*>(
         optimizer.vertex(pMP->mnId + iniMPid + 1));
 
@@ -1093,12 +1080,10 @@ void Optimizer::LocalBundleAdjustment(const shared_ptr<KeyFrame>& pKF,
     vector<MapPoint*> vpMPs = pKFi->GetMapPointMatches();
 
     for (auto pMP : vpMPs) {
-      if (pMP) {
-        if (!pMP->isBad() && pMP->GetMap() == pCurrentMap) {
-          if (pMP->mnBALocalForKF != pKF->mnId) {
-            lLocalMapPoints.push_back(pMP);
-            pMP->mnBALocalForKF = pKF->mnId;
-          }
+      if (pMP && !pMP->isBad() && pMP->GetMap() == pCurrentMap) {
+        if (pMP->mnBALocalForKF != pKF->mnId) {
+          lLocalMapPoints.push_back(pMP);
+          pMP->mnBALocalForKF = pKF->mnId;
         }
       }
     }
@@ -1228,10 +1213,7 @@ void Optimizer::LocalBundleAdjustment(const shared_ptr<KeyFrame>& pKF,
 
   int nEdges = 0;
 
-  for (list<MapPoint*>::iterator lit = lLocalMapPoints.begin(),
-                                 lend = lLocalMapPoints.end();
-       lit != lend; lit++) {
-    MapPoint* pMP = *lit;
+  for (auto pMP : lLocalMapPoints) {
     g2o::VertexPointXYZ* vPoint = new g2o::VertexPointXYZ();
     vPoint->setEstimate(pMP->GetWorldPos().cast<double>());
     int id = pMP->mnId + maxKFid + 1;
@@ -1517,22 +1499,15 @@ void Optimizer::OptimizeEssentialGraph(
 
   // Set Loop edges
   int count_loop = 0;
-  for (map<shared_ptr<KeyFrame>, set<shared_ptr<KeyFrame>>>::const_iterator
-           mit = LoopConnections.begin(),
-           mend = LoopConnections.end();
-       mit != mend; mit++) {
-    shared_ptr<KeyFrame> pKF = mit->first;
+  for (auto const& [pKF, spConnections] : LoopConnections) {
     const long unsigned int nIDi = pKF->mnId;
-    const set<shared_ptr<KeyFrame>>& spConnections = mit->second;
     const g2o::Sim3 Siw = vScw[nIDi];
     const g2o::Sim3 Swi = Siw.inverse();
 
-    for (set<shared_ptr<KeyFrame>>::const_iterator sit = spConnections.begin(),
-                                                   send = spConnections.end();
-         sit != send; sit++) {
-      const long unsigned int nIDj = (*sit)->mnId;
+    for (auto const& pConnection : spConnections) {
+      auto const nIDj = pConnection->mnId;
       if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) &&
-          pKF->GetWeight(*sit) < minFeat)
+          pKF->GetWeight(pConnection) < minFeat)
         continue;
 
       const g2o::Sim3 Sjw = vScw[nIDj];
@@ -1624,10 +1599,8 @@ void Optimizer::OptimizeEssentialGraph(
     // Covisibility graph edges
     const vector<shared_ptr<KeyFrame>> vpConnectedKFs =
         pKF->GetCovisiblesByWeight(minFeat);
-    for (vector<shared_ptr<KeyFrame>>::const_iterator vit =
-             vpConnectedKFs.begin();
-         vit != vpConnectedKFs.end(); vit++) {
-      shared_ptr<KeyFrame> pKFn = *vit;
+
+    for (auto pKFn : vpConnectedKFs) {
       if (pKFn && pKFn != pParentKF &&
           !pKF->hasChild(pKFn) /*&& !sLoopEdges.count(pKFn)*/) {
         if (!pKFn->isBad() && pKFn->mnId < pKF->mnId) {
@@ -1704,9 +1677,7 @@ void Optimizer::OptimizeEssentialGraph(
 
   // Correct points. Transform to "non-optimized" reference keyframe pose and
   // transform back with optimized pose
-  for (size_t i = 0, iend = vpMPs.size(); i < iend; i++) {
-    MapPoint* pMP = vpMPs[i];
-
+  for (auto pMP : vpMPs) {
     if (pMP->isBad()) continue;
 
     int nIDr;
@@ -1786,7 +1757,7 @@ void Optimizer::OptimizeEssentialGraph(
 
   const int minFeat = 100;
 
-  for (auto pKFi : vpFixedKFs) {
+  for (auto const& pKFi : vpFixedKFs) {
     if (pKFi->isBad()) continue;
 
     g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
@@ -1816,7 +1787,7 @@ void Optimizer::OptimizeEssentialGraph(
                      Verbose::VERBOSITY_DEBUG);
 
   set<unsigned long> sIdKF;
-  for (auto pKFi : vpFixedCorrectedKFs) {
+  for (auto const& pKFi : vpFixedCorrectedKFs) {
     if (pKFi->isBad()) continue;
 
     g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
@@ -1848,7 +1819,7 @@ void Optimizer::OptimizeEssentialGraph(
     vpBadPose[nIDi] = true;
   }
 
-  for (auto pKFi : vpNonFixedKFs) {
+  for (auto const& pKFi : vpNonFixedKFs) {
     if (pKFi->isBad()) continue;
 
     const int nIDi = pKFi->mnId;
@@ -1891,7 +1862,7 @@ void Optimizer::OptimizeEssentialGraph(
   const Eigen::Matrix<double, 7, 7> matLambda =
       Eigen::Matrix<double, 7, 7>::Identity();
 
-  for (auto pKFi : vpKFs) {
+  for (auto const& pKFi : vpKFs) {
     int num_connections = 0;
     const int nIDi = pKFi->mnId;
 
@@ -1935,8 +1906,8 @@ void Optimizer::OptimizeEssentialGraph(
     }
 
     // Loop edges
-    const set<shared_ptr<KeyFrame>> sLoopEdges = pKFi->GetLoopEdges();
-    for (auto pLKF : sLoopEdges) {
+    const set<std::shared_ptr<KeyFrame>> sLoopEdges = pKFi->GetLoopEdges();
+    for (auto const& pLKF : sLoopEdges) {
       if (spKFs.find(pLKF) != spKFs.end() && pLKF->mnId < pKFi->mnId) {
         g2o::Sim3 Slw;
         bool bHasRelation = false;
@@ -2032,7 +2003,7 @@ void Optimizer::OptimizeEssentialGraph(
 
   // Correct points. Transform to "non-optimized" reference keyframe pose and
   // transform back with optimized pose
-  for (MapPoint* pMPi : vpNonCorrectedMPs) {
+  for (auto pMPi : vpNonCorrectedMPs) {
     if (pMPi->isBad()) continue;
 
     auto pRefKF = pMPi->GetReferenceKeyFrame();
@@ -2361,21 +2332,16 @@ void Optimizer::LocalInertialBA(const shared_ptr<KeyFrame>& pKF,
     }
   }
 
-  int N = vpOptimizableKFs.size();
-
   // Optimizable points seen by temporal optimizable keyframes
   list<MapPoint*> lLocalMapPoints;
-  for (int i = 0; i < N; i++) {
-    vector<MapPoint*> vpMPs = vpOptimizableKFs[i]->GetMapPointMatches();
-    for (vector<MapPoint*>::iterator vit = vpMPs.begin(), vend = vpMPs.end();
-         vit != vend; vit++) {
-      MapPoint* pMP = *vit;
-      if (pMP) {
-        if (!pMP->isBad()) {
-          if (pMP->mnBALocalForKF != pKF->mnId) {
-            lLocalMapPoints.push_back(pMP);
-            pMP->mnBALocalForKF = pKF->mnId;
-          }
+  for (auto const& pKF : vpOptimizableKFs) {
+    vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
+
+    for (auto pMP : vpMPs) {
+      if (pMP && !pMP->isBad()) {
+        if (pMP->mnBALocalForKF != pKF->mnId) {
+          lLocalMapPoints.push_back(pMP);
+          pMP->mnBALocalForKF = pKF->mnId;
         }
       }
     }
@@ -2395,10 +2361,9 @@ void Optimizer::LocalInertialBA(const shared_ptr<KeyFrame>& pKF,
 
   // Optimizable visual KFs
   const int maxCovKF = 0;
-  for (int i = 0, iend = vpNeighsKFs.size(); i < iend; i++) {
+  for (auto const& pKFi : vpNeighsKFs) {
     if (lpOptVisKFs.size() >= maxCovKF) break;
 
-    shared_ptr<KeyFrame> pKFi = vpNeighsKFs[i];
     if (pKFi->mnBALocalForKF == pKF->mnId || pKFi->mnBAFixedForKF == pKF->mnId)
       continue;
     pKFi->mnBALocalForKF = pKF->mnId;
@@ -2406,15 +2371,11 @@ void Optimizer::LocalInertialBA(const shared_ptr<KeyFrame>& pKF,
       lpOptVisKFs.push_back(pKFi);
 
       vector<MapPoint*> vpMPs = pKFi->GetMapPointMatches();
-      for (vector<MapPoint*>::iterator vit = vpMPs.begin(), vend = vpMPs.end();
-           vit != vend; vit++) {
-        MapPoint* pMP = *vit;
-        if (pMP) {
-          if (!pMP->isBad()) {
-            if (pMP->mnBALocalForKF != pKF->mnId) {
-              lLocalMapPoints.push_back(pMP);
-              pMP->mnBALocalForKF = pKF->mnId;
-            }
+      for (auto pMP : vpMPs) {
+        if (pMP && !pMP->isBad()) {
+          if (pMP->mnBALocalForKF != pKF->mnId) {
+            lLocalMapPoints.push_back(pMP);
+            pMP->mnBALocalForKF = pKF->mnId;
           }
         }
       }
@@ -2447,7 +2408,7 @@ void Optimizer::LocalInertialBA(const shared_ptr<KeyFrame>& pKF,
     if (lFixedKeyFrames.size() >= maxFixKF) break;
   }
 
-  bool bNonFixed = (lFixedKeyFrames.size() == 0);
+  // bool bNonFixed = (lFixedKeyFrames.size() == 0);
 
   // Setup optimizer
   g2o::SparseOptimizer optimizer;
@@ -2481,7 +2442,7 @@ void Optimizer::LocalInertialBA(const shared_ptr<KeyFrame>& pKF,
   }
 
   // Set Local temporal KeyFrame vertices
-  N = vpOptimizableKFs.size();
+  // N = vpOptimizableKFs.size();
   for (auto pKFi : vpOptimizableKFs) {
     VertexPose* VP = new VertexPose(pKFi);
     VP->setId(pKFi->mnId);
@@ -2538,6 +2499,9 @@ void Optimizer::LocalInertialBA(const shared_ptr<KeyFrame>& pKF,
   }
 
   // Create intertial constraints
+
+  int N = vpOptimizableKFs.size();
+
   vector<EdgeInertial*> vei(N, nullptr);
   vector<EdgeGyroRW*> vegr(N, nullptr);
   vector<EdgeAccRW*> vear(N, nullptr);
@@ -3137,7 +3101,6 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
   Rwg = VGDir->estimate().Rwg;
 
   // Keyframes velocities and biases
-  const int N = vpKFs.size();
   for (auto pKFi : vpKFs) {
     if (pKFi->mnId > maxKFid) continue;
 
@@ -3297,7 +3260,6 @@ void Optimizer::InertialOptimization(const std::shared_ptr<Map>& pMap,
   IMU::Bias b(vb[3], vb[4], vb[5], vb[0], vb[1], vb[2]);
 
   // Keyframes velocities and biases
-  const int N = vpKFs.size();
   for (auto pKFi : vpKFs) {
     if (pKFi->mnId > maxKFid) continue;
 
@@ -3575,21 +3537,18 @@ void Optimizer::LocalBundleAdjustment(const shared_ptr<KeyFrame>& pMainKF,
         pMPi->GetObservations();
     int nEdges = 0;
     // SET EDGES
-    for (map<shared_ptr<KeyFrame>, tuple<int, int>>::const_iterator mit =
-             observations.begin();
-         mit != observations.end(); mit++) {
-      shared_ptr<KeyFrame> pKF = mit->first;
+    for (auto const& [pKF, tObs] : observations) {
       if (pKF->isBad() || pKF->mnId > maxKFid ||
           pKF->mnBALocalForMerge != pMainKF->mnId ||
-          !pKF->GetMapPoint(get<0>(mit->second)))
+          !pKF->GetMapPoint(get<0>(tObs)))
         continue;
 
       nEdges++;
 
-      const cv::KeyPoint& kpUn = pKF->mvKeysUn[get<0>(mit->second)];
+      const cv::KeyPoint& kpUn = pKF->mvKeysUn[get<0>(tObs)];
 
       // Monocular
-      if (pKF->mvuRight[get<0>(mit->second)] < 0) {
+      if (pKF->mvuRight[get<0>(tObs)] < 0) {
         mpObsMPs[pMPi]++;
         Eigen::Matrix<double, 2, 1> obs;
         obs << kpUn.pt.x, kpUn.pt.y;
@@ -3621,7 +3580,7 @@ void Optimizer::LocalBundleAdjustment(const shared_ptr<KeyFrame>& pMainKF,
         // RGBD or Stereo
         mpObsMPs[pMPi] += 2;
         Eigen::Matrix<double, 3, 1> obs;
-        const float kp_ur = pKF->mvuRight[get<0>(mit->second)];
+        const float kp_ur = pKF->mvuRight[get<0>(tObs)];
         obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
 
         g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
@@ -3656,16 +3615,14 @@ void Optimizer::LocalBundleAdjustment(const shared_ptr<KeyFrame>& pMainKF,
     }
   }
 
-  if (pbStopFlag)
-    if (*pbStopFlag) return;
+  if (pbStopFlag && *pbStopFlag) return;
 
   optimizer.initializeOptimization();
   optimizer.optimize(5);
 
   bool bDoMore = true;
 
-  if (pbStopFlag)
-    if (*pbStopFlag) bDoMore = false;
+  if (pbStopFlag && *pbStopFlag) bDoMore = false;
 
   map<unsigned long int, int> mWrongObsKF;
   if (bDoMore) {
@@ -5261,24 +5218,15 @@ void Optimizer::OptimizeEssentialGraph4DoF(
   matLambda(0, 0) = 1e3;
 
   // Set Loop edges
-  Edge4DoF* e_loop;
-  for (map<std::shared_ptr<KeyFrame>,
-           set<std::shared_ptr<KeyFrame>>>::const_iterator
-           mit = LoopConnections.begin(),
-           mend = LoopConnections.end();
-       mit != mend; mit++) {
-    std::shared_ptr<KeyFrame> pKF = mit->first;
+  // Edge4DoF* e_loop;
+  for (auto const& [pKF, spConnections] : LoopConnections) {
     const long unsigned int nIDi = pKF->mnId;
-    const set<std::shared_ptr<KeyFrame>>& spConnections = mit->second;
     const g2o::Sim3 Siw = vScw[nIDi];
 
-    for (set<std::shared_ptr<KeyFrame>>::const_iterator
-             sit = spConnections.begin(),
-             send = spConnections.end();
-         sit != send; sit++) {
-      const long unsigned int nIDj = (*sit)->mnId;
+    for (auto pConnection : spConnections) {
+      const long unsigned int nIDj = pConnection->mnId;
       if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) &&
-          pKF->GetWeight(*sit) < minFeat)
+          pKF->GetWeight(pConnection) < minFeat)
         continue;
 
       const g2o::Sim3 Sjw = vScw[nIDj];
@@ -5295,7 +5243,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
                           optimizer.vertex(nIDi)));
 
       e->information() = matLambda;
-      e_loop = e;
+      // e_loop = e;
       optimizer.addEdge(e);
 
       sInsertedEdges.insert(make_pair(min(nIDi, nIDj), max(nIDi, nIDj)));
@@ -5303,7 +5251,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
   }
 
   // 1. Set normal edges
-  for (auto pKF : vpKFs) {
+  for (auto const& pKF : vpKFs) {
     const int nIDi = pKF->mnId;
 
     g2o::Sim3 Siw;
@@ -5348,7 +5296,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
     }
 
     // 1.1.1 Inertial edges
-    std::shared_ptr<KeyFrame> prevKF = pKF->mPrevKF;
+    auto const& prevKF = pKF->mPrevKF;
     if (prevKF) {
       int nIDj = prevKF->mnId;
 
@@ -5378,7 +5326,7 @@ void Optimizer::OptimizeEssentialGraph4DoF(
     }
 
     // 1.2 Loop edges
-    const auto sLoopEdges = pKF->GetLoopEdges();
+    auto const& sLoopEdges = pKF->GetLoopEdges();
     for (auto pLKF : sLoopEdges) {
       if (pLKF->mnId < pKF->mnId) {
         g2o::Sim3 Swl;
@@ -5410,10 +5358,8 @@ void Optimizer::OptimizeEssentialGraph4DoF(
     // 1.3 Covisibility graph edges
     const vector<std::shared_ptr<KeyFrame>> vpConnectedKFs =
         pKF->GetCovisiblesByWeight(minFeat);
-    for (vector<std::shared_ptr<KeyFrame>>::const_iterator vit =
-             vpConnectedKFs.begin();
-         vit != vpConnectedKFs.end(); vit++) {
-      std::shared_ptr<KeyFrame> pKFn = *vit;
+
+    for (auto const& pKFn : vpConnectedKFs) {
       if (pKFn && pKFn != pParentKF && pKFn != prevKF && pKFn != pKF->mNextKF &&
           !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) {
         if (!pKFn->isBad() && pKFn->mnId < pKF->mnId) {
@@ -5471,15 +5417,11 @@ void Optimizer::OptimizeEssentialGraph4DoF(
 
   // Correct points. Transform to "non-optimized" reference keyframe pose and
   // transform back with optimized pose
-  for (size_t i = 0, iend = vpMPs.size(); i < iend; i++) {
-    MapPoint* pMP = vpMPs[i];
-
+  for (auto const& pMP : vpMPs) {
     if (pMP->isBad()) continue;
 
-    int nIDr;
-
     std::shared_ptr<KeyFrame> pRefKF = pMP->GetReferenceKeyFrame();
-    nIDr = pRefKF->mnId;
+    const int nIDr = pRefKF->mnId;
 
     g2o::Sim3 Srw = vScw[nIDr];
     g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr];
diff --git a/src/SettingsLoader.cc b/src/SettingsLoader.cc
index 7b4fa9057c3..752b3e5de0c 100644
--- a/src/SettingsLoader.cc
+++ b/src/SettingsLoader.cc
@@ -149,7 +149,8 @@ SettingsLoader::Expected SettingsLoader::load(const std::string& configFile) {
          << endl;
     cerr << "Aborting..." << endl;
 
-    exit(-1);
+    return tl::make_unexpected(
+        ExpectedError::fmt("Unable to open configuration file {}", configFile));
   } else {
     spdlog::info("Loading settings from {}", configFile);
   }
@@ -197,7 +198,7 @@ SettingsLoader::Expected SettingsLoader::load(const std::string& configFile) {
   if (settings_->validate()) {
     return settings_;
   } else {
-    return tl::make_unexpected(false);
+    return tl::unexpected<ExpectedError>("Setting did not validate");
   }
 }
 
diff --git a/src/System.cc b/src/System.cc
index 3b14b8331bb..9b38905ce96 100644
--- a/src/System.cc
+++ b/src/System.cc
@@ -49,31 +49,51 @@ namespace ORB_SLAM3 {
 Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL;
 
 SystemFactory::Expected SystemFactory::create(
-    const std::shared_ptr<Settings> &settings) {
+    const std::shared_ptr<Settings> &settings, bool initFr,
+    const string &strSequence) {
   if (!settings->validate()) {
-    return tl::make_unexpected(false);
+    return tl::make_unexpected(ExpectedError::fmt("Settings do not validate"));
   }
 
   // Cannot use make_shared with friend constructors?
-  auto sys = std::shared_ptr<System>(new System(settings));
+  auto sys = std::shared_ptr<System>(new System(settings, initFr, strSequence));
 
   // Initialization must occur separately because we use shared_from_this
   if (!sys->initialize()) {
-    return tl::make_unexpected(false);
+    return tl::make_unexpected(
+        ExpectedError::fmt("Unable to initialize SLAM system"));
   }
 
   return sys;
 }
 
 SystemFactory::Expected SystemFactory::create(const std::string &configFile,
-                                              const SensorType sensor) {
-  auto settings = SettingsLoader::load(configFile, sensor);
+                                              const SensorType sensor,
+                                              bool initFr,
+                                              const string &strSequence) {
+  auto exSettings = SettingsLoader::load(configFile, sensor);
 
-  if (settings) {
-    return SystemFactory::create(settings.value());
+  if (!exSettings) {
+    return tl::make_unexpected(ExpectedError::fmt("Enable to load settings"));
   }
 
-  return tl::make_unexpected(false);
+  return SystemFactory::create(exSettings.value(), initFr, strSequence);
+}
+
+SystemFactory::Expected SystemFactory::create(const std::string &configFile,
+                                              const std::string &vocabFile,
+                                              const SensorType sensor,
+                                              bool initFr,
+                                              const string &strSequence) {
+  auto exSettings = SettingsLoader::load(configFile, sensor);
+
+  if (!exSettings) {
+    return tl::make_unexpected(ExpectedError::fmt("Enable to load settings"));
+  }
+
+  auto settings = exSettings.value();
+  settings->strVocFile_ = vocabFile;
+  return SystemFactory::create(settings, initFr, strSequence);
 }
 
 //===================================================================
@@ -609,7 +629,7 @@ void System::SaveTrajectoryEuRoC(const string &filename) {
   }*/
 
   vector<std::shared_ptr<Map>> vpMaps = mpAtlas->GetAllMaps();
-  int numMaxKFs = 0;
+  size_t numMaxKFs = 0;
   std::shared_ptr<Map> pBiggerMap;
   std::cout << "There are " << std::to_string(vpMaps.size())
             << " maps in the atlas" << std::endl;
diff --git a/src/Tracking.cc b/src/Tracking.cc
index 52152f36d8c..0220f79c66b 100644
--- a/src/Tracking.cc
+++ b/src/Tracking.cc
@@ -203,7 +203,7 @@ void Tracking::TrackStats2File() {
        "preint[ms], Pose pred[ms], LM track[ms], KF dec[ms], Total[ms]"
     << endl;
 
-  for (int i = 0; i < vdTrackTotal_ms.size(); ++i) {
+  for (size_t i = 0; i < vdTrackTotal_ms.size(); ++i) {
     double stereo_rect = 0.0;
     if (!vdRectStereo_ms.empty()) {
       stereo_rect = vdRectStereo_ms[i];
@@ -1298,11 +1298,13 @@ void Tracking::Track() {
       if (bOK && !mbVO) bOK = TrackLocalMap();
     }
 
+#ifdef REGISTER_TIMES
     spdlog::info(
         "[Tracking::Track] TrackLocalMap  {} ms ",
         std::chrono::duration_cast<std::chrono::duration<double, std::milli>>(
             std::chrono::steady_clock::now() - time_StartLMTrack)
             .count());
+#endif
 
     if (bOK) {
       mState = OK;
@@ -2644,13 +2646,11 @@ void Tracking::UpdateLocalKeyFrames() {
 
   // All keyframes that observe a map point are included in the local map. Also
   // check which keyframe shares most points
-  for (auto const& it : keyframeCounter) {
-    std::shared_ptr<KeyFrame> pKF = it.first;
-
+  for (auto const& [pKF, count] : keyframeCounter) {
     if (pKF->isBad()) continue;
 
-    if (it.second > max) {
-      max = it.second;
+    if (count > max) {
+      max = count;
       pKFmax = pKF;
     }
 
@@ -3057,7 +3057,7 @@ void Tracking::UpdateFrameIMU(
     const float s, const IMU::Bias& b,
     const std::shared_ptr<KeyFrame>& pCurrentKeyFrame) {
   std::shared_ptr<Map> pMap = pCurrentKeyFrame->GetMap();
-  unsigned int index = mnFirstFrameId;
+  // unsigned int index = mnFirstFrameId;
   list<std::shared_ptr<KeyFrame>>::iterator lRit = mlpReferences.begin();
   list<bool>::iterator lbL = mlbLost.begin();
 

From 9088681641a7c18fde385c5c53bec6b5dfaa492e Mon Sep 17 00:00:00 2001
From: Aaron Marburg <amarburg@uw.edu>
Date: Tue, 6 Jan 2026 22:13:03 +0000
Subject: [PATCH 8/9] Replacee cout with spdlog, explicit destructor for Frame.

---
 include/Frame.h    | 7 ++++---
 include/KeyFrame.h | 4 ++--
 include/Settings.h | 2 +-
 src/Frame.cc       | 2 ++
 4 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/include/Frame.h b/include/Frame.h
index 04df683fa11..85a9026a99f 100644
--- a/include/Frame.h
+++ b/include/Frame.h
@@ -31,6 +31,7 @@
 #include "Converter.h"
 #include "Eigen/Core"
 #include "ImuTypes.h"
+#include "Logging.h"
 #include "ORBVocabulary.h"
 #include "Settings.h"
 #include "Thirdparty/DBoW2/DBoW2/BowVector.h"
@@ -84,7 +85,7 @@ class Frame : public std::enable_shared_from_this<Frame> {
         const IMU::Calib &ImuCalib = IMU::Calib());
 
   // Destructor
-  // ~Frame();
+  ~Frame();
 
   // Extract ORB on the image. 0 for left image and 1 for right image.
   void ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1);
@@ -388,8 +389,8 @@ class Frame : public std::enable_shared_from_this<Frame> {
           right++;
       }
     }
-    cout << "Point distribution in Frame: left-> " << left << " --- right-> "
-         << right << endl;
+    spdlog::debug("Point distribution in Frame: left-> {} --- right-> {}", left,
+                  right);
   }
 
   Sophus::SE3<double> T_test;
diff --git a/include/KeyFrame.h b/include/KeyFrame.h
index fff23f193f6..287d7db4b28 100644
--- a/include/KeyFrame.h
+++ b/include/KeyFrame.h
@@ -544,8 +544,8 @@ class KeyFrame : public std::enable_shared_from_this<KeyFrame> {
           right++;
       }
     }
-    cout << "Point distribution in KeyFrame: left-> " << left << " --- right-> "
-         << right << endl;
+    spdlog::debug("Point distribution in KeyFrame: left-> {} --- right-> {}",
+                  left, right);
   }
 };
 
diff --git a/include/Settings.h b/include/Settings.h
index 12259fae247..e61c47f6ee3 100644
--- a/include/Settings.h
+++ b/include/Settings.h
@@ -23,7 +23,7 @@
 
 // Flag to activate the measurement of time in each process (track,localmap,
 // place recognition).
-#define REGISTER_TIMES
+// #define REGISTER_TIMES
 
 #include <unistd.h>
 
diff --git a/src/Frame.cc b/src/Frame.cc
index 146f5de95a8..b9bf06ffcf5 100644
--- a/src/Frame.cc
+++ b/src/Frame.cc
@@ -511,6 +511,8 @@ Frame::Frame(const cv::Mat &imGray, const double &timeStamp,
   mpMutexImu = std::make_shared<std::mutex>();
 }
 
+Frame::~Frame() {}
+
 void Frame::AssignFeaturesToGrid() {
   // Fill matrix with points
   const int nCells = FRAME_GRID_COLS * FRAME_GRID_ROWS;

From d91fa16da7bd68e2f503323428ef5f26d3b878aa Mon Sep 17 00:00:00 2001
From: Aaron Marburg <amarburg@uw.edu>
Date: Wed, 7 Jan 2026 15:31:13 -0800
Subject: [PATCH 9/9] Add a CI build in Github workflow, transition to vcpkg
 (#5)

Implements an actual CI build in a Github workflow.   Though it started out innocently enough, dependency resolution quickly got out of hand (*cough* Pangolin).  This led to a wholesale conversion to `vcpkg` for standalone, non-ROS builds, both in and out of CI.

README updated to match.
---
 .clang-format                       |  1 +
 .github/workflows/cmake_ci.yaml     | 73 +++++++++++++++++++++++++++++
 .gitignore                          |  1 +
 .gitmodules                         |  3 ++
 CMakeLists.txt                      | 11 +++--
 CMakePresets.json                   | 59 +++++++++++++++++++++++
 README.md                           | 40 ++++++++++++----
 build.sh                            | 43 +++--------------
 build_ros.sh                        |  7 ---
 install_apt_dependencies.sh         | 41 ++++++++++++++++
 src/Settings.cc                     |  2 +-
 vcpkg                               |  1 +
 vcpkg.json                          | 21 +++++++++
 vcpkg_overlay/ffmpeg/portfile.cmake |  1 +
 vcpkg_overlay/ffmpeg/vcpkg.json     | 24 ++++++++++
 15 files changed, 271 insertions(+), 57 deletions(-)
 create mode 100644 .clang-format
 create mode 100644 .github/workflows/cmake_ci.yaml
 create mode 100644 .gitmodules
 create mode 100644 CMakePresets.json
 delete mode 100755 build_ros.sh
 create mode 100755 install_apt_dependencies.sh
 create mode 160000 vcpkg
 create mode 100644 vcpkg.json
 create mode 100644 vcpkg_overlay/ffmpeg/portfile.cmake
 create mode 100644 vcpkg_overlay/ffmpeg/vcpkg.json

diff --git a/.clang-format b/.clang-format
new file mode 100644
index 00000000000..f6cb8ad931f
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1 @@
+BasedOnStyle: Google
diff --git a/.github/workflows/cmake_ci.yaml b/.github/workflows/cmake_ci.yaml
new file mode 100644
index 00000000000..7d3e51d3d3d
--- /dev/null
+++ b/.github/workflows/cmake_ci.yaml
@@ -0,0 +1,73 @@
+name: Build
+
+on:
+  pull_request:
+  push:
+    branches:
+      - master
+  workflow_dispatch:
+
+jobs:
+  build-project:
+    name: Build Project
+    runs-on: ubuntu-24.04
+    steps:
+      - name: Checkout Project
+        uses: actions/checkout@v4.2.2
+        with:
+          submodules: true
+
+      # Load the set of APT dependencies from the install_apt_dependencies scripts
+      # to reduce repetition
+      - name: Generate dependency list
+        id: generate_deps
+        run: |
+              # Run your script and capture its output into a shell variable
+              SCRIPT_OUTPUT=$(./install_apt_dependencies.sh --deps)
+
+              # Write the variable to the GITHUB_OUTPUT file
+              echo "apt_deps=$SCRIPT_OUTPUT" >> "$GITHUB_OUTPUT"
+
+      - name: Install apt dependencies
+        uses: awalsh128/cache-apt-pkgs-action@latest
+        with:
+          packages: ${{ steps.generate_deps.outputs.apt_deps }}
+          version: 1.0
+
+      # I think this necessary to ensure alternatives are set up (for libblas, etc)
+      # It's not properly set up when restoring packages from cache
+      - name: Force install libblas and liblapack
+        run: sudo apt-get install --reinstall libblas-dev liblapack-dev
+        shell: bash
+
+      # Based on sample workflow from https://github.com/lukka/CppCMakeVcpkgTemplate/blob/v11/.github/workflows/hosted-ninja-vcpkg_submod.yml
+      - uses: lukka/get-cmake@latest
+
+      #
+      - name: Cache vcpkg
+        uses: actions/cache@v4
+        with:
+          path: ${{ github.workspace }}/vcpkg_cache
+          key: vcpkg-${{ matrix.triplet }}-${{ hashFiles('vcpkg.json', 'vcpkg_overlay/**', 'CMakeLists.txt', '**/CMakeLists.txt', 'CMakePresets.json') }}
+
+      - name: Restore from cache and setup vcpkg executable and data files.
+        uses: lukka/run-vcpkg@v11
+        with:
+          doNotCache: false
+          vcpkgGitCommitId: e3db8f65d2414c301c29a8467c6aee94e3ba09fc
+
+      # Note: if the preset misses the "configuration", it is possible to explicitly select the
+      # configuration with the additional `--config` flag, e.g.:
+      #   buildPreset: 'ninja-vcpkg'
+      #   buildPresetAdditionalArgs: "[`--config`, `Release`]"
+      #   testPreset: 'ninja-vcpkg'
+      #   testPresetAdditionalArgs: "[`--config`, `Release`]"
+      - name: Run CMake+vcpkg+Ninja+CTest to build packages and generate/build/test the code.
+        uses: lukka/run-cmake@v10
+        env:
+          VCPKG_BINARY_SOURCES: clear;files,${{ github.workspace }}/vcpkg_cache,readwrite
+          VCPKG_DEFAULT_TRIPLET: ${{ matrix.triplet }}
+        with:
+          configurePreset: 'ninja-multi-vcpkg'
+          buildPreset: 'ninja-vcpkg-release'
+          testPreset: 'test-release'
diff --git a/.gitignore b/.gitignore
index 7227515c0a3..907b1aa6044 100644
--- a/.gitignore
+++ b/.gitignore
@@ -66,6 +66,7 @@ Thirdparty/g2o/config.h
 Thirdparty/g2o/lib/
 Vocabulary/ORBvoc.txt
 build/
+builds/
 
 lib/
 
diff --git a/.gitmodules b/.gitmodules
new file mode 100644
index 00000000000..a0a57f3d70f
--- /dev/null
+++ b/.gitmodules
@@ -0,0 +1,3 @@
+[submodule "vcpkg"]
+	path = vcpkg
+	url = https://github.com/microsoft/vcpkg.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
index a03a584b5f2..73fe275306f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,14 +25,16 @@ find_package(OpenCV 4.4)
 MESSAGE("OPENCV VERSION:")
 MESSAGE(${OpenCV_VERSION})
 
-find_package(Eigen3 3.1.0 REQUIRED)
-find_package(Pangolin REQUIRED)
-find_package(Sophus REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Pangolin CONFIG REQUIRED)
+find_package(Sophus CONFIG REQUIRED)
 find_package(g2o REQUIRED)
 find_package(fmt REQUIRED)
 find_package(spdlog REQUIRED)
 find_package(Boost REQUIRED COMPONENTS serialization)
 
+add_subdirectory(Thirdparty/DBoW2)
+
 include_directories(
         ${PROJECT_SOURCE_DIR}
         ${PROJECT_SOURCE_DIR}/include
@@ -73,13 +75,14 @@ add_library(${PROJECT_NAME} SHARED
         src/TwoViewReconstruction.cc
         src/Viewer.cc
 )
+target_compile_definitions(${PROJECT_NAME} PUBLIC REGISTER_TIMES)
 
 target_link_libraries(${PROJECT_NAME}
         ${OpenCV_LIBS}
         Eigen3::Eigen
         Sophus::Sophus
         ${Pangolin_LIBRARIES}
-        ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
+        DBoW2
         fmt::fmt
         g2o::core
         g2o::types_sim3
diff --git a/CMakePresets.json b/CMakePresets.json
new file mode 100644
index 00000000000..db9b6595a43
--- /dev/null
+++ b/CMakePresets.json
@@ -0,0 +1,59 @@
+{
+  "version": 8,
+  "cmakeMinimumRequired": {
+    "major": 3,
+    "minor": 21,
+    "patch": 0
+  },
+  "configurePresets": [
+    {
+      "name": "ninja-multi-vcpkg",
+      "displayName": "Ninja Multi-Config",
+      "description": "Configure with vcpkg toolchain and generate Ninja project files for all configurations",
+      "binaryDir": "${sourceDir}/builds/${presetName}",
+      "generator": "Ninja Multi-Config",
+      "toolchainFile": "${sourceDir}/vcpkg/scripts/buildsystems/vcpkg.cmake"
+    }
+  ],
+  "buildPresets": [
+    {
+      "name": "ninja-vcpkg-debug",
+      "configurePreset": "ninja-multi-vcpkg",
+      "displayName": "Build (Debug)",
+      "description": "Build with Ninja/vcpkg (Debug)",
+      "configuration": "Debug"
+    },
+    {
+      "name": "ninja-vcpkg-release",
+      "configurePreset": "ninja-multi-vcpkg",
+      "displayName": "Build (Release)",
+      "description": "Build with Ninja/vcpkg (Release)",
+      "configuration": "Release"
+    }
+  ],
+  "testPresets": [
+    {
+      "name": "test-ninja-vcpkg",
+      "configurePreset": "ninja-multi-vcpkg",
+      "hidden": true
+    },
+    {
+      "name": "test-debug",
+      "description": "Test (Debug)",
+      "displayName": "Test (Debug)",
+      "configuration": "Debug",
+      "inherits": [
+        "test-ninja-vcpkg"
+      ]
+    },
+    {
+      "name": "test-release",
+      "description": "Test (Release)",
+      "displayName": "Test (Release)",
+      "configuration": "Release",
+      "inherits": [
+        "test-ninja-vcpkg"
+      ]
+    }
+  ]
+}
diff --git a/README.md b/README.md
index 6534edb898d..934f6932f88 100644
--- a/README.md
+++ b/README.md
@@ -1,15 +1,39 @@
 > [!NOTE]
-> This is my personal "working" fork of ORBSLAM3, which focuses on integrating ORBSLAM3 into ROS2.   The actual ROS2 integration is implemented in [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2).
+> This is my personal "working" fork of ORBSLAM3, which is part of a larger effort to integrate ORBSLAM3 into ROS2.   This repo remains (distantly) related to the author's original upstream repo, and contains no ROS2-specific code.   The actual ROS2 integration is implemented in [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2) which includes this repo as a submodule.
 
-Relative to the original code, this repo contains multiple updates:
+As I dug further into the code, I got more opinionated and have made multiple stylistic changes.   My intention is to make only positive readability, portability and performance improvements but YMMV:
 
-* I removed the "ThirdParty" copies of "Sophus" and "g2o" in lieu of packages which can be installed "rosdep" (or `apt`).  Due to API changes, this necessitated some syntactically invasive (but functionally equivalent) changes.
-* This branch contains preliminary migration to [spdlog](https://github.com/gabime/spdlog) as a more controllable logging backend.  This is a slow-motion migration to better manage text output from ORBSLAM3.
-* As I dug further into the code, I got more opinionated.  I also added [pre-commit](.pre-commit-config.yaml), which introduced significant textual changes.   No going back!
-* [`Thirdparty/tl/`](Thirdparty/tl/) includes a copy of [TartanLlama's expected](https://github.com/TartanLlama/expected) which is released under the [CC0-1.0 (Public doamin) license](http://creativecommons.org/publicdomain/zero/1.0/)
+* Started modernization, currently to C++17
+* Replace bare pointers with managed pointers in most cases.
+* Minor updates to the System and Setting initialization procedure, primarily to separate creation of the Settings (from a file or otherwise) from the initialization of System, and provide more paths to catching and reporting errors during initialization.  See the [Examples/](Examples/).
+* Added [pre-commit](.pre-commit-config.yaml), which introduced significant textual changes.
+* Cleanup on dependencies:
+   * Removed built-in `g2o` and `Sophus` sources, get these from a dependency manager (vcpkg for non-ROS, and rosdep for ROS)
+   * Add [TartanLlama's expected](https://github.com/TartanLlama/expected) which is released under the [CC0-1.0 (Public doamin) license](http://creativecommons.org/publicdomain/zero/1.0/)  (this may be remove if/when I standardize on C++20)
+* I am only targetting Ubuntu 24.04 right now.   I've updated the build process as follows:
+   * When building for ROS2, use [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2) which includes this repo as a submodule.   Dependencies (g2o, Sophus, Pangolin) are included from ROS apt via rosdep.
+   * For non-ROS builds, I am now using `vcpkg` as a dependency manager as it can build the non-apt-gettable dependencies (Pangolin).   **However** I am using overlays to preferentially use apt versions of packages whenever feasible (ffmpeg, etc).
+   * In some cases (`g2o` and its dependencies) we use vcpkg's version to ensure dependencies stay in sycn.
+* Other minor changes:
+   * Removed integrated Realsense support.    Realsense-enabled binaries should go in a separate package.
 
-> [!WARNING]
-> I _am not_ testing this repo outside of ROS2.   I am *only* checking [orbslam3_ros2](https://gitlab.com/apl-ocean-engineering/orbslam3_ros2) in a ROS2 / colcon environment.   I do not expect this to build with `cmake` nor do I expect the original `Examples/` to run.
+
+## Building
+
+I am only testing on Ubuntu 24.04.
+
+As noted above, I prefer to use system packages as much as possible and use `vcpkg` for dependencies with no published binaries (outside of ROS).  To override this behavior and have vcpkg build additional packages from source, remove the relevant directories from the [`vcpkg_overlays/`](vcpkg_overlays/) directory.
+
+I've gone full koolaid and adopted `ninja` as a builder as well.
+
+To build in Ubuntu, use the convenience scripts:
+
+```
+./install_apt_dependencies.sh
+./build.sh
+```
+
+This will build Release versions the ORB_SLAM3 library and all of the `Examples`.
 
 -----
 -----
diff --git a/build.sh b/build.sh
index 87f9c4bf653..aa18f2fe39b 100755
--- a/build.sh
+++ b/build.sh
@@ -1,40 +1,9 @@
-echo "Configuring and building Thirdparty/DBoW2 ..."
+# Options are "release" and "debug" (see CMakePresets.json)
+BUILD_TYPE=${BUILD_TYPE:-release}
 
-cd Thirdparty/DBoW2
-mkdir build
-cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j2
-
-cd ../../g2o
-
-echo "Configuring and building Thirdparty/g2o ..."
-
-mkdir build
-cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j2
-
-cd ../../Sophus
-
-echo "Configuring and building Thirdparty/Sophus ..."
-
-mkdir build
-cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j2
-
-cd ../../../
-
-echo "Uncompress vocabulary ..."
-
-cd Vocabulary
-tar -xf ORBvoc.txt.tar.gz
+git submodule sync vcpkg
+cd vcpkg && ./bootstrap-vcpkg.sh
 cd ..
 
-echo "Configuring and building ORB_SLAM3 ..."
-
-mkdir build
-cd build
-cmake .. -DCMAKE_BUILD_TYPE=Release
-make -j2
+cmake --preset ninja-multi-vcpkg
+cmake --build --preset ninja-vcpkg-${BUILD_TYPE}
diff --git a/build_ros.sh b/build_ros.sh
deleted file mode 100755
index 1f13d2155fc..00000000000
--- a/build_ros.sh
+++ /dev/null
@@ -1,7 +0,0 @@
-echo "Building ROS nodes"
-
-cd Examples/ROS/ORB_SLAM3
-mkdir build
-cd build
-cmake .. -DROS_BUILD_TYPE=Release
-make -j
diff --git a/install_apt_dependencies.sh b/install_apt_dependencies.sh
new file mode 100755
index 00000000000..c5d94ddc453
--- /dev/null
+++ b/install_apt_dependencies.sh
@@ -0,0 +1,41 @@
+#!/usr/bin/bash
+
+APT_DEPENDENCIES="cmake \
+                  g++ \
+                  libavcodec-dev \
+                  libavdevice-dev \
+                  libavfilter-dev \
+                  libavformat-dev \
+                  libavutil-dev \
+                  libblas-dev \
+                  libboost-serialization-dev \
+                  libc++-dev \
+                  libegl1-mesa-dev \
+                  libeigen3-dev \
+                  libepoxy-dev \
+                  libfmt-dev \
+                  libgl1-mesa-dev \
+                  libgles2-mesa-dev \
+                  libglew-dev \
+                  liblapack-dev \
+                  libopencv-dev \
+                  libspdlog-dev \
+                  libswresample-dev \
+                  libswscale-dev \
+                  libwayland-dev \
+                  libx11-dev \
+                  libxkbcommon-dev \
+                  nasm \
+                  ninja-build \
+                  wayland-protocols"
+
+
+myarg=$1
+if [[ "$myarg" = "--deps" ]]; then
+    echo $APT_DEPENDENCIES
+    exit 0
+fi
+
+
+sudo apt-get update && \
+sudo apt-get install --no-install-recommends -y $APT_DEPENDENCIES
diff --git a/src/Settings.cc b/src/Settings.cc
index 736aa162f97..e88b5cc0e8f 100644
--- a/src/Settings.cc
+++ b/src/Settings.cc
@@ -117,7 +117,7 @@ void Settings::setMonoCamera(CameraType type, const std::vector<float>& k,
     //       vOverlapping;
     // }
   } else {
-    spdlog::error("Error: {} not known", type);
+    spdlog::error("Error: {} not known", static_cast<int>(type));
     exit(-1);
   }
 }
diff --git a/vcpkg b/vcpkg
new file mode 160000
index 00000000000..2cf2bcc60ad
--- /dev/null
+++ b/vcpkg
@@ -0,0 +1 @@
+Subproject commit 2cf2bcc60add50f79b2c418487d9cd1b6c7c1fec
diff --git a/vcpkg.json b/vcpkg.json
new file mode 100644
index 00000000000..bced0c493ce
--- /dev/null
+++ b/vcpkg.json
@@ -0,0 +1,21 @@
+{
+  "name": "main",
+  "version-string": "latest",
+  "configuration": {
+    "default-registry":
+        {
+          "kind": "git",
+          "baseline": "e3db8f65d2414c301c29a8467c6aee94e3ba09fc",
+          "repository": "https://github.com/Microsoft/vcpkg"
+        },
+    "overlay-ports": [
+      "vcpkg_overlay"
+    ]
+  },
+  "dependencies": [
+    "pangolin",
+    "sophus",
+    "g2o",
+    "eigen3"
+  ]
+}
diff --git a/vcpkg_overlay/ffmpeg/portfile.cmake b/vcpkg_overlay/ffmpeg/portfile.cmake
new file mode 100644
index 00000000000..065116c276a
--- /dev/null
+++ b/vcpkg_overlay/ffmpeg/portfile.cmake
@@ -0,0 +1 @@
+set(VCPKG_POLICY_EMPTY_PACKAGE enabled)
diff --git a/vcpkg_overlay/ffmpeg/vcpkg.json b/vcpkg_overlay/ffmpeg/vcpkg.json
new file mode 100644
index 00000000000..91b34b7c2c5
--- /dev/null
+++ b/vcpkg_overlay/ffmpeg/vcpkg.json
@@ -0,0 +1,24 @@
+{
+  "name": "ffmpeg",
+  "version": "6.1.1",
+  "features": {
+    "avcodec": {
+      "description": ""
+    },
+    "avdevice": {
+      "description": ""
+    },
+    "avfilter": {
+      "description": ""
+    },
+    "avformat": {
+      "description": ""
+    },
+    "swresample": {
+      "description": ""
+    },
+    "swscale": {
+      "description": ""
+    }
+  }
+}