diff --git a/CMakeLists.txt b/CMakeLists.txt
index b0e32133f73896f6e2917ec60561308ce939e62b..32b54ef41250966e28423027639c47a1dd5eb428 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 1)
diff --git a/include/gnss/factor/factor_gnss_tdcp_3d.h b/include/gnss/factor/factor_gnss_tdcp_3d.h
index bec9a1a3084445150db4007321e93d8e780a4c56..c352aeebe7f7a7e4b8f1f98e4e21faa342d8350c 100644
--- a/include/gnss/factor/factor_gnss_tdcp_3d.h
+++ b/include/gnss/factor/factor_gnss_tdcp_3d.h
@@ -66,7 +66,7 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
- assert(_ftr_ptr->getMeasurement().size() == 3 && "FactorGnssTdcp3d uses 3d measurements. For FeatureGnssTdcp with also delta clock, use FactorGnssTdcpBatch instead");
+ assert(_ftr_ptr->getMeasurement().size() == 3 && "FactorGnssTdcp3d uses 3d measurements. For FeatureGnssTdcp with also delta clock, use FactorGnssTdcpWithClock instead");
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
}
diff --git a/include/gnss/factor/factor_gnss_tdcp_batch.h b/include/gnss/factor/factor_gnss_tdcp_with_clock.h
similarity index 91%
rename from include/gnss/factor/factor_gnss_tdcp_batch.h
rename to include/gnss/factor/factor_gnss_tdcp_with_clock.h
index b13b3b13df18d8121cea955654388cdc93bcb94a..566e7912a2689a21bfae499eb4d6ff3645dbd54b 100644
--- a/include/gnss/factor/factor_gnss_tdcp_batch.h
+++ b/include/gnss/factor/factor_gnss_tdcp_with_clock.h
@@ -20,8 +20,7 @@
//
//--------LICENSE_END--------
-#ifndef FACTOR_GNSS_TDCP_BATCH_H_
-#define FACTOR_GNSS_TDCP_BATCH_H_
+#pragma once
//Wolf includes
#include "core/common/wolf.h"
@@ -30,22 +29,22 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorGnssTdcpBatch);
+WOLF_PTR_TYPEDEFS(FactorGnssTdcpWithClock);
-class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>
+class FactorGnssTdcpWithClock : public FactorAutodiff<FactorGnssTdcpWithClock, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
public:
- FactorGnssTdcpBatch(const FeatureBasePtr& _ftr_ptr,
+ FactorGnssTdcpWithClock(const FeatureBasePtr& _ftr_ptr,
const CaptureBasePtr& _capture_other_ptr,
const SensorGnssPtr& _sensor_gnss_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>("FactorGnssTdcpBatch",
+ FactorAutodiff<FactorGnssTdcpWithClock, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>("FactorGnssTdcpWithClock",
TOP_GEOM,
_ftr_ptr,
_capture_other_ptr->getFrame(),
@@ -67,11 +66,11 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
- assert(_ftr_ptr->getMeasurement().size() == 4 && "FactorGnssTdcpBatch uses 4d measurements (pos.displacement, delta clock). For FeatureGnssTdcp with only displacement, use FactorGnssTdcp3d instead");
+ assert(_ftr_ptr->getMeasurement().size() == 4 && "FactorGnssTdcpWithClock uses 4d measurements (pos.displacement, delta clock). For FeatureGnssTdcp with only displacement, use FactorGnssTdcp3d instead");
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
}
- ~FactorGnssTdcpBatch() override = default;
+ ~FactorGnssTdcpWithClock() override = default;
template<typename T>
bool operator ()(const T* const _x1,
@@ -89,7 +88,7 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
};
template<typename T>
-inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
+inline bool FactorGnssTdcpWithClock::operator ()(const T* const _x1,
const T* const _o1,
const T* const _t1,
const T* const _x2,
@@ -135,5 +134,3 @@ inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
}
} // namespace wolf
-
-#endif
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index fade0dc4b3cb8420021d8532b6f95b7b2c0cd9dc..01c6dccfe8785a48a9982e767a50e6acda1d4bdd 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -245,15 +245,16 @@ void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
// Store the first capture that established a factor (for later initialization ENU-MAP)
if (first_frame_state_.empty() and
- not sensor_gnss_->isEnuMapFixed())
+ not sensor_gnss_->isEnuMapFixed() and
+ feature->getCapture() and feature->getCapture()->getFrame())
{
- first_frame_state_ = feature->getCapture()->getFrame()->getState();
+ first_frame_state_ = feature->getCapture()->getFrame()->getState("PO");
first_pos_ = feature->getMeasurement().head<3>();
}
// Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized (and not fixed) and far enough
if (params_gnss_->init_enu_map and
- not first_frame_state_.empty() and
+ first_frame_state_.includesStructure("PO") and
sensor_gnss_->isEnuDefined() and
not sensor_gnss_->isEnuMapInitialized() and
not sensor_gnss_->isEnuMapFixed() and
@@ -319,18 +320,17 @@ bool ProcessorGnssFix::voteForKeyFrame() const
sensor_gnss_->isEnuDefined() and
not sensor_gnss_->isEnuMapInitialized() and
not sensor_gnss_->isEnuMapFixed() and
- (first_pos_-incoming_feature_->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min and
- (first_pos_-incoming_feature_->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
+ (first_pos_-incoming_feature_->getMeasurement().head<3>()).norm() > params_gnss_->enu_map_init_dist_min and
+ (first_pos_-incoming_feature_->getMeasurement().head<3>()).norm() < params_gnss_->enu_map_init_dist_max)
{
WOLF_DEBUG("KF because of enu map not initialized");
return true;
}
// Distance criterion (ENU defined and ENU-MAP initialized)
- if (last_KF_capture_ != nullptr and
- (incoming_feature_->getMeasurement() - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
+ if ((incoming_feature_->getMeasurement().head<3>() - last_KF_feature_->getMeasurement().head<3>()).norm() > params_gnss_->dist_traveled)
{
- WOLF_DEBUG("KF because of distance criterion: ", (incoming_feature_->getMeasurement() - last_KF_feature_->getMeasurement()).norm());
+ WOLF_DEBUG("KF because of distance criterion: ", (incoming_feature_->getMeasurement().head<3>() - last_KF_feature_->getMeasurement().head<3>()).norm());
return true;
}
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 4dddad23146d7b3759890b6f14cdd2de09f0c878..6dc911cae5f8776b884f302a30e3aa638f55dcf0 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -25,7 +25,7 @@
#include "gnss/feature/feature_gnss_tdcp.h"
#include "gnss/feature/feature_gnss_fix.h"
#include "gnss/factor/factor_gnss_tdcp.h"
-#include "gnss/factor/factor_gnss_tdcp_batch.h"
+#include "gnss/factor/factor_gnss_tdcp_with_clock.h"
#include "gnss/factor/factor_gnss_pseudo_range.h"
#include "gnss/factor/factor_gnss_fix_3d.h"
#include "gnss_utils/utils/rcv_position.h"
@@ -339,7 +339,7 @@ void ProcessorTrackerGnss::establishFactors()
// initialize frame state with antenna position in map coordinates
// (since we don't have orientation for removing extrinsics)
if (params_tracker_gnss_->init_frames and fix_last_.success)
- last_ptr_->getFrame()->getP()->setState(sensor_gnss_->getQEnuMap().conjugate() * (sensor_gnss_->getQEnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
+ last_frame_ptr_->getP()->setState(sensor_gnss_->getQEnuMap().conjugate() * (sensor_gnss_->getQEnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
FactorBasePtrList new_factors;
@@ -408,7 +408,7 @@ void ProcessorTrackerGnss::establishFactors()
// Displacement factor from batch TDCP (FactorGnssTdcp3d)
if (params_tracker_gnss_->tdcp_params.batch)
{
- WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
+ WOLF_DEBUG("TDCP BATCH frame ", last_frame_ptr_->id());
FactorBasePtr last_fac_ptr = nullptr;
auto frame_map = getProblem()->getTrajectory()->getFrameMap();
@@ -420,7 +420,7 @@ void ProcessorTrackerGnss::establishFactors()
WOLF_DEBUG("TDCP BATCH ref frame ", ref_KF->id());
// discard non-key frames, last-last pair and frames without CaptureGnss
- if (ref_KF == last_ptr_->getFrame() or
+ if (ref_KF == last_frame_ptr_ or
ref_KF->getCaptureOf(getSensor(),"CaptureGnss") == nullptr)
continue;
@@ -494,7 +494,7 @@ void ProcessorTrackerGnss::establishFactors()
tdcp_output.cov_d);
// EMPLACE FACTOR
- last_fac_ptr = FactorBase::emplace<FactorGnssTdcpBatch>(ftr,
+ last_fac_ptr = FactorBase::emplace<FactorGnssTdcpWithClock>(ftr,
ftr,
ref_cap_gnss,
sensor_gnss_,
diff --git a/src/tree_manager/tree_manager_sliding_window_tdcp.cpp b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
index 1a4800a82b62dfa56a509f90b84ea57126ab3a76..7b1347cc947a889d72cad2562c399252e0e42570 100644
--- a/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
@@ -50,7 +50,7 @@ void TreeManagerSlidingWindowTdcp::keyFrameCallback(FrameBasePtr _key_frame)
(fac->getType() == "FactorGnssTdcp" or
fac->getType() == "FactorGnssTdcp2d" or
fac->getType() == "FactorGnssTdcp3d" or
- fac->getType() == "FactorGnssTdcpBatch"))
+ fac->getType() == "FactorGnssTdcpWithClock"))
fac->setStatus(FAC_ACTIVE);
}
}
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 6209d341bcd32308bd522ab1990eb0cc82692012..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} gtest_main ${PLUGIN_NAME})
- endif()
+ target_link_libraries(${target} gtest_main ${PLUGIN_NAME})
add_test(NAME ${target} COMMAND ${target})
endfunction()