diff --git a/CMakeLists.txt b/CMakeLists.txt
index 27b8fc49322f322af861f32c2b0d595f6cc16147..b434e686863e4ec473eaa501968dc9d56eb09fc6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.16)
 
 # MAC OSX RPATH
 SET(CMAKE_MACOSX_RPATH true)
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index 6950995f5a2de3a9c438f69960cb3f7f460b8bf4..22454b9bc77d8717deb9503b5fa39374fd5b0993 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -434,7 +434,7 @@ void ProcessorVisualOdometry::establishFactors()
         LandmarkBasePtr associated_lmk = nullptr;
         for (auto lmk: getProblem()->getMap()->getLandmarkList())
         {
-            if (lmk->id() == feat_pi->trackId()){
+            if (lmk->trackId() == feat_pi->trackId()){
                 associated_lmk = lmk;
             }
         }
@@ -444,11 +444,14 @@ void ProcessorVisualOdometry::establishFactors()
         if (associated_lmk)
         {
             LandmarkHpPtr associated_lmk_hp = std::dynamic_pointer_cast<LandmarkHp>(associated_lmk);
-            FactorBase::emplace<FactorPixelHp>(feat_pi,
-                                               feat_pi,
-                                               associated_lmk_hp,
-                                               shared_from_this(),
-                                               params_visual_odometry_->apply_loss_function);
+            
+            if (associated_lmk_hp) {
+                FactorBase::emplace<FactorPixelHp>(feat_pi,
+                                                feat_pi,
+                                                associated_lmk_hp,
+                                                shared_from_this(),
+                                                params_visual_odometry_->apply_loss_function);
+            }
         }
 
         // 2) create landmark if track is not associated with one and has enough length
@@ -456,7 +459,6 @@ void ProcessorVisualOdometry::establishFactors()
         {
             // std::cout << "NEW valid track \\o/" << std::endl;
             LandmarkBasePtr lmk = emplaceLandmark(feat_pi);
-            lmk->setId(feat_pi->trackId());
 
             // Add factors from all KFs of this track to the new lmk
             Track track_kf = track_matrix_.trackAtKeyframes(feat->trackId());
@@ -513,8 +515,8 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
 
     // Set all IDs equal to track ID
     size_t track_id = _feat->trackId();
-    lmk_hp_ptr->setId(track_id);
-    _feat->setLandmarkId(track_id);
+    lmk_hp_ptr->setTrackId(track_id);
+    _feat->setLandmarkId(lmk_hp_ptr->id());
 
     return lmk_hp_ptr;
 }
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 2cae2569bf53ce0052f496edfe0ce56e99f4bd14..9f9ee1dde0f2ca8afa670c1182945a933cf9cdc0 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,73 +1,15 @@
-if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
-  message("CMake version less than 3.11.0")
+include(FetchContent)
 
-  # Enable ExternalProject CMake module
-  include(ExternalProject)
+FetchContent_Declare(
+  googletest
+  GIT_REPOSITORY https://github.com/google/googletest.git 
+  GIT_TAG main)
 
-  set(GTEST_FORCE_SHARED_CRT ON)
-  set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
+SET(INSTALL_GTEST OFF) # Disable installation of googletest
+FetchContent_MakeAvailable(googletest)
 
-  # Download GoogleTest
-  ExternalProject_Add(googletest
-      GIT_REPOSITORY https://github.com/google/googletest.git
-      GIT_TAG        v1.8.x
-      # TIMEOUT 1 # We'll try this
-      CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
-      -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
-      -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
-      -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
-      -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
-      -DBUILD_GTEST=ON
-      PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
-      # Disable install step
-      INSTALL_COMMAND ""
-      UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-  )
-
-  # Get GTest source and binary directories from CMake project
-
-  # Specify include dir
-  ExternalProject_Get_Property(googletest source_dir)
-  set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-
-  # Specify MainTest's link libraries
-  ExternalProject_Get_Property(googletest binary_dir)
-  set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-
-  # Create a libgtest target to be used as a dependency by test programs
-  add_library(libgtest IMPORTED STATIC GLOBAL)
-  add_dependencies(libgtest googletest)
-
-  # Set libgtest properties
-  set_target_properties(libgtest PROPERTIES
-      "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
-      "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-  )
-
-else()
-
-  message("CMake version equal or greater than 3.11.0")
-
-  include(FetchContent)
-
-  FetchContent_Declare(
-    googletest
-    GIT_REPOSITORY https://github.com/google/googletest.git 
-    GIT_TAG main)
-
-  SET(INSTALL_GTEST OFF) # Disable installation of googletest
-  FetchContent_MakeAvailable(googletest)
-    
-endif()
-  
 function(wolf_add_gtest target)
   add_executable(${target} ${ARGN})
-  if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
-    add_dependencies(${target} libgtest)
-    target_link_libraries(${target} libgtest ${PLUGIN_NAME})
-    target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
-  else()
-    target_link_libraries(${target} PUBLIC gtest_main ${PLUGIN_NAME})
-  endif()
+  target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
   add_test(NAME ${target} COMMAND ${target})
 endfunction()