From 59839d6d539cc042273db634ad8ed8d33f76e62c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 17 Jan 2022 13:07:58 +0100
Subject: [PATCH] After 2nd RAL submission
---
.gitignore | 37 +-
.gitlab-ci.yml | 219 +++--
.vscode/c_cpp_properties.json | 20 +
CMakeLists.txt | 210 ++---
LICENSE | 619 +++++++++++++
README.md | 2 +-
ROS-based code style eclipse indented 4sp.xml | 167 ----
ROS-based wolf code style QT indented 4sp.xml | 39 -
cmake_modules/wolfgnssConfig.cmake | 21 +-
codetemplates eclipse.xml | 76 --
include/gnss/capture/capture_gnss.h | 80 ++
include/gnss/capture/capture_gnss_fix.h | 63 +-
.../gnss/capture/capture_gnss_single_diff.h | 63 --
include/gnss/capture/capture_gnss_tdcp.h | 96 ++
...tor_gnss_fix_2D.h => factor_gnss_fix_2d.h} | 77 +-
...tor_gnss_fix_3D.h => factor_gnss_fix_3d.h} | 73 +-
.../gnss/factor/factor_gnss_pseudo_range.h | 190 ++++
.../gnss/factor/factor_gnss_single_diff_2d.h | 126 +++
include/gnss/factor/factor_gnss_tdcp.h | 213 +++++
...single_diff_2D.h => factor_gnss_tdcp_2d.h} | 71 +-
include/gnss/factor/factor_gnss_tdcp_3d.h | 127 +++
include/gnss/factor/factor_gnss_tdcp_batch.h | 139 +++
include/gnss/feature/feature_gnss_fix.h | 42 +-
include/gnss/feature/feature_gnss_satellite.h | 101 ++
.../gnss/feature/feature_gnss_single_diff.h | 33 -
include/gnss/feature/feature_gnss_tdcp.h | 56 ++
include/gnss/processor/processor_gnss_fix.h | 157 +++-
.../processor/processor_gnss_single_diff.h | 116 ---
include/gnss/processor/processor_gnss_tdcp.h | 154 ++++
.../gnss/processor/processor_tracker_gnss.h | 322 +++++++
include/gnss/sensor/sensor_gnss.h | 249 +++--
.../tree_manager_sliding_window_tdcp.h | 64 ++
internal/config.h.in | 4 +-
license_header_2022.txt | 17 +
package.xml | 2 -
src/capture/capture_gnss.cpp | 53 ++
src/capture/capture_gnss_fix.cpp | 27 +-
src/processor/processor_gnss_fix.cpp | 322 ++++---
src/processor/processor_gnss_single_diff.cpp | 51 +-
src/processor/processor_gnss_tdcp.cpp | 270 ++++++
src/processor/processor_tracker_gnss.cpp | 870 ++++++++++++++++++
src/sensor/sensor_gnss.cpp | 343 ++++---
.../tree_manager_sliding_window_tdcp.cpp | 66 ++
test/CMakeLists.txt | 22 +-
test/gtest_example.cpp | 21 +
...ix_2D.cpp => gtest_factor_gnss_fix_2d.cpp} | 232 ++---
test/gtest_factor_gnss_pseudo_range.cpp | 322 +++++++
test/gtest_factor_gnss_single_diff_2D.cpp | 267 ------
test/gtest_factor_gnss_tdcp.cpp | 440 +++++++++
49 files changed, 5713 insertions(+), 1638 deletions(-)
create mode 100644 .vscode/c_cpp_properties.json
create mode 100644 LICENSE
delete mode 100644 ROS-based code style eclipse indented 4sp.xml
delete mode 100644 ROS-based wolf code style QT indented 4sp.xml
delete mode 100644 codetemplates eclipse.xml
create mode 100644 include/gnss/capture/capture_gnss.h
delete mode 100644 include/gnss/capture/capture_gnss_single_diff.h
create mode 100644 include/gnss/capture/capture_gnss_tdcp.h
rename include/gnss/factor/{factor_gnss_fix_2D.h => factor_gnss_fix_2d.h} (69%)
rename include/gnss/factor/{factor_gnss_fix_3D.h => factor_gnss_fix_3d.h} (64%)
create mode 100644 include/gnss/factor/factor_gnss_pseudo_range.h
create mode 100644 include/gnss/factor/factor_gnss_single_diff_2d.h
create mode 100644 include/gnss/factor/factor_gnss_tdcp.h
rename include/gnss/factor/{factor_gnss_single_diff_2D.h => factor_gnss_tdcp_2d.h} (70%)
create mode 100644 include/gnss/factor/factor_gnss_tdcp_3d.h
create mode 100644 include/gnss/factor/factor_gnss_tdcp_batch.h
create mode 100644 include/gnss/feature/feature_gnss_satellite.h
delete mode 100644 include/gnss/feature/feature_gnss_single_diff.h
create mode 100644 include/gnss/feature/feature_gnss_tdcp.h
delete mode 100644 include/gnss/processor/processor_gnss_single_diff.h
create mode 100644 include/gnss/processor/processor_gnss_tdcp.h
create mode 100644 include/gnss/processor/processor_tracker_gnss.h
create mode 100644 include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
create mode 100644 license_header_2022.txt
create mode 100644 src/capture/capture_gnss.cpp
create mode 100644 src/processor/processor_gnss_tdcp.cpp
create mode 100644 src/processor/processor_tracker_gnss.cpp
create mode 100644 src/tree_manager/tree_manager_sliding_window_tdcp.cpp
rename test/{gtest_factor_gnss_fix_2D.cpp => gtest_factor_gnss_fix_2d.cpp} (51%)
create mode 100644 test/gtest_factor_gnss_pseudo_range.cpp
delete mode 100644 test/gtest_factor_gnss_single_diff_2D.cpp
create mode 100644 test/gtest_factor_gnss_tdcp.cpp
diff --git a/.gitignore b/.gitignore
index ca5fdede9..16b374548 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,36 +1,9 @@
-.cproject
+
+.settings/language.settings.xml
.project
-.settings/
-README.txt
-bin/
+.cproject
build/
-build_debug/
-build_release/
+bin/
lib/
-.idea/
-./Wolf.user
-./Wolf.config
-./Wolf.creator
-./Wolf.files
-./Wolf.includes
-./Wolf/
-/CMakeLists.txt.user
-src/examples/map_polyline_example_write.yaml
-*.gch
-/CMakeFiles/
-/CMakeCache.txt
-*.dat
-.DS_Store
-src/examples
-*.graffle
-/Default/
-
-src/CMakeCache.txt
-
-src/CMakeFiles/cmake.check_cache
-src/examples/map_apriltag_save.yaml
-
-\.vscode/
-build_release/
-
gnss.found
+
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index e90446258..045cce1f0 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,108 +1,187 @@
-image: labrobotica/ceres:1.14
+stages:
+ - license
+ - build_and_test
-before_script:
- ##
+############ YAML ANCHORS ############
+.preliminaries_template: &preliminaries_definition
## Install ssh-agent if not already installed, it is required by Docker.
## (change apt-get to yum if you use an RPM-based image)
- ##
- 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )'
- ##
## Run ssh-agent (inside the build environment)
- ##
- eval $(ssh-agent -s)
- ##
## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store
## We're using tr to fix line endings which makes ed25519 keys work
## without extra base64 encoding.
## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556
- ##
- mkdir -p ~/.ssh
- chmod 700 ~/.ssh
- echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
# - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts
- ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts
- ##
- ## Create the SSH directory and give it the right permissions
- ##
- - ls
+ # update apt
- apt-get update
- - apt-get install -y build-essential cmake
-
-# SPDLOG
-# - apt-get install -y libspdlog-dev
- - if [ -d spdlog ]; then
- - echo "directory exists"
- - if [ "$(ls -A ./spdlog)" ]; then
- - echo "directory not empty"
- - cd spdlog
- - git pull
- - else
- - echo "directory empty"
- - git clone https://github.com/gabime/spdlog.git
- - cd spdlog
- - fi
+
+ # create 'ci_deps' folder (if not exists)
+ - mkdir -pv ci_deps
+
+.license_header_template: &license_header_definition
+ - cd $CI_PROJECT_DIR
+
+ # configure git
+ - export CI_NEW_BRANCH=ci_processing$RANDOM
+ - echo creating new temporary branch... $CI_NEW_BRANCH
+ - git config --global user.email "${CI_EMAIL}"
+ - git config --global user.name "${CI_USERNAME}"
+ - git checkout -b $CI_NEW_BRANCH # temporary branch
+
+ # license headers
+ - export CURRENT_YEAR=$( date +'%Y' )
+ - echo "current year:" ${CURRENT_YEAR}
+ - if [ -f license_header_${CURRENT_YEAR}.txt ]; then
+ # add license headers to new files
+ - echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..."
+ - ./ci_deps/wolf/wolf_scripts/license_manager.sh --add --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
+ - else
+ # update license headers of all files
+ - export PREV_YEAR=$(( CURRENT_YEAR-1 ))
+ - echo "Creating new file license_header_${CURRENT_YEAR}.txt..."
+ - git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt
+ - sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt
+ - ./ci_deps/wolf/wolf_scripts/license_manager.sh --update --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps
+ - fi
+
+ # push changes (if any)
+ - if git commit -a -m "[skip ci] license headers added or modified" ; then
+ - git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git"
+ - git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME}
- else
- - echo "directory inexistent"
- - git clone https://github.com/gabime/spdlog.git
- - cd spdlog
+ - echo "No changes, nothing to commit!"
+ - fi
+
+.install_wolf_template: &install_wolf_definition
+ - cd ${CI_PROJECT_DIR}/ci_deps
+ - if [ -d wolf ]; then
+ - echo "directory wolf exists"
+ - cd wolf
+ - git checkout devel
+ - git pull
+ - git checkout $WOLF_CORE_BRANCH
+ - else
+ - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
+ - cd wolf
+ - git checkout $WOLF_CORE_BRANCH
- fi
- - git fetch
- - git checkout v0.17.0
- mkdir -pv build
- cd build
- - ls
- - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF ..
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF ..
+ - make -j$(nproc)
- make install
- - cd ../..
-
-# YAML
-# - apt-get install -y libyaml-cpp-dev
- - if [ -d yaml-cpp ]; then
- - echo "directory exists"
- - if [ "$(ls -A ./yaml-cpp)" ]; then
- - echo "directory not empty"
- - cd yaml-cpp
- - git pull
- - else
- - echo "directory empty"
- - git clone https://github.com/jbeder/yaml-cpp.git
- - cd yaml-cpp
- - fi
+
+.install_gnssutils_template: &install_gnssutils_definition
+ - cd ${CI_PROJECT_DIR}/ci_deps
+ - if [ -d gnss_utils ]; then
+ - echo "directory gnss_utils exists"
+ - cd gnss_utils
+ - git pull
- else
- - echo "directory inexistent"
- - git clone https://github.com/jbeder/yaml-cpp.git
- - cd yaml-cpp
+ - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils.git
+ - cd gnss_utils
+ - git submodule update --init
- fi
- mkdir -pv build
- cd build
- - ls
- - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF ..
+ - cmake -DCMAKE_BUILD_TYPE=release-DBUILD_TESTS=ON ..
+ - make -j$(nproc)
+ - ctest -j$(nproc)
- make install
- - cd ../..
-#Wolf core
- - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git
- - cd wolf
+
+.build_and_test_template: &build_and_test_definition
+ - cd $CI_PROJECT_DIR
- mkdir -pv build
- cd build
- - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=ON ..
- make -j$(nproc)
- ctest -j$(nproc)
- make install
- - cd ../..
-wolf_build_and_test:
- stage: build
+############ LICENSE HEADERS ############
+license_headers:
+ stage: license
+ image: labrobotica/wolf_deps:16.04
+ cache:
+ - key: wolf-xenial
+ paths:
+ - ci_deps/wolf/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ script:
+ - *license_header_definition
+
+############ UBUNTU 16.04 TESTS ############
+build_and_test:xenial:
+ image: labrobotica/wolf_deps:16.04
+ stage: build_and_test
+ cache:
+ - key: wolf-xenial
+ paths:
+ - ci_deps/wolf/
+ - key: gnssutils-xenial
+ paths:
+ - ci_deps/gnss_utils/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_gnssutils_definition
+ - ldconfig # update links (shared libraries)
+ script:
+ - *build_and_test_definition
+
+############ UBUNTU 18.04 TESTS ############
+build_and_test:bionic:
+ image: labrobotica/wolf_deps:18.04
+ stage: build_and_test
+ cache:
+ - key: wolf-bionic
+ paths:
+ - ci_deps/wolf/
+ - key: gnssutils-bionic
+ paths:
+ - ci_deps/gnss_utils/
+ except:
+ - master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_gnssutils_definition
+ - ldconfig # update links (shared libraries)
+ script:
+ - *build_and_test_definition
+
+############ UBUNTU 20.04 TESTS ############
+build_and_test:focal:
+ image: labrobotica/wolf_deps:20.04
+ stage: build_and_test
+ cache:
+ - key: wolf-focal
+ paths:
+ - ci_deps/wolf/
+ - key: gnssutils-focal
+ paths:
+ - ci_deps/gnss_utils/
except:
- master
+ before_script:
+ - *preliminaries_definition
+ - *install_wolf_definition
+ - *install_gnssutils_definition
+ - ldconfig # update links (shared libraries)
script:
- - mkdir -pv build
- - cd build
- - ls # we can check whether the directory was already full
- - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON ..
- - make -j$(nproc)
- - ctest -j$(nproc)
- - ctest -V -R single diff
- - make install
+ - *build_and_test_definition
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 000000000..c10fac6d6
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,20 @@
+{
+ "configurations": [
+ {
+ "name": "Linux",
+ "includePath": [
+ "${workspaceFolder}/**",
+ "/home/aeroarms/iri-lab/external_libs/wolf/include",
+ "/usr/local/include/iri-algorithms/wolf/plugin_core",
+ "/usr/local/include/eigen3",
+ "/usr/local/include/iri-algorithms"
+ ],
+ "defines": [],
+ "compilerPath": "/usr/bin/gcc",
+ "cStandard": "gnu11",
+ "cppStandard": "c++17",
+ "intelliSenseMode": "gcc-x64"
+ }
+ ],
+ "version": 4
+}
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9780c827d..89e8ce877 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,18 +1,19 @@
# Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
if(COMMAND cmake_policy)
- cmake_policy(SET CMP0005 NEW)
+ cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
-
# The project name
PROJECT(gnss)
set(PLUGIN_NAME wolf${PROJECT_NAME})
+MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
+
SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin)
SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
SET(CMAKE_INSTALL_PREFIX /usr/local)
@@ -26,18 +27,14 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
-#Set compiler according C++11 support
+#Set compiler according C++14 support
include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+if(COMPILER_SUPPORTS_CXX14)
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
else()
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
if(UNIX)
@@ -46,7 +43,6 @@ if(UNIX)
"${CMAKE_CXX_FLAGS} -Werror=all -Werror=extra -Wno-unknown-pragmas -Wno-sign-compare -Wno-unused-parameter -Wno-missing-field-initializers")
endif(UNIX)
-
IF(NOT BUILD_TESTS)
OPTION(BUILD_TESTS "Build Unit tests" ON)
ENDIF(NOT BUILD_TESTS)
@@ -71,48 +67,24 @@ if(BUILD_TESTS)
enable_testing()
endif()
-MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
-
#CMAKE modules
-
SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
MESSAGE(STATUS ${CMAKE_MODULE_PATH})
# Some wolf compilation options
-
IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug))
set(_WOLF_DEBUG true)
ENDIF()
option(_WOLF_TRACE "Enable wolf tracing macro" ON)
-# Does this has any other interest
-# but for the examples ?
-# yes, for the tests !
-IF(BUILD_EXAMPLES OR BUILD_TESTS)
- string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
- set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_EXAMPLES OR BUILD_TESTS)
-
-
-#find dependencies.
-# ============EXAMPLE==================
-FIND_PACKAGE(wolf REQUIRED)
-
-#FIND_PATH(
-# Suitesparse_INCLUDE_DIRS
-# NAMES SuiteSparse_config.h
-# PATHS /usr/include/suitesparse /usr/local/include/suitesparse)
-#MESSAGE("Found suitesparse_INCLUDE_DIRS:" ${Suitesparse_INCLUDE_DIRS})
-
-#IF(Suitesparse_INCLUDE_DIRS)
-# SET(Suitesparse_FOUND TRUE)
-# MESSAGE("Suitesparse FOUND: wolf_solver will be built.")
-#ELSE (Suitesparse_INCLUDE_DIRS)
-# SET(Suitesparse_FOUND FALSE)
-# MESSAGE(FATAL_ERROR "Suitesparse NOT FOUND")
-#ENDIF (Suitesparse_INCLUDE_DIRS)
+# ============ DEPENDENCIES ============
+FIND_PACKAGE(wolfcore REQUIRED)
+FIND_PACKAGE(gnss_utils REQUIRED)
+
+# ============ CONFIG.H ============
+string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
+set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
# Define the directory where will be the configured config.h
SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -131,125 +103,85 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D
message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
include_directories("${PROJECT_BINARY_DIR}/conf")
-#INCLUDES SECTION
-# ==============================
-INCLUDE_DIRECTORIES(${wolf_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${laser_utils_INCLUDE_DIRS})
+# ============ INCLUDES ============
+INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${gnss_utils_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(BEFORE "include")
-#HEADERS
-SET(HDRS_COMMON
- )
-SET(HDRS_MATH
- )
-SET(HDRS_UTILS
- )
-SET(HDRS_STATE_BLOCK
- )
+# ============ HEADERS ============
SET(HDRS_CAPTURE
+ include/gnss/capture/capture_gnss.h
include/gnss/capture/capture_gnss_fix.h
- include/gnss/capture/capture_gnss_single_diff.h
+ include/gnss/capture/capture_gnss_tdcp.h
)
SET(HDRS_FACTOR
- include/gnss/factor/factor_gnss_fix_2D.h
- include/gnss/factor/factor_gnss_fix_3D.h
- include/gnss/factor/factor_gnss_single_diff_2D.h
+ include/gnss/factor/factor_gnss_fix_2d.h
+ include/gnss/factor/factor_gnss_fix_3d.h
+ include/gnss/factor/factor_gnss_pseudo_range.h
+ include/gnss/factor/factor_gnss_single_diff_2d.h
+ include/gnss/factor/factor_gnss_tdcp.h
+ include/gnss/factor/factor_gnss_tdcp_2d.h
+ include/gnss/factor/factor_gnss_tdcp_3d.h
)
SET(HDRS_FEATURE
include/gnss/feature/feature_gnss_fix.h
- include/gnss/feature/feature_gnss_single_diff.h
- )
-SET(HDRS_LANDMARK
+ include/gnss/feature/feature_gnss_tdcp.h
+ include/gnss/feature/feature_gnss_satellite.h
)
SET(HDRS_PROCESSOR
include/gnss/processor/processor_gnss_fix.h
- include/gnss/processor/processor_gnss_single_diff.h
+ include/gnss/processor/processor_gnss_tdcp.h
+ include/gnss/processor/processor_tracker_gnss.h
)
SET(HDRS_SENSOR
include/gnss/sensor/sensor_gnss.h
)
-SET(HDRS_SOLVER
- )
-SET(HDRS_DTASSC
+SET(HDRS_TREE_MANAGER
+ include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
)
-#SOURCES
-SET(SRCS_COMMON
- )
-SET(SRCS_MATH
- )
-SET(SRCS_UTILS
- )
-SET(SRCS_STATE_BLOCK
- )
+# ============ SOURCES ============
SET(SRCS_CAPTURE
+ src/capture/capture_gnss.cpp
src/capture/capture_gnss_fix.cpp
)
-SET(SRCS_FACTOR
- )
-SET(SRCS_FEATURE
- )
-SET(SRCS_LANDMARK
- )
SET(SRCS_PROCESSOR
src/processor/processor_gnss_fix.cpp
- src/processor/processor_gnss_single_diff.cpp
+ src/processor/processor_gnss_tdcp.cpp
+ src/processor/processor_tracker_gnss.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_gnss.cpp
)
-SET(SRCS_DTASSC
- )
-SET(SRCS_SOLVER
- )
-SET(SRCS_YAML
+SET(SRCS_TREE_MANAGER
+ src/tree_manager/tree_manager_sliding_window_tdcp.cpp
)
-#OPTIONALS
-#optional HDRS and SRCS
-# ==================EXAMPLE===============
-# IF (Ceres_FOUND)
-# SET(HDRS_WRAPPER
-# include/base/solver_suitesparse/sparse_utils.h
-# include/base/solver/solver_manager.h
-# include/base/ceres_wrapper/ceres_manager.h
-# include/base/ceres_wrapper/cost_function_wrapper.h
-# include/base/ceres_wrapper/create_numeric_diff_cost_function.h
-# include/base/ceres_wrapper/local_parametrization_wrapper.h
-# )
-# SET(SRCS_WRAPPER
-# src/solver/solver_manager.cpp
-# src/ceres_wrapper/ceres_manager.cpp
-# src/ceres_wrapper/local_parametrization_wrapper.cpp
-# )
-# ELSE(Ceres_FOUND)
-# SET(HDRS_WRAPPER)
-# SET(SRCS_WRAPPER)
-# ENDIF(Ceres_FOUND)
# create the shared library
ADD_LIBRARY(${PLUGIN_NAME}
SHARED
${SRCS_CAPTURE}
- ${SRCS_COMMON}
- ${SRCS_DTASSC}
- ${SRCS_FACTOR}
- ${SRCS_FEATURE}
- ${SRCS_LANDMARK}
- ${SRCS_MATH}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
- ${SRCS_SOLVER}
- ${SRCS_STATE_BLOCK}
- ${SRCS_UTILS}
- ${SRCS_WRAPPER}
- ${SRCS_YAML}
+ ${SRCS_TREE_MANAGER}
)
-
+
+# Set compiler options
+# ====================
+if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ message(STATUS "Using C++ compiler clang")
+ target_compile_options(${PLUGIN_NAME} PRIVATE -Winconsistent-missing-override)
+ # using Clang
+elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+ message(STATUS "Using C++ compiler gnu")
+ target_compile_options(${PLUGIN_NAME} PRIVATE -Wsuggest-override)
+ # using GCC
+endif()
+
#Link the created libraries
#===============EXAMPLE=========================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolf_LIBRARIES})
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${laser_scan_utils_LIBRARY})
-
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${gnss_utils_LIBRARIES})
#Build tests
#===============EXAMPLE=========================
@@ -262,37 +194,23 @@ ENDIF(BUILD_TESTS)
#=============================================================
INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
RUNTIME DESTINATION bin
- LIBRARY DESTINATION lib/iri-algorithms
- ARCHIVE DESTINATION lib/iri-algorithms)
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib)
install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
#install headers
-INSTALL(FILES ${HDRS_STATE_BLOCK}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/state_block)
-INSTALL(FILES ${HDRS_DTASSC}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/association)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
INSTALL(FILES ${HDRS_FACTOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
-INSTALL(FILES ${HDRS_SENSOR}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
-INSTALL(FILES ${HDRS_LANDMARK}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/ceres_wrapper)
-#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
-# DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse)
-INSTALL(FILES ${HDRS_SOLVER}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/serialization)
-INSTALL(FILES ${HDRS_YAML}
- DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml)
+INSTALL(FILES ${HDRS_SENSOR}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+INSTALL(FILES ${HDRS_TREE_MANAGER}
+ DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/tree_manager)
FILE(WRITE ${PROJECT_NAME}.found "")
INSTALL(FILES ${PROJECT_NAME}.found
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 000000000..281d399f1
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,619 @@
+ GNU GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+ Preamble
+
+ The GNU General Public License is a free, copyleft license for
+software and other kinds of works.
+
+ The licenses for most software and other practical works are designed
+to take away your freedom to share and change the works. By contrast,
+the GNU General Public License is intended to guarantee your freedom to
+share and change all versions of a program--to make sure it remains free
+software for all its users. We, the Free Software Foundation, use the
+GNU General Public License for most of our software; it applies also to
+any other work released this way by its authors. You can apply it to
+your programs, too.
+
+ When we speak of free software, we are referring to freedom, not
+price. Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+them if you wish), that you receive source code or can get it if you
+want it, that you can change the software or use pieces of it in new
+free programs, and that you know you can do these things.
+
+ To protect your rights, we need to prevent others from denying you
+these rights or asking you to surrender the rights. Therefore, you have
+certain responsibilities if you distribute copies of the software, or if
+you modify it: responsibilities to respect the freedom of others.
+
+ For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must pass on to the recipients the same
+freedoms that you received. You must make sure that they, too, receive
+or can get the source code. And you must show them these terms so they
+know their rights.
+
+ Developers that use the GNU GPL protect your rights with two steps:
+(1) assert copyright on the software, and (2) offer you this License
+giving you legal permission to copy, distribute and/or modify it.
+
+ For the developers' and authors' protection, the GPL clearly explains
+that there is no warranty for this free software. For both users' and
+authors' sake, the GPL requires that modified versions be marked as
+changed, so that their problems will not be attributed erroneously to
+authors of previous versions.
+
+ Some devices are designed to deny users access to install or run
+modified versions of the software inside them, although the manufacturer
+can do so. This is fundamentally incompatible with the aim of
+protecting users' freedom to change the software. The systematic
+pattern of such abuse occurs in the area of products for individuals to
+use, which is precisely where it is most unacceptable. Therefore, we
+have designed this version of the GPL to prohibit the practice for those
+products. If such problems arise substantially in other domains, we
+stand ready to extend this provision to those domains in future versions
+of the GPL, as needed to protect the freedom of users.
+
+ Finally, every program is threatened constantly by software patents.
+States should not allow patents to restrict development and use of
+software on general-purpose computers, but in those that do, we wish to
+avoid the special danger that patents applied to a free program could
+make it effectively proprietary. To prevent this, the GPL assures that
+patents cannot be used to render the program non-free.
+
+ The precise terms and conditions for copying, distribution and
+modification follow.
+
+ TERMS AND CONDITIONS
+
+ 0. Definitions.
+
+ "This License" refers to version 3 of the GNU General Public License.
+
+ "Copyright" also means copyright-like laws that apply to other kinds of
+works, such as semiconductor masks.
+
+ "The Program" refers to any copyrightable work licensed under this
+License. Each licensee is addressed as "you". "Licensees" and
+"recipients" may be individuals or organizations.
+
+ To "modify" a work means to copy from or adapt all or part of the work
+in a fashion requiring copyright permission, other than the making of an
+exact copy. The resulting work is called a "modified version" of the
+earlier work or a work "based on" the earlier work.
+
+ A "covered work" means either the unmodified Program or a work based
+on the Program.
+
+ To "propagate" a work means to do anything with it that, without
+permission, would make you directly or secondarily liable for
+infringement under applicable copyright law, except executing it on a
+computer or modifying a private copy. Propagation includes copying,
+distribution (with or without modification), making available to the
+public, and in some countries other activities as well.
+
+ To "convey" a work means any kind of propagation that enables other
+parties to make or receive copies. Mere interaction with a user through
+a computer network, with no transfer of a copy, is not conveying.
+
+ An interactive user interface displays "Appropriate Legal Notices"
+to the extent that it includes a convenient and prominently visible
+feature that (1) displays an appropriate copyright notice, and (2)
+tells the user that there is no warranty for the work (except to the
+extent that warranties are provided), that licensees may convey the
+work under this License, and how to view a copy of this License. If
+the interface presents a list of user commands or options, such as a
+menu, a prominent item in the list meets this criterion.
+
+ 1. Source Code.
+
+ The "source code" for a work means the preferred form of the work
+for making modifications to it. "Object code" means any non-source
+form of a work.
+
+ A "Standard Interface" means an interface that either is an official
+standard defined by a recognized standards body, or, in the case of
+interfaces specified for a particular programming language, one that
+is widely used among developers working in that language.
+
+ The "System Libraries" of an executable work include anything, other
+than the work as a whole, that (a) is included in the normal form of
+packaging a Major Component, but which is not part of that Major
+Component, and (b) serves only to enable use of the work with that
+Major Component, or to implement a Standard Interface for which an
+implementation is available to the public in source code form. A
+"Major Component", in this context, means a major essential component
+(kernel, window system, and so on) of the specific operating system
+(if any) on which the executable work runs, or a compiler used to
+produce the work, or an object code interpreter used to run it.
+
+ The "Corresponding Source" for a work in object code form means all
+the source code needed to generate, install, and (for an executable
+work) run the object code and to modify the work, including scripts to
+control those activities. However, it does not include the work's
+System Libraries, or general-purpose tools or generally available free
+programs which are used unmodified in performing those activities but
+which are not part of the work. For example, Corresponding Source
+includes interface definition files associated with source files for
+the work, and the source code for shared libraries and dynamically
+linked subprograms that the work is specifically designed to require,
+such as by intimate data communication or control flow between those
+subprograms and other parts of the work.
+
+ The Corresponding Source need not include anything that users
+can regenerate automatically from other parts of the Corresponding
+Source.
+
+ The Corresponding Source for a work in source code form is that
+same work.
+
+ 2. Basic Permissions.
+
+ All rights granted under this License are granted for the term of
+copyright on the Program, and are irrevocable provided the stated
+conditions are met. This License explicitly affirms your unlimited
+permission to run the unmodified Program. The output from running a
+covered work is covered by this License only if the output, given its
+content, constitutes a covered work. This License acknowledges your
+rights of fair use or other equivalent, as provided by copyright law.
+
+ You may make, run and propagate covered works that you do not
+convey, without conditions so long as your license otherwise remains
+in force. You may convey covered works to others for the sole purpose
+of having them make modifications exclusively for you, or provide you
+with facilities for running those works, provided that you comply with
+the terms of this License in conveying all material for which you do
+not control copyright. Those thus making or running the covered works
+for you must do so exclusively on your behalf, under your direction
+and control, on terms that prohibit them from making any copies of
+your copyrighted material outside their relationship with you.
+
+ Conveying under any other circumstances is permitted solely under
+the conditions stated below. Sublicensing is not allowed; section 10
+makes it unnecessary.
+
+ 3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+
+ No covered work shall be deemed part of an effective technological
+measure under any applicable law fulfilling obligations under article
+11 of the WIPO copyright treaty adopted on 20 December 1996, or
+similar laws prohibiting or restricting circumvention of such
+measures.
+
+ When you convey a covered work, you waive any legal power to forbid
+circumvention of technological measures to the extent such circumvention
+is effected by exercising rights under this License with respect to
+the covered work, and you disclaim any intention to limit operation or
+modification of the work as a means of enforcing, against the work's
+users, your or third parties' legal rights to forbid circumvention of
+technological measures.
+
+ 4. Conveying Verbatim Copies.
+
+ You may convey verbatim copies of the Program's source code as you
+receive it, in any medium, provided that you conspicuously and
+appropriately publish on each copy an appropriate copyright notice;
+keep intact all notices stating that this License and any
+non-permissive terms added in accord with section 7 apply to the code;
+keep intact all notices of the absence of any warranty; and give all
+recipients a copy of this License along with the Program.
+
+ You may charge any price or no price for each copy that you convey,
+and you may offer support or warranty protection for a fee.
+
+ 5. Conveying Modified Source Versions.
+
+ You may convey a work based on the Program, or the modifications to
+produce it from the Program, in the form of source code under the
+terms of section 4, provided that you also meet all of these conditions:
+
+ a) The work must carry prominent notices stating that you modified
+ it, and giving a relevant date.
+
+ b) The work must carry prominent notices stating that it is
+ released under this License and any conditions added under section
+ 7. This requirement modifies the requirement in section 4 to
+ "keep intact all notices".
+
+ c) You must license the entire work, as a whole, under this
+ License to anyone who comes into possession of a copy. This
+ License will therefore apply, along with any applicable section 7
+ additional terms, to the whole of the work, and all its parts,
+ regardless of how they are packaged. This License gives no
+ permission to license the work in any other way, but it does not
+ invalidate such permission if you have separately received it.
+
+ d) If the work has interactive user interfaces, each must display
+ Appropriate Legal Notices; however, if the Program has interactive
+ interfaces that do not display Appropriate Legal Notices, your
+ work need not make them do so.
+
+ A compilation of a covered work with other separate and independent
+works, which are not by their nature extensions of the covered work,
+and which are not combined with it such as to form a larger program,
+in or on a volume of a storage or distribution medium, is called an
+"aggregate" if the compilation and its resulting copyright are not
+used to limit the access or legal rights of the compilation's users
+beyond what the individual works permit. Inclusion of a covered work
+in an aggregate does not cause this License to apply to the other
+parts of the aggregate.
+
+ 6. Conveying Non-Source Forms.
+
+ You may convey a covered work in object code form under the terms
+of sections 4 and 5, provided that you also convey the
+machine-readable Corresponding Source under the terms of this License,
+in one of these ways:
+
+ a) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by the
+ Corresponding Source fixed on a durable physical medium
+ customarily used for software interchange.
+
+ b) Convey the object code in, or embodied in, a physical product
+ (including a physical distribution medium), accompanied by a
+ written offer, valid for at least three years and valid for as
+ long as you offer spare parts or customer support for that product
+ model, to give anyone who possesses the object code either (1) a
+ copy of the Corresponding Source for all the software in the
+ product that is covered by this License, on a durable physical
+ medium customarily used for software interchange, for a price no
+ more than your reasonable cost of physically performing this
+ conveying of source, or (2) access to copy the
+ Corresponding Source from a network server at no charge.
+
+ c) Convey individual copies of the object code with a copy of the
+ written offer to provide the Corresponding Source. This
+ alternative is allowed only occasionally and noncommercially, and
+ only if you received the object code with such an offer, in accord
+ with subsection 6b.
+
+ d) Convey the object code by offering access from a designated
+ place (gratis or for a charge), and offer equivalent access to the
+ Corresponding Source in the same way through the same place at no
+ further charge. You need not require recipients to copy the
+ Corresponding Source along with the object code. If the place to
+ copy the object code is a network server, the Corresponding Source
+ may be on a different server (operated by you or a third party)
+ that supports equivalent copying facilities, provided you maintain
+ clear directions next to the object code saying where to find the
+ Corresponding Source. Regardless of what server hosts the
+ Corresponding Source, you remain obligated to ensure that it is
+ available for as long as needed to satisfy these requirements.
+
+ e) Convey the object code using peer-to-peer transmission, provided
+ you inform other peers where the object code and Corresponding
+ Source of the work are being offered to the general public at no
+ charge under subsection 6d.
+
+ A separable portion of the object code, whose source code is excluded
+from the Corresponding Source as a System Library, need not be
+included in conveying the object code work.
+
+ A "User Product" is either (1) a "consumer product", which means any
+tangible personal property which is normally used for personal, family,
+or household purposes, or (2) anything designed or sold for incorporation
+into a dwelling. In determining whether a product is a consumer product,
+doubtful cases shall be resolved in favor of coverage. For a particular
+product received by a particular user, "normally used" refers to a
+typical or common use of that class of product, regardless of the status
+of the particular user or of the way in which the particular user
+actually uses, or expects or is expected to use, the product. A product
+is a consumer product regardless of whether the product has substantial
+commercial, industrial or non-consumer uses, unless such uses represent
+the only significant mode of use of the product.
+
+ "Installation Information" for a User Product means any methods,
+procedures, authorization keys, or other information required to install
+and execute modified versions of a covered work in that User Product from
+a modified version of its Corresponding Source. The information must
+suffice to ensure that the continued functioning of the modified object
+code is in no case prevented or interfered with solely because
+modification has been made.
+
+ If you convey an object code work under this section in, or with, or
+specifically for use in, a User Product, and the conveying occurs as
+part of a transaction in which the right of possession and use of the
+User Product is transferred to the recipient in perpetuity or for a
+fixed term (regardless of how the transaction is characterized), the
+Corresponding Source conveyed under this section must be accompanied
+by the Installation Information. But this requirement does not apply
+if neither you nor any third party retains the ability to install
+modified object code on the User Product (for example, the work has
+been installed in ROM).
+
+ The requirement to provide Installation Information does not include a
+requirement to continue to provide support service, warranty, or updates
+for a work that has been modified or installed by the recipient, or for
+the User Product in which it has been modified or installed. Access to a
+network may be denied when the modification itself materially and
+adversely affects the operation of the network or violates the rules and
+protocols for communication across the network.
+
+ Corresponding Source conveyed, and Installation Information provided,
+in accord with this section must be in a format that is publicly
+documented (and with an implementation available to the public in
+source code form), and must require no special password or key for
+unpacking, reading or copying.
+
+ 7. Additional Terms.
+
+ "Additional permissions" are terms that supplement the terms of this
+License by making exceptions from one or more of its conditions.
+Additional permissions that are applicable to the entire Program shall
+be treated as though they were included in this License, to the extent
+that they are valid under applicable law. If additional permissions
+apply only to part of the Program, that part may be used separately
+under those permissions, but the entire Program remains governed by
+this License without regard to the additional permissions.
+
+ When you convey a copy of a covered work, you may at your option
+remove any additional permissions from that copy, or from any part of
+it. (Additional permissions may be written to require their own
+removal in certain cases when you modify the work.) You may place
+additional permissions on material, added by you to a covered work,
+for which you have or can give appropriate copyright permission.
+
+ Notwithstanding any other provision of this License, for material you
+add to a covered work, you may (if authorized by the copyright holders of
+that material) supplement the terms of this License with terms:
+
+ a) Disclaiming warranty or limiting liability differently from the
+ terms of sections 15 and 16 of this License; or
+
+ b) Requiring preservation of specified reasonable legal notices or
+ author attributions in that material or in the Appropriate Legal
+ Notices displayed by works containing it; or
+
+ c) Prohibiting misrepresentation of the origin of that material, or
+ requiring that modified versions of such material be marked in
+ reasonable ways as different from the original version; or
+
+ d) Limiting the use for publicity purposes of names of licensors or
+ authors of the material; or
+
+ e) Declining to grant rights under trademark law for use of some
+ trade names, trademarks, or service marks; or
+
+ f) Requiring indemnification of licensors and authors of that
+ material by anyone who conveys the material (or modified versions of
+ it) with contractual assumptions of liability to the recipient, for
+ any liability that these contractual assumptions directly impose on
+ those licensors and authors.
+
+ All other non-permissive additional terms are considered "further
+restrictions" within the meaning of section 10. If the Program as you
+received it, or any part of it, contains a notice stating that it is
+governed by this License along with a term that is a further
+restriction, you may remove that term. If a license document contains
+a further restriction but permits relicensing or conveying under this
+License, you may add to a covered work material governed by the terms
+of that license document, provided that the further restriction does
+not survive such relicensing or conveying.
+
+ If you add terms to a covered work in accord with this section, you
+must place, in the relevant source files, a statement of the
+additional terms that apply to those files, or a notice indicating
+where to find the applicable terms.
+
+ Additional terms, permissive or non-permissive, may be stated in the
+form of a separately written license, or stated as exceptions;
+the above requirements apply either way.
+
+ 8. Termination.
+
+ You may not propagate or modify a covered work except as expressly
+provided under this License. Any attempt otherwise to propagate or
+modify it is void, and will automatically terminate your rights under
+this License (including any patent licenses granted under the third
+paragraph of section 11).
+
+ However, if you cease all violation of this License, then your
+license from a particular copyright holder is reinstated (a)
+provisionally, unless and until the copyright holder explicitly and
+finally terminates your license, and (b) permanently, if the copyright
+holder fails to notify you of the violation by some reasonable means
+prior to 60 days after the cessation.
+
+ Moreover, your license from a particular copyright holder is
+reinstated permanently if the copyright holder notifies you of the
+violation by some reasonable means, this is the first time you have
+received notice of violation of this License (for any work) from that
+copyright holder, and you cure the violation prior to 30 days after
+your receipt of the notice.
+
+ Termination of your rights under this section does not terminate the
+licenses of parties who have received copies or rights from you under
+this License. If your rights have been terminated and not permanently
+reinstated, you do not qualify to receive new licenses for the same
+material under section 10.
+
+ 9. Acceptance Not Required for Having Copies.
+
+ You are not required to accept this License in order to receive or
+run a copy of the Program. Ancillary propagation of a covered work
+occurring solely as a consequence of using peer-to-peer transmission
+to receive a copy likewise does not require acceptance. However,
+nothing other than this License grants you permission to propagate or
+modify any covered work. These actions infringe copyright if you do
+not accept this License. Therefore, by modifying or propagating a
+covered work, you indicate your acceptance of this License to do so.
+
+ 10. Automatic Licensing of Downstream Recipients.
+
+ Each time you convey a covered work, the recipient automatically
+receives a license from the original licensors, to run, modify and
+propagate that work, subject to this License. You are not responsible
+for enforcing compliance by third parties with this License.
+
+ An "entity transaction" is a transaction transferring control of an
+organization, or substantially all assets of one, or subdividing an
+organization, or merging organizations. If propagation of a covered
+work results from an entity transaction, each party to that
+transaction who receives a copy of the work also receives whatever
+licenses to the work the party's predecessor in interest had or could
+give under the previous paragraph, plus a right to possession of the
+Corresponding Source of the work from the predecessor in interest, if
+the predecessor has it or can get it with reasonable efforts.
+
+ You may not impose any further restrictions on the exercise of the
+rights granted or affirmed under this License. For example, you may
+not impose a license fee, royalty, or other charge for exercise of
+rights granted under this License, and you may not initiate litigation
+(including a cross-claim or counterclaim in a lawsuit) alleging that
+any patent claim is infringed by making, using, selling, offering for
+sale, or importing the Program or any portion of it.
+
+ 11. Patents.
+
+ A "contributor" is a copyright holder who authorizes use under this
+License of the Program or a work on which the Program is based. The
+work thus licensed is called the contributor's "contributor version".
+
+ A contributor's "essential patent claims" are all patent claims
+owned or controlled by the contributor, whether already acquired or
+hereafter acquired, that would be infringed by some manner, permitted
+by this License, of making, using, or selling its contributor version,
+but do not include claims that would be infringed only as a
+consequence of further modification of the contributor version. For
+purposes of this definition, "control" includes the right to grant
+patent sublicenses in a manner consistent with the requirements of
+this License.
+
+ Each contributor grants you a non-exclusive, worldwide, royalty-free
+patent license under the contributor's essential patent claims, to
+make, use, sell, offer for sale, import and otherwise run, modify and
+propagate the contents of its contributor version.
+
+ In the following three paragraphs, a "patent license" is any express
+agreement or commitment, however denominated, not to enforce a patent
+(such as an express permission to practice a patent or covenant not to
+sue for patent infringement). To "grant" such a patent license to a
+party means to make such an agreement or commitment not to enforce a
+patent against the party.
+
+ If you convey a covered work, knowingly relying on a patent license,
+and the Corresponding Source of the work is not available for anyone
+to copy, free of charge and under the terms of this License, through a
+publicly available network server or other readily accessible means,
+then you must either (1) cause the Corresponding Source to be so
+available, or (2) arrange to deprive yourself of the benefit of the
+patent license for this particular work, or (3) arrange, in a manner
+consistent with the requirements of this License, to extend the patent
+license to downstream recipients. "Knowingly relying" means you have
+actual knowledge that, but for the patent license, your conveying the
+covered work in a country, or your recipient's use of the covered work
+in a country, would infringe one or more identifiable patents in that
+country that you have reason to believe are valid.
+
+ If, pursuant to or in connection with a single transaction or
+arrangement, you convey, or propagate by procuring conveyance of, a
+covered work, and grant a patent license to some of the parties
+receiving the covered work authorizing them to use, propagate, modify
+or convey a specific copy of the covered work, then the patent license
+you grant is automatically extended to all recipients of the covered
+work and works based on it.
+
+ A patent license is "discriminatory" if it does not include within
+the scope of its coverage, prohibits the exercise of, or is
+conditioned on the non-exercise of one or more of the rights that are
+specifically granted under this License. You may not convey a covered
+work if you are a party to an arrangement with a third party that is
+in the business of distributing software, under which you make payment
+to the third party based on the extent of your activity of conveying
+the work, and under which the third party grants, to any of the
+parties who would receive the covered work from you, a discriminatory
+patent license (a) in connection with copies of the covered work
+conveyed by you (or copies made from those copies), or (b) primarily
+for and in connection with specific products or compilations that
+contain the covered work, unless you entered into that arrangement,
+or that patent license was granted, prior to 28 March 2007.
+
+ Nothing in this License shall be construed as excluding or limiting
+any implied license or other defenses to infringement that may
+otherwise be available to you under applicable patent law.
+
+ 12. No Surrender of Others' Freedom.
+
+ If conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot convey a
+covered work so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you may
+not convey it at all. For example, if you agree to terms that obligate you
+to collect a royalty for further conveying from those to whom you convey
+the Program, the only way you could satisfy both those terms and this
+License would be to refrain entirely from conveying the Program.
+
+ 13. Use with the GNU Affero General Public License.
+
+ Notwithstanding any other provision of this License, you have
+permission to link or combine any covered work with a work licensed
+under version 3 of the GNU Affero General Public License into a single
+combined work, and to convey the resulting work. The terms of this
+License will continue to apply to the part which is the covered work,
+but the special requirements of the GNU Affero General Public License,
+section 13, concerning interaction through a network will apply to the
+combination as such.
+
+ 14. Revised Versions of this License.
+
+ The Free Software Foundation may publish revised and/or new versions of
+the GNU General Public License from time to time. Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+ Each version is given a distinguishing version number. If the
+Program specifies that a certain numbered version of the GNU General
+Public License "or any later version" applies to it, you have the
+option of following the terms and conditions either of that numbered
+version or of any later version published by the Free Software
+Foundation. If the Program does not specify a version number of the
+GNU General Public License, you may choose any version ever published
+by the Free Software Foundation.
+
+ If the Program specifies that a proxy can decide which future
+versions of the GNU General Public License can be used, that proxy's
+public statement of acceptance of a version permanently authorizes you
+to choose that version for the Program.
+
+ Later license versions may give you additional or different
+permissions. However, no additional obligations are imposed on any
+author or copyright holder as a result of your choosing to follow a
+later version.
+
+ 15. Disclaimer of Warranty.
+
+ THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY
+APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT
+HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY
+OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM
+IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF
+ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. Limitation of Liability.
+
+ IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS
+THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY
+GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE
+USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF
+DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD
+PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS),
+EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF
+SUCH DAMAGES.
+
+ 17. Interpretation of Sections 15 and 16.
+
+ If the disclaimer of warranty and limitation of liability provided
+above cannot be given local legal effect according to their terms,
+reviewing courts shall apply local law that most closely approximates
+an absolute waiver of all civil liability in connection with the
+Program, unless a warranty or assumption of liability accompanies a
+copy of the Program in return for a fee.
diff --git a/README.md b/README.md
index 31948d180..9b8566e24 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
WOLF - Windowed Localization Frames | GNSS Plugin
===================================
-For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/).
\ No newline at end of file
+For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/).
diff --git a/ROS-based code style eclipse indented 4sp.xml b/ROS-based code style eclipse indented 4sp.xml
deleted file mode 100644
index 73a372adf..000000000
--- a/ROS-based code style eclipse indented 4sp.xml
+++ /dev/null
@@ -1,167 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<profiles version="1">
-<profile kind="CodeFormatterProfile" name="ROS Formatting indented private" version="1">
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_for" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_in_empty_block" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.lineSplit" value="120"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_member_access" value="0"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_base_types" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_else_statement_on_same_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_switchstatements_compare_to_switch" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_constructor_initializer_list" value="2"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_method_declaration_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_if" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_parenthesized_expression" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_exception_specification" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_base_types" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_body_declarations_compare_to_access_specifier" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_exception_specification" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_template_arguments" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_block" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.use_tabs_only_for_leading_indentations" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_labeled_statement" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_case" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.comment.min_distance_between_code_and_line_comment" value="1"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_array_initializer" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_enum_declarations" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_expressions_in_array_initializer" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_declarator_list" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_bracket" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_for" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_prefix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.tabulation.size" value="4"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_else_in_if_statement" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_enumerator_list" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_parenthesized_expression" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_parens_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_switch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_declarator_list" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_parenthesized_expression" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_empty_lines" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_switchstatements_compare_to_cases" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_empty_array_initializer_on_one_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_method_declaration" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.put_empty_statement_on_new_line" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_switch" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_cast" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_braces_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_method_declaration" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_while" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_question_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_semicolon" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_angle_bracket_in_template_arguments" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_base_clause" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_breaks_compare_to_cases" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_unary_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.join_wrapped_lines" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_declarator_list" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_arguments_in_method_invocation" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.comment.never_indent_line_comments_on_first_column" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_while" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_brackets" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_bracket" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_parameters_in_method_declaration" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_closing_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.number_of_empty_lines_to_preserve" value="1"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_semicolon_in_for" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.comment.preserve_white_space_between_code_and_line_comments" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_block" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_type_declaration" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_assignment_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_angle_bracket_in_template_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_expression_list" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_angle_bracket_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.continuation_indentation" value="2"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_expression_list" value="0"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_method_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_default" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_binary_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_conditional_expression" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_parens_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_if" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.format_guardian_clause_on_one_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_access_specifier_extra_spaces" value="0"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_cast" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_access_specifier_compare_to_type_header" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_type_declaration" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.continuation_indentation_for_array_initializer" value="2"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_labeled_statement" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_method_declaration_parameters" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_semicolon_in_for" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_body_declarations_compare_to_namespace_header" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_compact_if" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_assignment_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_brace_in_block" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_array_initializer" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_at_end_of_file_if_missing" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_assignment" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_conditional_expression_chain" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_template_parameters" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_expression_list" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_question_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_exception_specification" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_binary_operator" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_base_clause_in_type_declaration" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_identifier_in_function_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_method_declaration_throws" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_between_empty_parens_in_exception_specification" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_method_invocation_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_declaration_compare_to_template_header" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_unary_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_switch" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_statements_compare_to_body" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_method_declaration_throws" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_binary_expression" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.indent_statements_compare_to_block" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_template_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_catch_in_try_statement" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_throws_clause_in_method_declaration" value="18"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_method_invocation" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_paren_in_catch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_paren_in_cast" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_angle_bracket_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.tabulation.char" value="space"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_angle_bracket_in_template_parameters" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_colon_in_constructor_initializer_list" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_while" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_comma_in_method_invocation_arguments" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_block_in_case" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.compact_else_if" value="true"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_postfix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_after_template_declaration" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_base_clause" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_catch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_then_statement_on_same_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_switch" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.alignment_for_overloaded_left_shift_chain" value="16"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_if" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_paren_in_switch" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.keep_imple_if_on_one_line" value="false"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_after_opening_brace_in_array_initializer" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.indentation.size" value="4"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_namespace_declaration" value="next_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_colon_in_conditional" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_comma_in_enum_declarations" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_prefix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_angle_bracket_in_template_arguments" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.brace_position_for_array_initializer" value="end_of_line"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_colon_in_case" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_catch" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_brace_in_namespace_declaration" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_postfix_operator" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_closing_bracket" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_new_line_before_while_in_do_statement" value="do not insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_before_opening_paren_in_for" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_closing_angle_bracket_in_template_parameters" value="insert"/>
-<setting id="org.eclipse.cdt.core.formatter.insert_space_after_opening_angle_bracket_in_template_arguments" value="do not insert"/>
-</profile>
-</profiles>
diff --git a/ROS-based wolf code style QT indented 4sp.xml b/ROS-based wolf code style QT indented 4sp.xml
deleted file mode 100644
index 7ef65eaad..000000000
--- a/ROS-based wolf code style QT indented 4sp.xml
+++ /dev/null
@@ -1,39 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!DOCTYPE QtCreatorCodeStyle>
-<!-- Written by QtCreator 3.0.1, 2016-02-22T12:02:02. -->
-<qtcreator>
- <data>
- <variable>CodeStyleData</variable>
- <valuemap type="QVariantMap">
- <value type="bool" key="AlignAssignments">false</value>
- <value type="bool" key="AutoSpacesForTabs">false</value>
- <value type="bool" key="BindStarToIdentifier">false</value>
- <value type="bool" key="BindStarToLeftSpecifier">false</value>
- <value type="bool" key="BindStarToRightSpecifier">false</value>
- <value type="bool" key="BindStarToTypeName">true</value>
- <value type="bool" key="ExtraPaddingForConditionsIfConfusingAlign">true</value>
- <value type="bool" key="IndentAccessSpecifiers">true</value>
- <value type="bool" key="IndentBlockBody">true</value>
- <value type="bool" key="IndentBlockBraces">false</value>
- <value type="bool" key="IndentBlocksRelativeToSwitchLabels">true</value>
- <value type="bool" key="IndentClassBraces">false</value>
- <value type="bool" key="IndentControlFlowRelativeToSwitchLabels">true</value>
- <value type="bool" key="IndentDeclarationsRelativeToAccessSpecifiers">true</value>
- <value type="bool" key="IndentEnumBraces">false</value>
- <value type="bool" key="IndentFunctionBody">true</value>
- <value type="bool" key="IndentFunctionBraces">false</value>
- <value type="bool" key="IndentNamespaceBody">false</value>
- <value type="bool" key="IndentNamespaceBraces">false</value>
- <value type="int" key="IndentSize">4</value>
- <value type="bool" key="IndentStatementsRelativeToSwitchLabels">true</value>
- <value type="bool" key="IndentSwitchLabels">true</value>
- <value type="int" key="PaddingMode">2</value>
- <value type="bool" key="SpacesForTabs">true</value>
- <value type="int" key="TabSize">4</value>
- </valuemap>
- </data>
- <data>
- <variable>DisplayName</variable>
- <value type="QString">ROS wolf</value>
- </data>
-</qtcreator>
diff --git a/cmake_modules/wolfgnssConfig.cmake b/cmake_modules/wolfgnssConfig.cmake
index 486e5af0e..43a786472 100644
--- a/cmake_modules/wolfgnssConfig.cmake
+++ b/cmake_modules/wolfgnssConfig.cmake
@@ -12,7 +12,7 @@ ENDIF(wolfgnss_INCLUDE_DIRS)
FIND_LIBRARY(
wolfgnss_LIBRARIES
NAMES libwolfgnss.so
- PATHS /usr/local/lib/iri-algorithms)
+ PATHS /usr/local/lib)
IF(wolfgnss_LIBRARIES)
MESSAGE("Found gnss lib: ${wolfgnss_LIBRARIES}")
ELSE(wolfgnss_LIBRARIES)
@@ -66,18 +66,23 @@ set(wolfgnss_FOUND TRUE)
# Now we gather all the required dependencies for Wolf Laser
-FIND_PACKAGE(laser_scan_utils REQUIRED)
-list(APPEND wolfgnss_INCLUDE_DIRS ${laser_scan_utils_INCLUDE_DIRS})
-list(APPEND wolfgnss_LIBRARIES ${laser_scan_utils_LIBRARY})
+FIND_PACKAGE(gnss_utils REQUIRED)
+list(APPEND wolfgnss_INCLUDE_DIRS ${gnss_utils_INCLUDE_DIRS})
+list(APPEND wolfgnss_LIBRARIES ${gnss_utils_LIBRARY})
if(NOT wolf_FOUND)
- FIND_PACKAGE(wolf REQUIRED)
+ FIND_PACKAGE(wolfcore REQUIRED)
#We reverse in order to insert at the start
list(REVERSE wolfgnss_INCLUDE_DIRS)
- list(APPEND wolfgnss_INCLUDE_DIRS ${wolf_INCLUDE_DIRS})
+ list(APPEND wolfgnss_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
list(REVERSE wolfgnss_INCLUDE_DIRS)
list(REVERSE wolfgnss_LIBRARIES)
- list(APPEND wolfgnss_LIBRARIES ${wolf_LIBRARIES})
+ list(APPEND wolfgnss_LIBRARIES ${wolfcore_LIBRARIES})
list(REVERSE wolfgnss_LIBRARIES)
-endif()
\ No newline at end of file
+endif()
+
+# provide both INCLUDE_DIR and INCLUDE_DIRS
+SET(wolfgnss_INCLUDE_DIR ${wolfgnss_INCLUDE_DIRS})
+# provide both LIBRARY and LIBRARIES
+SET(wolfgnss_LIBRARY ${wolfgnss_LIBRARIES})
\ No newline at end of file
diff --git a/codetemplates eclipse.xml b/codetemplates eclipse.xml
deleted file mode 100644
index 85f43146f..000000000
--- a/codetemplates eclipse.xml
+++ /dev/null
@@ -1,76 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?><templates><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.constructorcomment_context" deleted="false" description="Comment for created constructors" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.constructorcomment" name="constructorcomment">/*
- *
- */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.destructorcomment_context" deleted="false" description="Comment for created destructors" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.destructorcomment" name="destructorcomment">/*
- *
- */</template><template autoinsert="false" context="org.eclipse.cdt.ui.text.codetemplates.filecomment_context" deleted="false" description="Comment for created C/C++ files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.filecomment" name="filecomment">/**
- * \file ${file_name}
- *
- * Created on: ${date}
- * \author: ${user}
- */
-</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.typecomment_context" deleted="false" description="Comment for created classes" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.typecomment" name="typecomment">/*
- *
- */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.fieldcomment_context" deleted="false" description="Comment for fields" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.fieldcomment" name="fieldcomment">/*
- *
- */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.methodcomment_context" deleted="false" description="Comment for methods" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.methodcomment" name="methodcomment">/*
- *
- */</template><template autoinsert="false" context="org.eclipse.cdt.core.cxxSource.contenttype_context" deleted="false" description="Default template for newly created C++ source files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cppsourcefile" name="Default C++ source template">${filecomment}
-${includes}
-
-${namespace_begin}
-
-${declarations}
-
-${namespace_end}</template><template autoinsert="false" context="org.eclipse.cdt.core.cxxSource.contenttype_context" deleted="false" description="Default template for newly created C++ test files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cpptestfile" name="Default C++ test template">${filecomment}
-
-#include <core/utils/utils_gtest.h>
-
-${includes}
-
-${namespace_begin}
-
-${declarations}
-
-TEST(TestGroup, DummyTestExample)
-{
- // ${todo}: Automatically generated TEST stub
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
-${namespace_end}</template><template autoinsert="true" context="org.eclipse.cdt.core.cxxHeader.contenttype_context" deleted="false" description="Default template for newly created C++ header files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cppheaderfile" name="Default C++ header template">${filecomment}
-#ifndef ${include_guard_symbol}
-#define ${include_guard_symbol}
-
-${includes}
-
-${namespace_begin}
-
-${declarations}
-
-${namespace_end}
-
-#endif /* ${include_guard_symbol} */</template><template autoinsert="true" context="org.eclipse.cdt.core.cSource.contenttype_context" deleted="false" description="Default template for newly created C source files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.csourcefile" name="Default C source template">${filecomment}
-${includes}
-
-${declarations}</template><template autoinsert="true" context="org.eclipse.cdt.core.cHeader.contenttype_context" deleted="false" description="Default template for newly created C header files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.cheaderfile" name="Default C header template">${filecomment}
-#ifndef ${include_guard_symbol}
-#define ${include_guard_symbol}
-
-${includes}
-
-${declarations}
-
-#endif /* ${include_guard_symbol} */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.namespace_context" deleted="false" description="Beginning of namespace declaration" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.namespace_begin" name="namespace_begin">namespace ${namespace_name} {</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.namespace_context" deleted="false" description="End of namespace declaration" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.namespace_end" name="namespace_end">} /* namespace ${namespace_name} */</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.class_context" deleted="false" description="Code in created class definitions" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.class_body" name="class_body">${declarations}</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.methodbody_context" deleted="false" description="Code in created method stubs" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.methodbody" name="methodbody"> // ${todo} Auto-generated method stub
- ${body_statement}</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.constructorbody_context" deleted="false" description="Code in created constructor stubs" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.constructorbody" name="constructorbody"> // ${todo} Auto-generated constructor stub
- ${body_statement}</template><template autoinsert="true" context="org.eclipse.cdt.ui.text.codetemplates.destructorbody_context" deleted="false" description="Code in created destructor stubs" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.destructorbody" name="destructorbody"> ${body_statement}
- // ${todo} Auto-generated destructor stub</template><template autoinsert="true" context="org.eclipse.cdt.core.asmSource.contenttype_context" deleted="false" description="Default template for newly created assembly files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.asmsourcefile" name="Default assembly template">${filecomment}
-</template><template autoinsert="true" context="org.eclipse.core.runtime.text.contenttype_context" deleted="false" description="Default template for newly created text files" enabled="true" id="org.eclipse.cdt.ui.text.codetemplates.textfile" name="Default text file template">${file_name}
- Created on: ${date}
- Author: ${user}
-
-</template></templates>
\ No newline at end of file
diff --git a/include/gnss/capture/capture_gnss.h b/include/gnss/capture/capture_gnss.h
new file mode 100644
index 000000000..1a8980534
--- /dev/null
+++ b/include/gnss/capture/capture_gnss.h
@@ -0,0 +1,80 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef CAPTURE_GNSS_H_
+#define CAPTURE_GNSS_H_
+
+//Wolf includes
+#include "gnss/feature/feature_gnss_fix.h"
+#include "core/capture/capture_base.h"
+
+//std includes
+#include "gnss_utils/snapshot.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureGnss);
+
+//class CaptureGnss
+class CaptureGnss : public CaptureBase
+{
+ protected:
+ GnssUtils::SnapshotPtr snapshot_; ///< observation and navigation data
+
+ public:
+ CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUtils::SnapshotPtr _snapshot);
+ ~CaptureGnss() override;
+
+ GnssUtils::SnapshotPtr getSnapshot() const;
+ GnssUtils::ObservationsPtr getObservations() const;
+ GnssUtils::NavigationPtr getNavigation() const;
+ GnssUtils::Satellites& getSatellites();
+ const GnssUtils::Satellites& getSatellites() const;
+
+};
+
+inline GnssUtils::SnapshotPtr CaptureGnss::getSnapshot() const
+{
+ return snapshot_;
+}
+
+inline GnssUtils::ObservationsPtr CaptureGnss::getObservations() const
+{
+ return snapshot_->getObservations();
+}
+
+inline GnssUtils::NavigationPtr CaptureGnss::getNavigation() const
+{
+ return snapshot_->getNavigation();
+}
+
+inline GnssUtils::Satellites& CaptureGnss::getSatellites()
+{
+ return snapshot_->getSatellites();
+}
+
+inline const GnssUtils::Satellites& CaptureGnss::getSatellites() const
+{
+ return snapshot_->getSatellites();
+}
+
+} //namespace wolf
+#endif
diff --git a/include/gnss/capture/capture_gnss_fix.h b/include/gnss/capture/capture_gnss_fix.h
index 08af47ba2..c894d3e7f 100644
--- a/include/gnss/capture/capture_gnss_fix.h
+++ b/include/gnss/capture/capture_gnss_fix.h
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
#ifndef CAPTURE_GNSS_FIX_H_
#define CAPTURE_GNSS_FIX_H_
@@ -16,31 +37,45 @@ WOLF_PTR_TYPEDEFS(CaptureGnssFix);
class CaptureGnssFix : public CaptureBase
{
protected:
- Eigen::VectorXs data_; ///< Raw data.
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
+ Eigen::Vector3d position_ECEF_; ///< position in ECEF coordinates.
+ Eigen::Matrix3d covariance_ECEF_; ///< Noise of the fix in ECEF coordinates.
+ TimeStamp ts_GPST_; ///< Time stamp in GPS time
public:
- CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
- virtual ~CaptureGnssFix();
- const Eigen::VectorXs& getData() const;
- const Eigen::MatrixXs& getDataCovariance() const;
- void getDataAndCovariance(Eigen::VectorXs& data, Eigen::MatrixXs& data_cov) const;
+ CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::Vector3d& _position, const Eigen::Matrix3d& _covariance, bool _ecef_coordinates = true);
+ ~CaptureGnssFix() override;
+
+ const Eigen::Vector3d& getPositionEcef() const;
+ const Eigen::Matrix3d& getCovarianceEcef() const;
+ void getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const;
+ const TimeStamp& getGpsTimeStamp() const;
+ void setGpsTimeStamp(const TimeStamp& _ts_GPST);
};
-inline const Eigen::VectorXs& CaptureGnssFix::getData() const
+inline const Eigen::Vector3d& CaptureGnssFix::getPositionEcef() const
+{
+ return position_ECEF_;
+}
+
+inline const Eigen::Matrix3d& CaptureGnssFix::getCovarianceEcef() const
+{
+ return covariance_ECEF_;
+}
+
+inline void CaptureGnssFix::getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const
{
- return data_;
+ position = position_ECEF_;
+ cov = covariance_ECEF_;
}
-inline const Eigen::MatrixXs& CaptureGnssFix::getDataCovariance() const
+inline const wolf::TimeStamp& CaptureGnssFix::getGpsTimeStamp() const
{
- return data_covariance_;
+ return ts_GPST_;
}
-inline void CaptureGnssFix::getDataAndCovariance(Eigen::VectorXs& data, Eigen::MatrixXs& data_cov) const
+inline void CaptureGnssFix::setGpsTimeStamp(const TimeStamp& _ts_GPST)
{
- data = data_;
- data_cov = data_covariance_;
+ ts_GPST_ = _ts_GPST;
}
} //namespace wolf
diff --git a/include/gnss/capture/capture_gnss_single_diff.h b/include/gnss/capture/capture_gnss_single_diff.h
deleted file mode 100644
index 3423496bd..000000000
--- a/include/gnss/capture/capture_gnss_single_diff.h
+++ /dev/null
@@ -1,63 +0,0 @@
-#ifndef CAPTURE_GNSS_SINGLE_DIFF_H_
-#define CAPTURE_GNSS_SINGLE_DIFF_H_
-
-//Wolf includes
-#include "gnss/feature/feature_gnss_single_diff.h"
-#include "core/capture/capture_base.h"
-
-//std includes
-//
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureGnssSingleDiff);
-
-//class CaptureGnssSingleDiff
-class CaptureGnssSingleDiff : public CaptureBase
-{
- protected:
- Eigen::Vector3s data_; ///< Raw data.
- Eigen::Matrix3s data_covariance_; ///< Noise of the capture.
- FrameBasePtr origin_frame_ptr_;
-
- public:
- CaptureGnssSingleDiff(const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector3s& _data,
- const Eigen::Matrix3s& _data_covariance,
- const FrameBasePtr& _origin_frame_ptr) :
- CaptureBase("GNSS SINGLE DIFFERENCE", _ts, _sensor_ptr),
- data_(_data),
- data_covariance_(_data_covariance),
- origin_frame_ptr_(_origin_frame_ptr)
- {};
- virtual ~CaptureGnssSingleDiff(){};
- const Eigen::Vector3s& getData() const;
- const Eigen::Matrix3s& getDataCovariance() const;
- void getDataAndCovariance(Eigen::Vector3s& data, Eigen::Matrix3s& data_cov) const;
- FrameBasePtr getOriginFrame() const;
-};
-
-inline const Eigen::Vector3s& CaptureGnssSingleDiff::getData() const
-{
- return data_;
-}
-
-inline const Eigen::Matrix3s& CaptureGnssSingleDiff::getDataCovariance() const
-{
- return data_covariance_;
-}
-
-inline void CaptureGnssSingleDiff::getDataAndCovariance(Eigen::Vector3s& data, Eigen::Matrix3s& data_cov) const
-{
- data = data_;
- data_cov = data_covariance_;
-}
-
-inline FrameBasePtr CaptureGnssSingleDiff::getOriginFrame() const
-{
- return origin_frame_ptr_;
-}
-
-} //namespace wolf
-#endif
diff --git a/include/gnss/capture/capture_gnss_tdcp.h b/include/gnss/capture/capture_gnss_tdcp.h
new file mode 100644
index 000000000..73af63d7c
--- /dev/null
+++ b/include/gnss/capture/capture_gnss_tdcp.h
@@ -0,0 +1,96 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef CAPTURE_GNSS_TDCP_H_
+#define CAPTURE_GNSS_TDCP_H_
+
+//Wolf includes
+#include "core/capture/capture_base.h"
+#include "../feature/feature_gnss_tdcp.h"
+
+//std includes
+//
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CaptureGnssTdcp);
+
+class CaptureGnssTdcp : public CaptureBase
+{
+ protected:
+ Eigen::Vector3d data_; ///< Displacement in ECEF coordinates.
+ Eigen::Matrix3d data_covariance_; ///< Noise of the capture.
+ FrameBasePtr origin_frame_ptr_;
+ TimeStamp ts_GPST_; ///< Time stamp in GPS time
+
+ public:
+ CaptureGnssTdcp(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector3d& _data,
+ const Eigen::Matrix3d& _data_covariance,
+ const FrameBasePtr& _origin_frame_ptr) :
+ CaptureBase("CaptureGnssTdcp", _ts, _sensor_ptr),
+ data_(_data),
+ data_covariance_(_data_covariance),
+ origin_frame_ptr_(_origin_frame_ptr)
+ {};
+ ~CaptureGnssTdcp() override{};
+ const Eigen::Vector3d& getData() const;
+ const Eigen::Matrix3d& getDataCovariance() const;
+ void getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const;
+ FrameBasePtr getOriginFrame() const;
+ const TimeStamp& getGpsTimeStamp() const;
+ void setGpsTimeStamp(const TimeStamp &_ts_GPST);
+};
+
+inline const Eigen::Vector3d& CaptureGnssTdcp::getData() const
+{
+ return data_;
+}
+
+inline const Eigen::Matrix3d& CaptureGnssTdcp::getDataCovariance() const
+{
+ return data_covariance_;
+}
+
+inline void CaptureGnssTdcp::getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const
+{
+ data = data_;
+ data_cov = data_covariance_;
+}
+
+inline FrameBasePtr CaptureGnssTdcp::getOriginFrame() const
+{
+ return origin_frame_ptr_;
+}
+
+inline const wolf::TimeStamp& CaptureGnssTdcp::getGpsTimeStamp() const
+{
+ return ts_GPST_;
+}
+
+inline void CaptureGnssTdcp::setGpsTimeStamp(const TimeStamp &_ts_GPST)
+{
+ ts_GPST_ = _ts_GPST;
+}
+
+} //namespace wolf
+#endif
diff --git a/include/gnss/factor/factor_gnss_fix_2D.h b/include/gnss/factor/factor_gnss_fix_2d.h
similarity index 69%
rename from include/gnss/factor/factor_gnss_fix_2D.h
rename to include/gnss/factor/factor_gnss_fix_2d.h
index 773a84d8b..26a8735be 100644
--- a/include/gnss/factor/factor_gnss_fix_2D.h
+++ b/include/gnss/factor/factor_gnss_fix_2d.h
@@ -1,6 +1,27 @@
-
-#ifndef FACTOR_GNSS_FIX_2D_H_
-#define FACTOR_GNSS_FIX_2D_H_
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#ifndef FACTOR_GNSS_FIX_2d_H_
+#define FACTOR_GNSS_FIX_2d_H_
//Wolf includes
#include "core/common/wolf.h"
@@ -10,37 +31,39 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorGnssFix2D);
+WOLF_PTR_TYPEDEFS(FactorGnssFix2d);
-class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, 1, 1>
+class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, 1, 1>
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
public:
- FactorGnssFix2D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2D",
- nullptr,
- nullptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _sensor_gnss_ptr->getStateBlock(0),
- _sensor_gnss_ptr->getEnuMapTranslation(),
- _sensor_gnss_ptr->getEnuMapRoll(),
- _sensor_gnss_ptr->getEnuMapPitch(),
- _sensor_gnss_ptr->getEnuMapYaw()),
+ FactorGnssFix2d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2d",
+ TOP_ABS,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapTranslation(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
- WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an active GNSS Fix 2D factor without initializing ENU");
+ WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an active GNSS Fix 2d factor without initializing ENU");
}
- virtual ~FactorGnssFix2D() = default;
+ ~FactorGnssFix2d() override = default;
template<typename T>
bool operator ()(const T* const _x,
@@ -55,7 +78,7 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1,
};
template<typename T>
-inline bool FactorGnssFix2D::operator ()(const T* const _x,
+inline bool FactorGnssFix2d::operator ()(const T* const _x,
const T* const _o,
const T* const _x_antena,
const T* const _t_ENU_map,
@@ -79,19 +102,19 @@ inline bool FactorGnssFix2D::operator ()(const T* const _x,
//std::cout << "getREnuEcef():\n" << sensor_gnss_ptr_->gettEnuEcef() << std::endl;
//std::cout << "gettEnuEcef(): " << sensor_gnss_ptr_->gettEnuEcef().transpose() << std::endl;
- // Transform ECEF 3D Fix Feature to 2D Fix in Map coordinates (removing z)
+ // Transform ECEF 3d Fix Feature to 2d Fix in Map coordinates (removing z)
//std::cout << "R_map_ENU:\n\t" << R_map_ENU(0,0) << "\t" << R_map_ENU(0,1) << "\t" << R_map_ENU(0,2) << "\n\t" << R_map_ENU(1,0) << "\t" << R_map_ENU(1,1) << "\t" << R_map_ENU(1,2) << std::endl;
//std::cout << "R_ENU_ECEF:\n\t" << R_ENU_ECEF(0,0) << "\t" << R_ENU_ECEF(0,1) << "\t" << R_ENU_ECEF(0,2) << "\n\t" << R_ENU_ECEF(1,0) << "\t" << R_ENU_ECEF(1,1) << "\t" << R_ENU_ECEF(1,2) << "\n\t" << R_ENU_ECEF(2,0) << "\t" << R_ENU_ECEF(2,1) << "\t" << R_ENU_ECEF(2,2) << std::endl;
//std::cout << "t_ENU_ECEF:\n\t" << t_ENU_ECEF(0) << "\n\t" << t_ENU_ECEF(1) << "\n\t" << t_ENU_ECEF(2) << std::endl;
Eigen::Matrix<T,2,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map);
- //std::cout << "fix 3D:\n\t" << getMeasurement()(0) << "\n\t" << getMeasurement()(1) << "\n\t" << getMeasurement()(2) << std::endl;
+ //std::cout << "fix 3d:\n\t" << getMeasurement()(0) << "\n\t" << getMeasurement()(1) << "\n\t" << getMeasurement()(2) << std::endl;
//std::cout << "fix_map:\n\t" << fix_map[0] << "\n\t" << fix_map[1] << std::endl;
// Antena position in Map coordinates
Eigen::Matrix<T,2,1> antena_map = R_map_base * t_base_antena.head(2) + t_map_base;
//std::cout << "antena_map:\n\t" << antena_map[0] << "\n\t" << antena_map[1] << std::endl;
- // Compute residual rotating information matrix to 2D Map coordinates
+ // Compute residual rotating information matrix to 2d Map coordinates
// Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
// In this case R = R_map_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_map_ENU'
residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_map_ENU.transpose()) * (antena_map - fix_map);
diff --git a/include/gnss/factor/factor_gnss_fix_3D.h b/include/gnss/factor/factor_gnss_fix_3d.h
similarity index 64%
rename from include/gnss/factor/factor_gnss_fix_3D.h
rename to include/gnss/factor/factor_gnss_fix_3d.h
index c620ee58a..650a1055f 100644
--- a/include/gnss/factor/factor_gnss_fix_3D.h
+++ b/include/gnss/factor/factor_gnss_fix_3d.h
@@ -1,6 +1,27 @@
-
-#ifndef FACTOR_GNSS_FIX_3D_H_
-#define FACTOR_GNSS_FIX_3D_H_
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#ifndef FACTOR_GNSS_FIX_3d_H_
+#define FACTOR_GNSS_FIX_3d_H_
//Wolf includes
#include "core/common/wolf.h"
@@ -9,37 +30,39 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorGnssFix3D);
+WOLF_PTR_TYPEDEFS(FactorGnssFix3d);
-class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1, 1, 1>
+class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, 1, 1>
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
public:
- FactorGnssFix3D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3D",
- nullptr,
- nullptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _sensor_gnss_ptr->getStateBlock(0),
- _sensor_gnss_ptr->getEnuMapTranslation(),
- _sensor_gnss_ptr->getEnuMapRoll(),
- _sensor_gnss_ptr->getEnuMapPitch(),
- _sensor_gnss_ptr->getEnuMapYaw()),
+ FactorGnssFix3d(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3d",
+ TOP_ABS,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapTranslation(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
- WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3D factor without initializing ENU");
+ WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3d factor without initializing ENU");
}
- virtual ~FactorGnssFix3D() = default;
+ ~FactorGnssFix3d() override = default;
template<typename T>
bool operator ()(const T* const _x,
@@ -54,7 +77,7 @@ class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1,
};
template<typename T>
-inline bool FactorGnssFix3D::operator ()(const T* const _x,
+inline bool FactorGnssFix3d::operator ()(const T* const _x,
const T* const _o,
const T* const _x_antena,
const T* const _t_ENU_map,
@@ -74,7 +97,7 @@ inline bool FactorGnssFix3D::operator ()(const T* const _x,
Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
Eigen::Matrix<T,3,1> fix_ECEF = getMeasurement().cast<T>();
- // Transform ECEF 3D Fix Feature to Map coordinates
+ // Transform ECEF 3d Fix Feature to Map coordinates
Eigen::Matrix<T,3,1> fix_map = R_map_ENU * (R_ENU_ECEF * fix_ECEF + t_ENU_ECEF - t_ENU_map);
// Antena position in Map coordinates
diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
new file mode 100644
index 000000000..ac1825f5a
--- /dev/null
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -0,0 +1,190 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_GNSS_PSEUDO_RANGE_H_
+#define FACTOR_GNSS_PSEUDO_RANGE_H_
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/capture/capture_gnss.h"
+#include "gnss/feature/feature_gnss_satellite.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorGnssPseudoRange);
+
+class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>
+{
+ protected:
+ SensorGnssPtr sensor_gnss_ptr_;
+ Eigen::Vector3d satellite_ENU_, satellite_ECEF_;
+ mutable double sagnac_correction_;
+ mutable bool sagnac_set_;
+ bool not_GPS_;
+
+ public:
+
+ FactorGnssPseudoRange(FeatureGnssSatellitePtr& _ftr_ptr,
+ const SensorGnssPtr& _sensor_gnss_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>("FactorGnssPseudoRange",
+ TOP_ABS,
+ _ftr_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getStateBlock('T'),
+ (_ftr_ptr->getSatellite().sys == SYS_GLO ?
+ _ftr_ptr->getCapture()->getStateBlock('G') :
+ (_ftr_ptr->getSatellite().sys == SYS_GAL ?
+ _ftr_ptr->getCapture()->getStateBlock('E') :
+ _ftr_ptr->getCapture()->getStateBlock('M'))),//in case GPS, M is set but anyway not used
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapTranslation(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
+ sensor_gnss_ptr_(_sensor_gnss_ptr),
+ satellite_ENU_(sensor_gnss_ptr_->getREnuEcef() * _ftr_ptr->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()),
+ satellite_ECEF_(_ftr_ptr->getSatellite().pos),
+ sagnac_correction_(0),
+ sagnac_set_(false),
+ not_GPS_(_ftr_ptr->getSatellite().sys != SYS_GPS)
+ {
+ //WOLF_INFO("FactorPseudoRange: sat ", _ftr_ptr->getSatellite().sat,"\n\tpos = ", _ftr_ptr->getSatellite().pos.transpose(), "\n\tprange = ", _ftr_ptr->getMeasurement());
+ WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating FactorGnssPseudoRange without initializing ENU");
+ }
+
+ ~FactorGnssPseudoRange() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _x,
+ const T* const _o,
+ const T* const _clock_bias,
+ const T* const _clock_bias_constellation,
+ const T* const _x_antena,
+ const T* const _t_ENU_map,
+ const T* const _roll_ENU_map,
+ const T* const _pitch_ENU_map,
+ const T* const _yaw_ENU_map,
+ T* _residual) const;
+
+ template<typename T>
+ void initSagnac(const Eigen::Matrix<T,3,1>& antena_ENU) const;
+
+};
+
+template<typename T>
+inline bool FactorGnssPseudoRange::operator ()(const T* const _x,
+ const T* const _o,
+ const T* const _clock_bias,
+ const T* const _clock_bias_constellation,
+ const T* const _x_antena,
+ const T* const _t_ENU_map,
+ const T* const _roll_ENU_map,
+ const T* const _pitch_ENU_map,
+ const T* const _yaw_ENU_map,
+ T* _residual) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base(_x);
+ Eigen::Map<const Eigen::Quaternion<T> > q_map_base(_o);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
+
+ /* NOT EFFICIENT IMPLE;ENTATION
+
+ Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
+
+ // Antenna position in ENU coordinates
+ Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + R_ENU_map * (t_map_base + q_map_base * t_base_antena);
+ */
+
+ /* SAGNAC EFFECT CORRECTION
+ * It should be recomputed with the new antenna position in ECEF coordinates,
+ * but it is multiplied by a factor of 1e-14 (earth rotation / CLIGHT).
+ * We use instead a precomputed sagnac effect taking the first value of antenna position
+ */
+ if (!sagnac_set_)
+ {
+ Eigen::Matrix<T,3,1> antena_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena);
+ initSagnac(antena_ENU);
+ }
+
+ // Expected pseudo-range
+ T exp = (t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base + q_map_base * t_base_antena) - satellite_ENU_.cast<T>()).norm() + // norm
+ sagnac_correction_ + // sagnac correction (precomputed)
+ _clock_bias[0] + // receiver clock bias (w.r.t. GPS clock)
+ (not_GPS_ ? _clock_bias_constellation[0] : T(0)); // interconstellation clock bias
+ /*
+ T exp = (antena_ENU-satellite_ENU_.cast<T>()).norm() + // norm
+ sagnac_correction_ + // sagnac correction (precomputed)
+ _clock_bias[0] + // receiver clock bias (w.r.t. GPS clock)
+ (not_GPS_ ? _clock_bias_constellation[0] : T(0)); // interconstellation clock bias
+ */
+
+ //std::cout << "sat " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl;
+ //std::cout << "\tsys " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sys << std::endl;
+ //std::cout << std::setprecision(13) <<"\tsat_pos= " << satellite_ENU_.transpose() << "\n";
+ //std::cout << "\tx = " << antena_ENU(0) << " "<< antena_ENU(1) << " "<< antena_ENU(2) << "\n";
+ //std::cout << "\tprange = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().p << "\n";
+ //std::cout << "\tiono_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().iono_corr << "\n";
+ //std::cout << "\ttropo_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().tropo_corr << "\n";
+ //std::cout << "\tsat_clock_corr = " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getPseudoRange().sat_clock_corr << "\n";
+ //std::cout << "\tcorrected prange = " << getMeasurement() << "\n";
+ //std::cout << "\tclock_bias = " << _clock_bias[0] << " " << _clock_bias_constellation[0] << "\n";
+ //std::cout << "\tnorm = " << (antena_ENU-satellite_ENU_.cast<T>()).norm() + sagnac_correction_ << "\n";
+ //std::cout << "\texp_wo = " << (antena_ENU-satellite_ENU_.cast<T>()).norm() + sagnac_correction_ + _clock_bias[0] << "\n";
+ //std::cout << "\texp = " << exp << "\n";
+ //std::cout << "\terror = " << exp - getMeasurement()(0) << "\n";
+
+ // Residual
+ _residual[0] = (exp - getMeasurement()(0)) * getMeasurementSquareRootInformationUpper()(0,0);
+
+ return true;
+}
+
+template<typename T>
+void FactorGnssPseudoRange::initSagnac(const Eigen::Matrix<T,3,1>& antena_ENU) const
+{
+}
+
+template<>
+void FactorGnssPseudoRange::initSagnac<double>(const Eigen::Matrix<double,3,1>& antena_ENU) const
+{
+ Eigen::Vector3d antena_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_ENU - sensor_gnss_ptr_->gettEnuEcef());
+ sagnac_correction_ = GnssUtils::computeSagnacCorrection(antena_ECEF, satellite_ECEF_);
+ sagnac_set_ = true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/gnss/factor/factor_gnss_single_diff_2d.h b/include/gnss/factor/factor_gnss_single_diff_2d.h
new file mode 100644
index 000000000..f85495b05
--- /dev/null
+++ b/include/gnss/factor/factor_gnss_single_diff_2d.h
@@ -0,0 +1,126 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#ifndef FACTOR_GNSS_SINGLE_DIFF_2d_H_
+#define FACTOR_GNSS_SINGLE_DIFF_2d_H_
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "gnss/sensor/sensor_gnss.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorGnssSingleDiff2d);
+
+class FactorGnssSingleDiff2d : public FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>
+{
+ protected:
+ SensorGnssPtr sensor_gnss_ptr_;
+
+ public:
+
+ FactorGnssSingleDiff2d(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGnssSingleDiff2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2d",
+ TOP_GEOM,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
+ sensor_gnss_ptr_(_sensor_gnss_ptr)
+ {
+ WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2d factor without initializing ENU");
+ }
+
+ virtual ~FactorGnssSingleDiff2d() = default;
+
+ template<typename T>
+ bool operator ()(const T* const _x1,
+ const T* const _o1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _x_antena,
+ const T* const _roll_ENU_MAP,
+ const T* const _pitch_ENU_MAP,
+ const T* const _yaw_ENU_MAP,
+ T* _residuals) const;
+
+};
+
+template<typename T>
+inline bool FactorGnssSingleDiff2d::operator ()(const T* const _x1,
+ const T* const _o1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _x_antena,
+ const T* const _roll_ENU_MAP,
+ const T* const _pitch_ENU_MAP,
+ const T* const _yaw_ENU_MAP,
+ T* _residuals) const
+{
+ Eigen::Map<const Eigen::Matrix<T,2,1> > t_MAP_BASE1(_x1);
+ Eigen::Matrix<T,2,2> R_MAP_BASE1 = Eigen::Rotation2D<T>(_o1[0]).matrix();
+ Eigen::Map<const Eigen::Matrix<T,2,1> > t_MAP_BASE2(_x2);
+ Eigen::Matrix<T,2,2> R_MAP_BASE2 = Eigen::Rotation2D<T>(_o2[0]).matrix();
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_BASE_ANTENA(_x_antena);
+ Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
+
+ Eigen::Matrix<T,3,3> R_ENU_ECEF = sensor_gnss_ptr_->getREnuEcef().cast<T>();
+ Eigen::Matrix<T,2,3> R_MAP_ENU = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]).transpose().topRows(2);
+
+ // Transform ECEF 3d SingleDiff Feature to 2d SingleDiff in Map coordinates (removing z)
+ Eigen::Matrix<T,2,1> measured_diff_2d_MAP = R_MAP_ENU * (R_ENU_ECEF * getMeasurement().cast<T>());
+
+ // Substraction of expected antena positions in Map coordinates
+ Eigen::Matrix<T,2,1> expected_diff_2d_MAP = (R_MAP_BASE2 * t_BASE_ANTENA.head(2) + t_MAP_BASE2) - (R_MAP_BASE1 * t_BASE_ANTENA.head(2) + t_MAP_BASE1);
+
+ // Compute residual rotating information matrix to 2d Map coordinates
+ // Covariance & information are rotated by R*S*R' so for the squred root upper (or right) R*L*U*R'->U*R'
+ // In this case R = R_2dMAP_ENU * R_ENU_ECEF, then R' = R_ENU_ECEF' * R_2dMAP_ENU'
+ residuals_ECEF = (getMeasurementSquareRootInformationUpper().cast<T>() * R_ENU_ECEF.transpose() * R_MAP_ENU.transpose()) * (expected_diff_2d_MAP - measured_diff_2d_MAP);
+
+ //std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _o1[0] << std::endl;
+ //std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _o2[0] << std::endl;
+ //std::cout << "antena: " << _x_antena[0] << " " << _x_antena[1] << " " << _x_antena[2] << std::endl;
+ //std::cout << "RPY: " << _roll[0] << " " << _pitch[1] << " " << _yaw[2] << std::endl;
+ //std::cout << "measured_diff_2d: " << measured_diff_2d[0] << " " << measured_diff_2d[1] << std::endl;
+ //std::cout << "expected_diff_2d: " << expected_diff_2d[0] << " " << expected_diff_2d[1] << std::endl;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h
new file mode 100644
index 000000000..fb19510fb
--- /dev/null
+++ b/include/gnss/factor/factor_gnss_tdcp.h
@@ -0,0 +1,213 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_GNSS_TDCP_H_
+#define FACTOR_GNSS_TDCP_H_
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/capture/capture_gnss.h"
+#include "gnss/feature/feature_gnss_satellite.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorGnssTdcp);
+
+class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>
+{
+ protected:
+ SensorGnssPtr sensor_gnss_ptr_;
+ mutable double d_pseudo_range_;
+ mutable bool sagnac_set_;
+ double std_dev_;
+ Eigen::Vector3d satellite_ENU_k_, satellite_ENU_r_;
+
+ public:
+
+ FactorGnssTdcp(const double& _std_dev,
+ FeatureGnssSatellitePtr& _ftr_r,
+ FeatureGnssSatellitePtr& _ftr_k,
+ const SensorGnssPtr& _sensor_gnss_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>("FactorGnssTdcp",
+ TOP_GEOM,
+ _ftr_k,
+ nullptr,
+ _ftr_r->getCapture(),
+ _ftr_r,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_r->getFrame()->getP(),
+ _ftr_r->getFrame()->getO(),
+ _ftr_r->getCapture()->getStateBlock('T'),
+ _ftr_k->getFrame()->getP(),
+ _ftr_k->getFrame()->getO(),
+ _ftr_k->getCapture()->getStateBlock('T'),
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapTranslation(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
+ sensor_gnss_ptr_(_sensor_gnss_ptr),
+ sagnac_set_(false),
+ std_dev_(_std_dev),
+ satellite_ENU_k_(sensor_gnss_ptr_->getREnuEcef() * _ftr_k->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef()),
+ satellite_ENU_r_(sensor_gnss_ptr_->getREnuEcef() * _ftr_r->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef())
+ {
+ assert(_ftr_r != _ftr_k);
+ assert(_ftr_r->getCapture()->getStateBlock('T') != nullptr);
+ assert(_ftr_k->getCapture()->getStateBlock('T') != nullptr);
+
+ WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an FactorGnssTdcp without initializing ENU");
+
+ d_pseudo_range_ = _ftr_k->getRange().L_corrected - _ftr_r->getRange().L_corrected;
+ }
+
+ ~FactorGnssTdcp() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _x_r,
+ const T* const _o_r,
+ const T* const _clock_bias_r,
+ const T* const _x_k,
+ const T* const _o_k,
+ const T* const _clock_bias_k,
+ const T* const _x_antena,
+ const T* const _t_ENU_map,
+ const T* const _roll_ENU_map,
+ const T* const _pitch_ENU_map,
+ const T* const _yaw_ENU_map,
+ T* _residual) const;
+
+ template<typename T>
+ void initSagnac(const Eigen::Matrix<T,3,1>& antena_r_ENU, const Eigen::Matrix<T,3,1>& antena_k_ENU) const;
+
+};
+
+template<typename T>
+inline bool FactorGnssTdcp::operator ()(const T* const _x_r,
+ const T* const _o_r,
+ const T* const _clock_bias_r,
+ const T* const _x_k,
+ const T* const _o_k,
+ const T* const _clock_bias_k,
+ const T* const _x_antena,
+ const T* const _t_ENU_map,
+ const T* const _roll_ENU_map,
+ const T* const _pitch_ENU_map,
+ const T* const _yaw_ENU_map,
+ T* _residual) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base_r(_x_r);
+ Eigen::Map<const Eigen::Quaternion<T> > q_map_base_r(_o_r);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_map_base_k(_x_k);
+ Eigen::Map<const Eigen::Quaternion<T> > q_map_base_k(_o_k);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_base_antena(_x_antena);
+ Eigen::Map<const Eigen::Matrix<T,3,1> > t_ENU_map(_t_ENU_map);
+
+ Eigen::Matrix<T,3,3> R_ENU_map = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]);
+
+ /* INEFFICIENT IMPLEMENTATION
+ // Antenna position in ECEF coordinates
+ Eigen::Matrix<T,3,1> antena_r_ENU = t_ENU_map + R_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena);
+ Eigen::Matrix<T,3,1> antena_k_ENU = t_ENU_map + R_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena);
+
+ // Expected tdcp
+ T exp = (antena_k_ENU-satellite_ENU_k_.cast<T>()).norm()
+ + _clock_bias_k[0]
+ - (antena_r_ENU-satellite_ENU_r_.cast<T>()).norm()
+ - _clock_bias_r[0] ;
+ */
+ // Expected tdcp
+ //T exp = (t_ENU_map + R_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena)-satellite_ENU_k_.cast<T>()).norm()
+ // + _clock_bias_k[0]
+ // - (t_ENU_map + R_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena)-satellite_ENU_r_.cast<T>()).norm()
+ // - _clock_bias_r[0];
+
+ //std::cout << "sat_r " << std::static_pointer_cast<FeatureGnssSatellite>(getFeatureOther())->getSatellite().sat << std::endl;
+ //std::cout << "sat_k " << std::static_pointer_cast<FeatureGnssSatellite>(getFeature())->getSatellite().sat << std::endl;
+ //std::cout << std::setprecision(13) <<"\tsat_r_pos= " << satellite_ENU_r_.transpose() << "\n";
+ //std::cout << "\tsat_k_pos= " << satellite_ENU_k_.transpose() << "\n";
+ //std::cout << "\tantena_r_ENU = " << antena_r_ENU(0) << " "<< antena_r_ENU(1) << " "<< antena_r_ENU(2) << "\n";
+ //std::cout << "\tantena_k_ENU = " << antena_k_ENU(0) << " "<< antena_k_ENU(1) << " "<< antena_k_ENU(2) << "\n";
+ //std::cout << "\tdiff_range = " << d_pseudo_range_ << "\n";
+ //std::cout << "\tclock_bias_r = " << _clock_bias_r[0] << "\n";
+ //std::cout << "\tclock_bias_k = " << _clock_bias_k[0] << "\n";
+ //std::cout << "\tnorm_r = " << (antena_r_ENU-satellite_ENU_r_.cast<T>()).norm() << "\n";
+ //std::cout << "\tnorm_k = " << (antena_k_ENU-satellite_ENU_k_.cast<T>()).norm() << "\n";
+ //std::cout << "\tnorm_r (t corrected) = " << (antena_r_ENU-satellite_ENU_r_.cast<T>()).norm() + _clock_bias_r[0] << "\n";
+ //std::cout << "\tnorm_k (t corrected) = " << (antena_k_ENU-satellite_ENU_k_.cast<T>()).norm() + _clock_bias_k[0] << "\n";
+ //std::cout << "\texp = " << exp << "\n";
+ //std::cout << "\terror = " << exp - d_pseudo_range_ << "\n";
+
+ /* SAGNAC EFFECT CORRECTION
+ * It should be recomputed with the new antenna position in ECEF coordinates,
+ * but it is multiplied by a factor of 1e-14 (earth rotation / CLIGHT).
+ * We use instead a precomputed sagnac effect taking the first values of antenna position
+ */
+ if (not sagnac_set_)
+ {
+ Eigen::Matrix<T,3,1> antena_r_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_r + q_map_base_r * t_base_antena);
+ Eigen::Matrix<T,3,1> antena_k_ENU = t_ENU_map + sensor_gnss_ptr_->computeREnuMap(_roll_ENU_map[0], _pitch_ENU_map[0], _yaw_ENU_map[0]) * (t_map_base_k + q_map_base_k * t_base_antena);
+ initSagnac(antena_r_ENU, antena_k_ENU);
+ }
+
+ // Residual
+ _residual[0] = ((t_ENU_map + R_ENU_map * (t_map_base_k + q_map_base_k * t_base_antena)-satellite_ENU_k_.cast<T>()).norm()
+ + _clock_bias_k[0]
+ - (t_ENU_map + R_ENU_map * (t_map_base_r + q_map_base_r * t_base_antena)-satellite_ENU_r_.cast<T>()).norm()
+ - _clock_bias_r[0]
+ - d_pseudo_range_) / std_dev_;
+
+ return true;
+}
+
+template<typename T>
+void FactorGnssTdcp::initSagnac(const Eigen::Matrix<T,3,1>& antena_r_ENU, const Eigen::Matrix<T,3,1>& antena_k_ENU) const
+{
+}
+
+template<>
+void FactorGnssTdcp::initSagnac<double>(const Eigen::Matrix<double,3,1>& antena_r_ENU, const Eigen::Matrix<double,3,1>& antena_k_ENU) const
+{
+ Eigen::Vector3d antena_r_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_r_ENU - sensor_gnss_ptr_->gettEnuEcef());
+ Eigen::Vector3d sat_r_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (satellite_ENU_r_ - sensor_gnss_ptr_->gettEnuEcef());
+ double sagnac_corr_r = GnssUtils::computeSagnacCorrection(antena_r_ECEF, sat_r_ECEF);
+
+ Eigen::Vector3d antena_k_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (antena_k_ENU - sensor_gnss_ptr_->gettEnuEcef());
+ Eigen::Vector3d sat_k_ECEF = sensor_gnss_ptr_->getREnuEcef().transpose() * (satellite_ENU_k_ - sensor_gnss_ptr_->gettEnuEcef());
+ double sagnac_corr_k = GnssUtils::computeSagnacCorrection(antena_k_ECEF, sat_k_ECEF);
+
+ d_pseudo_range_ += -sagnac_corr_k + sagnac_corr_r;
+
+ sagnac_set_ = true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/gnss/factor/factor_gnss_single_diff_2D.h b/include/gnss/factor/factor_gnss_tdcp_2d.h
similarity index 70%
rename from include/gnss/factor/factor_gnss_single_diff_2D.h
rename to include/gnss/factor/factor_gnss_tdcp_2d.h
index 3cadfcb24..049106a4d 100644
--- a/include/gnss/factor/factor_gnss_single_diff_2D.h
+++ b/include/gnss/factor/factor_gnss_tdcp_2d.h
@@ -1,6 +1,27 @@
-
-#ifndef FACTOR_GNSS_SINGLE_DIFF_2D_H_
-#define FACTOR_GNSS_SINGLE_DIFF_2D_H_
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#ifndef FACTOR_GNSS_TDCP_2D_H_
+#define FACTOR_GNSS_TDCP_2D_H_
//Wolf includes
#include "core/common/wolf.h"
@@ -10,38 +31,40 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorGnssSingleDiff2D);
+WOLF_PTR_TYPEDEFS(FactorGnssTdcp2d);
-class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3, 2, 1, 2, 1, 3, 1, 1, 1>
+class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
public:
- FactorGnssSingleDiff2D(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGnssSingleDiff2D, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2D",
- _frame_other_ptr,
- nullptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _frame_other_ptr->getP(),
- _frame_other_ptr->getO(),
- _ftr_ptr->getFrame()->getP(),
- _ftr_ptr->getFrame()->getO(),
- _sensor_gnss_ptr->getStateBlock(0),
- _sensor_gnss_ptr->getEnuMapRoll(),
- _sensor_gnss_ptr->getEnuMapPitch(),
- _sensor_gnss_ptr->getEnuMapYaw()),
+ FactorGnssTdcp2d(const FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>("FactorGnssTdcp2d",
+ TOP_GEOM,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU");
}
- virtual ~FactorGnssSingleDiff2D() = default;
+ ~FactorGnssTdcp2d() override = default;
template<typename T>
bool operator ()(const T* const _x1,
@@ -57,7 +80,7 @@ class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3,
};
template<typename T>
-inline bool FactorGnssSingleDiff2D::operator ()(const T* const _x1,
+inline bool FactorGnssTdcp2d::operator ()(const T* const _x1,
const T* const _o1,
const T* const _x2,
const T* const _o2,
diff --git a/include/gnss/factor/factor_gnss_tdcp_3d.h b/include/gnss/factor/factor_gnss_tdcp_3d.h
new file mode 100644
index 000000000..f55c668ec
--- /dev/null
+++ b/include/gnss/factor/factor_gnss_tdcp_3d.h
@@ -0,0 +1,127 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#ifndef FACTOR_GNSS_TDCP_3D_H_
+#define FACTOR_GNSS_TDCP_3D_H_
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "gnss/sensor/sensor_gnss.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorGnssTdcp3d);
+
+class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>
+{
+ protected:
+ SensorGnssPtr sensor_gnss_ptr_;
+
+ public:
+
+ FactorGnssTdcp3d(const FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_other_ptr,
+ const SensorGnssPtr& _sensor_gnss_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>("FactorGnssTdcp3d",
+ TOP_GEOM,
+ _ftr_ptr,
+ _frame_other_ptr,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _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");
+ WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
+ }
+
+ ~FactorGnssTdcp3d() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _x1,
+ const T* const _o1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _x_antena,
+ const T* const _roll_ENU_MAP,
+ const T* const _pitch_ENU_MAP,
+ const T* const _yaw_ENU_MAP,
+ T* _residuals) const;
+
+};
+
+template<typename T>
+inline bool FactorGnssTdcp3d::operator ()(const T* const _x1,
+ const T* const _o1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _x_antena,
+ const T* const _roll_ENU_MAP,
+ const T* const _pitch_ENU_MAP,
+ const T* const _yaw_ENU_MAP,
+ T* _residuals) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1>> t_MAP_BASE1(_x1);
+ Eigen::Map<const Eigen::Quaternion<T>> q_MAP_BASE1(_o1);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> t_MAP_BASE2(_x2);
+ Eigen::Map<const Eigen::Quaternion<T>> q_MAP_BASE2(_o2);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> t_BASE_ANTENA(_x_antena);
+ Eigen::Map<Eigen::Matrix<T,3,1> > residuals_ECEF(_residuals);
+
+ Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().transpose().cast<T>();
+ Eigen::Matrix<T,3,3> R_ENU_MAP = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]);
+
+ // Expected displacement
+ Eigen::Matrix<T,3,1> expected_ECEF = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
+
+ // Compute residual
+ residuals_ECEF = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_ECEF - getMeasurement().cast<T>());
+
+ //std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _x1[2] << " " << _o1[0] << " " << _o1[1] << " " << _o1[2] << " " << _o1[3] << std::endl;
+ //std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _x2[2] << " " << _o2[0] << " " << _o2[1] << " " << _o2[2] << " " << _o2[3] << std::endl;
+ //std::cout << "antena: " << _x_antena[0] << " " << _x_antena[1] << " " << _x_antena[2] << std::endl;
+ //std::cout << "RPY: " << _roll[0] << " " << _pitch[1] << " " << _yaw[2] << std::endl;
+ //std::cout << "expected_ECEF: " << expected_ECEF[0] << " " << expected_ECEF[1] << " " << expected_ECEF[2] << std::endl;
+ //std::cout << "getMeasurement(): " << getMeasurement().transpose << std::endl;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/gnss/factor/factor_gnss_tdcp_batch.h b/include/gnss/factor/factor_gnss_tdcp_batch.h
new file mode 100644
index 000000000..1c0e9ecff
--- /dev/null
+++ b/include/gnss/factor/factor_gnss_tdcp_batch.h
@@ -0,0 +1,139 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+
+#ifndef FACTOR_GNSS_TDCP_BATCH_H_
+#define FACTOR_GNSS_TDCP_BATCH_H_
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/factor/factor_autodiff.h"
+#include "gnss/sensor/sensor_gnss.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorGnssTdcpBatch);
+
+class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>
+{
+ protected:
+ SensorGnssPtr sensor_gnss_ptr_;
+
+ public:
+
+ FactorGnssTdcpBatch(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",
+ TOP_GEOM,
+ _ftr_ptr,
+ _capture_other_ptr->getFrame(),
+ _capture_other_ptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _capture_other_ptr->getFrame()->getP(),
+ _capture_other_ptr->getFrame()->getO(),
+ _capture_other_ptr->getStateBlock('T'),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getStateBlock('T'),
+ _sensor_gnss_ptr->getP(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _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");
+ WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
+ }
+
+ ~FactorGnssTdcpBatch() override = default;
+
+ template<typename T>
+ bool operator ()(const T* const _x1,
+ const T* const _o1,
+ const T* const _t1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _t2,
+ const T* const _x_antena,
+ const T* const _roll_ENU_MAP,
+ const T* const _pitch_ENU_MAP,
+ const T* const _yaw_ENU_MAP,
+ T* _residuals) const;
+
+};
+
+template<typename T>
+inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
+ const T* const _o1,
+ const T* const _t1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _t2,
+ const T* const _x_antena,
+ const T* const _roll_ENU_MAP,
+ const T* const _pitch_ENU_MAP,
+ const T* const _yaw_ENU_MAP,
+ T* _residuals) const
+{
+ Eigen::Map<const Eigen::Matrix<T,3,1>> t_MAP_BASE1(_x1);
+ Eigen::Map<const Eigen::Quaternion<T>> q_MAP_BASE1(_o1);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> t_MAP_BASE2(_x2);
+ Eigen::Map<const Eigen::Quaternion<T>> q_MAP_BASE2(_o2);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> t_BASE_ANTENA(_x_antena);
+ Eigen::Map<Eigen::Matrix<T,3,1> > disp_residuals(_residuals);
+ Eigen::Map<Eigen::Matrix<T,4,1> > residuals(_residuals);
+
+ Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().transpose().cast<T>();
+ Eigen::Matrix<T,3,3> R_ENU_MAP = sensor_gnss_ptr_->computeREnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]);
+
+ // Expected d
+ Eigen::Matrix<T,4,1> exp;
+
+ // Expected displacement in ECEF
+ exp.head(3) = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
+
+ // clock error
+ exp(3) = *_t2 - *_t1;
+
+ // Compute residual
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (exp - getMeasurement().cast<T>());
+
+ //std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _x1[2] << " " << _o1[0] << " " << _o1[1] << " " << _o1[2] << " " << _o1[3] << std::endl;
+ //std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _x2[2] << " " << _o2[0] << " " << _o2[1] << " " << _o2[2] << " " << _o2[3] << std::endl;
+ //std::cout << "antena: " << _x_antena[0] << " " << _x_antena[1] << " " << _x_antena[2] << std::endl;
+ //std::cout << "RPY: " << _roll[0] << " " << _pitch[1] << " " << _yaw[2] << std::endl;
+ //std::cout << "expected_ECEF: " << expected_ECEF[0] << " " << expected_ECEF[1] << " " << expected_ECEF[2] << std::endl;
+ //std::cout << "getMeasurement(): " << getMeasurement().transpose << std::endl;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/gnss/feature/feature_gnss_fix.h b/include/gnss/feature/feature_gnss_fix.h
index f33fb8557..60f8debf6 100644
--- a/include/gnss/feature/feature_gnss_fix.h
+++ b/include/gnss/feature/feature_gnss_fix.h
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
#ifndef FEATURE_GNSS_FIX_H_
#define FEATURE_GNSS_FIX_H_
@@ -5,11 +26,13 @@
#include "core/feature/feature_base.h"
//std includes
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/rcv_position.h"
namespace wolf {
WOLF_PTR_TYPEDEFS(FeatureGnssFix);
-
+
//class FeatureGnssFix
class FeatureGnssFix : public FeatureBase
{
@@ -21,11 +44,20 @@ class FeatureGnssFix : public FeatureBase
* \param _meas_covariance the noise of the measurement
*
*/
- FeatureGnssFix(const Eigen::Vector3s& _measurement, const Eigen::Matrix3s& _meas_covariance) :
- FeatureBase("FeatureGnssFix", _measurement, _meas_covariance)
- {};
+ FeatureGnssFix(const GnssUtils::ComputePosOutput& _gnss_fix_output);
+
+ ~FeatureGnssFix() override{};
+
+ private:
+ GnssUtils::ComputePosOutput gnss_fix_output_;
- virtual ~FeatureGnssFix(){};
+};
+
+inline FeatureGnssFix::FeatureGnssFix(const GnssUtils::ComputePosOutput& _gnss_fix_output) :
+ FeatureBase("FeatureGnssFix", _gnss_fix_output.pos, _gnss_fix_output.pos_covar),
+ gnss_fix_output_(_gnss_fix_output)
+{
+ //
};
} // namespace wolf
diff --git a/include/gnss/feature/feature_gnss_satellite.h b/include/gnss/feature/feature_gnss_satellite.h
new file mode 100644
index 000000000..ff98d7177
--- /dev/null
+++ b/include/gnss/feature/feature_gnss_satellite.h
@@ -0,0 +1,101 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FEATURE_GNSS_SATELLITE_H_
+#define FEATURE_GNSS_SATELLITE_H_
+
+//Wolf includes
+#include "core/feature/feature_base.h"
+
+//std includes
+#include "gnss_utils/range.h"
+#include "gnss_utils/utils/satellite.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FeatureGnssSatellite);
+WOLF_LIST_TYPEDEFS(FeatureGnssSatellite);
+
+//class FeatureGnssSatellite
+class FeatureGnssSatellite : public FeatureBase
+{
+ public:
+
+ /** \brief Constructor
+ *
+ * \param _obs_sat satellite observation in rtklib format
+ *
+ * This constructor will take the pseudorange as measurement with 1 m² of variance
+ *
+ */
+ FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat, const GnssUtils::Range& _range);
+
+ ~FeatureGnssSatellite() override{};
+
+ const obsd_t& getObservation() const;
+ int satNumber() const;
+ const GnssUtils::Satellite& getSatellite() const;
+ const GnssUtils::Range& getRange() const;
+
+ private:
+ obsd_t obs_sat_;
+ GnssUtils::Satellite sat_;
+ GnssUtils::Range range_;
+};
+
+
+} // namespace wolf
+
+// IMPLEMENTATION
+namespace wolf {
+
+inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat, const GnssUtils::Range& _range) :
+ FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_range.P_corrected), Eigen::Matrix1d(_range.P_var)),
+ obs_sat_(_obs_sat),
+ sat_(_sat),
+ range_(_range)
+{
+ //
+}
+
+inline const obsd_t& FeatureGnssSatellite::getObservation() const
+{
+ return obs_sat_;
+}
+
+inline int FeatureGnssSatellite::satNumber() const
+{
+ return obs_sat_.sat;
+}
+
+inline const GnssUtils::Satellite& FeatureGnssSatellite::getSatellite() const
+{
+ return sat_;
+}
+
+inline const GnssUtils::Range& FeatureGnssSatellite::getRange() const
+{
+ return range_;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/gnss/feature/feature_gnss_single_diff.h b/include/gnss/feature/feature_gnss_single_diff.h
deleted file mode 100644
index 903a062ce..000000000
--- a/include/gnss/feature/feature_gnss_single_diff.h
+++ /dev/null
@@ -1,33 +0,0 @@
-#ifndef FEATURE_GNSS_SINGLE_DIFF_H_
-#define FEATURE_GNSS_SINGLE_DIFF_H_
-
-//Wolf includes
-#include "core/feature/feature_base.h"
-
-//std includes
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FeatureGnssSinleDiff);
-
-//class FeatureGnssSingleDiff
-class FeatureGnssSingleDiff : public FeatureBase
-{
- public:
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureGnssSingleDiff(const Eigen::Vector3s& _measurement, const Eigen::Matrix3s& _meas_covariance) :
- FeatureBase("GNSS SINGLE DIFFERENCE", _measurement, _meas_covariance)
- {};
-
- virtual ~FeatureGnssSingleDiff(){};
-};
-
-} // namespace wolf
-
-#endif
diff --git a/include/gnss/feature/feature_gnss_tdcp.h b/include/gnss/feature/feature_gnss_tdcp.h
new file mode 100644
index 000000000..1af89dda3
--- /dev/null
+++ b/include/gnss/feature/feature_gnss_tdcp.h
@@ -0,0 +1,56 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FEATURE_GNSS_TDCP_H_
+#define FEATURE_GNSS_TDCP_H_
+
+//Wolf includes
+#include "core/feature/feature_base.h"
+
+//std includes
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FeatureGnssSinleDiff);
+
+class FeatureGnssTdcp : public FeatureBase
+{
+ public:
+
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ *
+ */
+ FeatureGnssTdcp(const Eigen::Vector4d& _measurement, const Eigen::Matrix4d& _meas_covariance) :
+ FeatureBase("FeatureGnssTdcp", _measurement, _meas_covariance)
+ {};
+ FeatureGnssTdcp(const Eigen::Vector3d& _measurement, const Eigen::Matrix3d& _meas_covariance) :
+ FeatureBase("FeatureGnssTdcp", _measurement, _meas_covariance)
+ {};
+
+ ~FeatureGnssTdcp() override{};
+};
+
+} // namespace wolf
+
+#endif
diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index ebeeefab5..1fa78ddd3 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -1,39 +1,122 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
#ifndef WOLF_PROCESSOR_GNSS_FIX_H
#define WOLF_PROCESSOR_GNSS_FIX_H
// Wolf includes
#include "core/processor/processor_base.h"
-#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/capture/capture_gnss.h"
#include "gnss/sensor/sensor_gnss.h"
// Std includes
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/rcv_position.h"
namespace wolf {
-
+
WOLF_PTR_TYPEDEFS(ProcessorGnssFix);
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssFix);
-
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorGnssFix);
-struct ProcessorParamsGnssFix : public ProcessorParamsBase
+struct ParamsProcessorGnssFix : public ParamsProcessorBase
{
- Scalar time_th;
- Scalar dist_traveled;
- Scalar enu_map_init_dist_min;
- ProcessorParamsGnssFix() = default;
- ProcessorParamsGnssFix(std::string _unique_name, const ParamsServer& _server):
- ProcessorParamsBase(_unique_name, _server)
+ bool fix_from_raw, init_enu_map, remove_outliers;
+ GnssUtils::Options compute_pos_opt;
+ double max_time_span;
+ double dist_traveled;
+ double enu_map_init_dist_min, enu_map_init_dist_max;
+ double enu_map_fix_dist;
+ double outlier_residual_th;
+
+ ParamsProcessorGnssFix() = default;
+ ParamsProcessorGnssFix(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server)
{
- time_th = _server.getParam<Scalar>(_unique_name + "/time_th");
- dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled");
- enu_map_init_dist_min = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
+ max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span");
+ dist_traveled = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/dist_traveled");
+ init_enu_map = _server.getParam<bool> (prefix + _unique_name + "/init_enu_map");
+ if (init_enu_map)
+ {
+ enu_map_init_dist_min = _server.getParam<double> (prefix + _unique_name + "/enu_map_init_dist_min");
+ enu_map_init_dist_max = _server.getParam<double> (prefix + _unique_name + "/enu_map_init_dist_max");
+ }
+ enu_map_fix_dist = _server.getParam<double> (prefix + _unique_name + "/enu_map_fix_dist");
+ fix_from_raw = _server.getParam<bool> (prefix + _unique_name + "/fix_from_raw");
+ remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers");
+ outlier_residual_th = _server.getParam<double> (prefix + _unique_name + "/outlier_residual_th");
+
+ // COMPUTE POS PARAMS (only if compute fix from yaw)
+ if (fix_from_raw)
+ {
+ // GNSS OPTIONS (see gnss_utils.h)
+ compute_pos_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris/clock (0:broadcast ephemeris,1:precise ephemeris,2:broadcast + SBAS,3:ephemeris option: broadcast + SSR_APC,4:broadcast + SSR_COM,5: QZSS LEX ephemeris
+ compute_pos_opt.ionoopt = _server.getParam<int> (prefix + _unique_name + "/gnss/ionoopt"); // ionosphere option (0:correction off,1:broadcast mode,2:SBAS model,3:L1/L2 or L1/L5,4:estimation,5:IONEX TEC model,6:QZSS broadcast,7:QZSS LEX ionosphere,8:SLANT TEC mode)
+ compute_pos_opt.tropopt = _server.getParam<int> (prefix + _unique_name + "/gnss/tropopt"); // troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction)
+ compute_pos_opt.sbascorr = _server.getParam<int> (prefix + _unique_name + "/gnss/sbascorr");// SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging)
+ compute_pos_opt.raim = _server.getParam<bool> (prefix + _unique_name + "/gnss/raim"); // RAIM enabled
+ compute_pos_opt.elmin = _server.getParam<double>(prefix + _unique_name + "/gnss/elmin"); // min elevation (rad)
+ compute_pos_opt.maxgdop = _server.getParam<double>(prefix + _unique_name + "/gnss/maxgdop"); // maxgdop: reject threshold of gdop
+
+ compute_pos_opt.GPS = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/GPS"); /* navigation system */
+ compute_pos_opt.SBS = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/SBS");
+ compute_pos_opt.GLO = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/GLO");
+ compute_pos_opt.GAL = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/GAL");
+ compute_pos_opt.QZS = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/QZS");
+ compute_pos_opt.CMP = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/CMP");
+ compute_pos_opt.IRN = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/IRN");
+ compute_pos_opt.LEO = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/LEO");
+ }
}
- std::string print() const
+
+ std::string print() const override
{
- return "\n" + ProcessorParamsBase::print() + "\n"
- + "time_th: " + std::to_string(time_th) + "\n"
- + "dist_traveled: " + std::to_string(dist_traveled) + "\n"
- + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
+ return "\n" + ParamsProcessorBase::print() + "\n" +
+ "max_time_span: " + std::to_string(max_time_span) + "\n" +
+ "dist_traveled: " + std::to_string(dist_traveled) + "\n" +
+ "fix_from_raw: " + std::to_string(fix_from_raw) + "\n" +
+ "init_enu_map: " + std::to_string(init_enu_map) + "\n" +
+ (init_enu_map ?
+ "enu_map_init_dist_min: "+ std::to_string(enu_map_init_dist_min) + "\n" : "") +
+ "enu_map_fix_dist: " + std::to_string(enu_map_fix_dist) + "\n" +
+ "remove_outliers: " + std::to_string(remove_outliers) + "\n" +
+ "outlier_residual_th: " + std::to_string(outlier_residual_th) + "\n" +
+ "keyframe_vote/max_time_span: " + std::to_string(max_time_span) + "\n" +
+ (fix_from_raw ?
+ "gnss/sateph: " + std::to_string(compute_pos_opt.sateph) + "\n" +
+ "gnss/ionoopt: " + std::to_string(compute_pos_opt.ionoopt) + "\n" +
+ "gnss/tropopt: " + std::to_string(compute_pos_opt.tropopt) + "\n" +
+ "gnss/sbascorr: " + std::to_string(compute_pos_opt.sbascorr) + "\n" +
+ "gnss/raim: " + std::to_string(compute_pos_opt.raim) + "\n" +
+ "gnss/elmin: " + std::to_string(compute_pos_opt.elmin*R2D) + "\n" +
+ "gnss/maxgdop: " + std::to_string(compute_pos_opt.maxgdop) + "\n" +
+ "gnss/constellations/GPS: " + std::to_string(compute_pos_opt.GPS) + "\n" +
+ "gnss/constellations/SBS: " + std::to_string(compute_pos_opt.SBS) + "\n" +
+ "gnss/constellations/GLO: " + std::to_string(compute_pos_opt.GLO) + "\n" +
+ "gnss/constellations/GAL: " + std::to_string(compute_pos_opt.GAL) + "\n" +
+ "gnss/constellations/QZS: " + std::to_string(compute_pos_opt.QZS) + "\n" +
+ "gnss/constellations/CMP: " + std::to_string(compute_pos_opt.CMP) + "\n" +
+ "gnss/constellations/IRN: " + std::to_string(compute_pos_opt.IRN) + "\n" +
+ "gnss/constellations/LEO: " + std::to_string(compute_pos_opt.LEO) + "\n" :
+ "");
}
};
@@ -42,14 +125,21 @@ class ProcessorGnssFix : public ProcessorBase
{
protected:
SensorGnssPtr sensor_gnss_;
- ProcessorParamsGnssFixPtr params_gnss_;
- CaptureGnssFixPtr first_capture_, incoming_capture_;
+ ParamsProcessorGnssFixPtr params_gnss_;
+ CaptureBasePtr last_KF_capture_, incoming_capture_;
+ FeatureGnssFixPtr last_KF_feature_;
FrameBasePtr last_KF_;
+ GnssUtils::ComputePosOutput incoming_pos_out_;
+ Eigen::Vector3d first_pos_;
+ VectorComposite first_frame_state_;
+
public:
- ProcessorGnssFix(ProcessorParamsGnssFixPtr _params);
- virtual ~ProcessorGnssFix();
- virtual void configure(SensorBasePtr _sensor) override;
+ ProcessorGnssFix(ParamsProcessorGnssFixPtr _params);
+ ~ProcessorGnssFix() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ WOLF_PROCESSOR_CREATE(ProcessorGnssFix, ParamsProcessorGnssFix);
protected:
/** \brief process an incoming capture
@@ -57,46 +147,43 @@ class ProcessorGnssFix : public ProcessorBase
* Each derived processor should implement this function. It will be called if:
* - A new capture arrived and triggerInCapture() returned true.
*/
- virtual void processCapture(CaptureBasePtr) override;
+ void processCapture(CaptureBasePtr) override;
/** \brief process an incoming key-frame
*
* The ProcessorTracker only processes incoming captures (it is not called).
*/
- virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override {};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
/** \brief trigger in capture
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
- virtual bool triggerInCapture(CaptureBasePtr) const override;
+ bool triggerInCapture(CaptureBasePtr) const override;
/** \brief trigger in key-frame
*
* The ProcessorTracker only processes incoming captures, then it returns false.
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) const override {return false;}
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
- virtual bool storeKeyFrame(FrameBasePtr);
+ bool storeKeyFrame(FrameBasePtr) override;
/** \brief store capture
*
* Returns true if the capture should be stored
*/
- virtual bool storeCapture(CaptureBasePtr);
+ bool storeCapture(CaptureBasePtr) override;
- virtual bool voteForKeyFrame() const override;
+ bool voteForKeyFrame() const override;
private:
FactorBasePtr emplaceFactor(FeatureBasePtr _ftr_ptr);
- bool rejectOutlier(FactorBasePtr ctr_ptr);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
+ bool detectOutlier(FactorBasePtr ctr_ptr);
};
diff --git a/include/gnss/processor/processor_gnss_single_diff.h b/include/gnss/processor/processor_gnss_single_diff.h
deleted file mode 100644
index 12f7943c6..000000000
--- a/include/gnss/processor/processor_gnss_single_diff.h
+++ /dev/null
@@ -1,116 +0,0 @@
-#ifndef WOLF_PROCESSOR_GNSS_SINGLE_DIFF_H
-#define WOLF_PROCESSOR_GNSS_SINGLE_DIFF_H
-
-// Wolf includes
-#include "core/processor/processor_base.h"
-#include "gnss/capture/capture_gnss_single_diff.h"
-#include "gnss/sensor/sensor_gnss.h"
-
-// Std includes
-
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ProcessorGnssSingleDiff);
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssSingleDiff);
-
-
-struct ProcessorParamsGnssSingleDiff : public ProcessorParamsBase
-{
- Scalar time_th;
- Scalar dist_traveled;
- Scalar enu_map_init_dist_min;
- ProcessorParamsGnssSingleDiff() = default;
- ProcessorParamsGnssSingleDiff(std::string _unique_name, const ParamsServer& _server):
- ProcessorParamsBase(_unique_name, _server)
- {
- time_th = _server.getParam<Scalar>(_unique_name + "/time_th");
- dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled");
- enu_map_init_dist_min = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
- }
- std::string print() const
- {
- return "\n" + ProcessorParamsBase::print() + "\n"
- + "time_th: " + std::to_string(time_th) + "\n"
- + "dist_traveled: " + std::to_string(dist_traveled) + "\n"
- + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
- }
-};
-
-//class
-class ProcessorGnssSingleDiff : public ProcessorBase
-{
- protected:
- SensorGnssPtr sensor_gnss_;
- ProcessorParamsGnssSingleDiffPtr params_gnss_;
- CaptureGnssSingleDiffPtr incoming_capture_;
- FrameBasePtr last_KF_;
-
- public:
- ProcessorGnssSingleDiff(ProcessorParamsGnssSingleDiffPtr _params_gnss);
- virtual ~ProcessorGnssSingleDiff();
- virtual void configure(SensorBasePtr _sensor) override;
- FrameBasePtr getLastKF();
-
- protected:
- /** \brief process an incoming capture
- *
- * Each derived processor should implement this function. It will be called if:
- * - A new capture arrived and triggerInCapture() returned true.
- */
- virtual void processCapture(CaptureBasePtr) override;
-
- /** \brief process an incoming key-frame
- *
- * The ProcessorTracker only processes incoming captures (it is not called).
- */
- virtual void processKeyFrame(FrameBasePtr _keyframe, const Scalar& _time_tolerance) override {};
-
- /** \brief store key frame
- *
- * Returns true if the key frame should be stored
- */
- virtual bool storeKeyFrame(FrameBasePtr) override;
-
- /** \brief store capture
- *
- * Returns true if the capture should be stored
- */
- virtual bool storeCapture(CaptureBasePtr) override;
-
- /** \brief trigger in capture
- *
- * Returns true if processCapture() should be called after the provided capture arrived.
- */
- virtual bool triggerInCapture(CaptureBasePtr) const override;
-
- /** \brief trigger in key-frame
- *
- * The ProcessorTracker only processes incoming captures, then it returns false.
- */
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe, const Scalar& _time_tolerance) const override {return false;}
-
- virtual bool voteForKeyFrame() const override;
-
-
- private:
- FactorBasePtr emplaceFactor(FeatureBasePtr _ftr);
-
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params);
-
-};
-
-inline bool ProcessorGnssSingleDiff::triggerInCapture(CaptureBasePtr) const
-{
- return true;
-}
-
-inline wolf::FrameBasePtr ProcessorGnssSingleDiff::getLastKF()
-{
- return last_KF_;
-}
-
-} // namespace wolf
-
-#endif //WOLF_PROCESSOR_GNSS_SINGLE_DIFF_H
diff --git a/include/gnss/processor/processor_gnss_tdcp.h b/include/gnss/processor/processor_gnss_tdcp.h
new file mode 100644
index 000000000..0f1bbf1d0
--- /dev/null
+++ b/include/gnss/processor/processor_gnss_tdcp.h
@@ -0,0 +1,154 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef WOLF_PROCESSOR_GNSS_TDCP_H
+#define WOLF_PROCESSOR_GNSS_TDCP_H
+
+// Wolf includes
+#include "core/common/wolf.h"
+#include "core/processor/processor_base.h"
+
+#include "gnss/capture/capture_gnss.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/processor/processor_gnss_fix.h"
+
+// GNSS Utils includes
+#include "gnss_utils/tdcp.h"
+
+// Std includes
+
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(ProcessorGnssTdcp);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorGnssTdcp);
+
+
+struct ParamsProcessorGnssTdcp : public ParamsProcessorGnssFix
+{
+ struct GnssUtils::TdcpOptions tdcp;
+
+ ParamsProcessorGnssTdcp() = default;
+ ParamsProcessorGnssTdcp(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorGnssFix(_unique_name, _server)
+ {
+ tdcp.min_common_sats = _server.getParam<double>(prefix + _unique_name + "/tdcp/min_common_sats");
+ tdcp.raim_n = _server.getParam<double>(prefix + _unique_name + "/tdcp/raim_n");
+ tdcp.max_residual_ci = _server.getParam<double>(prefix + _unique_name + "/tdcp/max_residual_ci");
+ tdcp.residual_opt = _server.getParam<int> (prefix + _unique_name + "/tdcp/residual_opt");
+ tdcp.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/tdcp/sigma_atm");
+ tdcp.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/tdcp/sigma_carrier");
+ tdcp.time_window = _server.getParam<double>(prefix + _unique_name + "/tdcp/time_window");
+
+ // hardcoded params
+ compute_pos_opt.carrier_opt.corr_tropo = false;
+ compute_pos_opt.carrier_opt.corr_iono = false;
+ compute_pos_opt.carrier_opt.corr_clock = false;
+ tdcp.relinearize_jacobian = false;
+ tdcp.max_iterations = 1;
+ tdcp.use_old_nav = false;
+ tdcp.multi_freq = false;
+ }
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorBase::print() + "\n"
+ + "tdcp/min_common_sats: " + std::to_string(tdcp.min_common_sats) + "\n"
+ + "tdcp/raim_n: " + std::to_string(tdcp.raim_n) + "\n"
+ + "tdcp/max_residual_ci: " + std::to_string(tdcp.max_residual_ci) + "\n"
+ + "tdcp/residual_opt: " + std::to_string(tdcp.residual_opt) + "\n"
+ + "tdcp/sigma_atm: " + std::to_string(tdcp.sigma_atm) + "\n"
+ + "tdcp/sigma_carrier: " + std::to_string(tdcp.sigma_carrier) + "\n"
+ + "tdcp/time_window: " + std::to_string(tdcp.time_window) + "\n";
+ }
+};
+
+//class
+class ProcessorGnssTdcp : public ProcessorBase
+{
+ protected:
+ SensorGnssPtr sensor_gnss_;
+ ParamsProcessorGnssTdcpPtr params_tdcp_;
+ CaptureGnssPtr incoming_capture_;
+ FrameBasePtr last_KF_;
+
+ public:
+ ProcessorGnssTdcp(ParamsProcessorGnssTdcpPtr _params_gnss);
+ ~ProcessorGnssTdcp() override;
+ void configure(SensorBasePtr _sensor) override;
+
+ WOLF_PROCESSOR_CREATE(ProcessorGnssTdcp, ParamsProcessorGnssTdcp);
+
+ FrameBasePtr getLastKF();
+
+ protected:
+ /** \brief process an incoming capture
+ *
+ * Each derived processor should implement this function. It will be called if:
+ * - A new capture arrived and triggerInCapture() returned true.
+ */
+ void processCapture(CaptureBasePtr) override;
+
+ /** \brief process an incoming key-frame
+ *
+ * The ProcessorTracker only processes incoming captures (it is not called).
+ */
+ void processKeyFrame(FrameBasePtr _keyframe) override;
+
+ /** \brief store key frame
+ *
+ * Returns true if the key frame should be stored
+ */
+ bool storeKeyFrame(FrameBasePtr) override {return false;};
+
+ /** \brief store capture
+ *
+ * Returns true if the capture should be stored
+ */
+ bool storeCapture(CaptureBasePtr) override {return false;};
+
+ /** \brief trigger in capture
+ *
+ * Returns true if processCapture() should be called after the provided capture arrived.
+ */
+ bool triggerInCapture(CaptureBasePtr) const override {return true;};
+
+ /** \brief trigger in key-frame
+ *
+ * The ProcessorTracker only processes incoming captures, then it returns false.
+ */
+ bool triggerInKeyFrame(FrameBasePtr _keyframe) const override {return true;};
+
+ bool voteForKeyFrame() const override;
+
+
+ private:
+ FactorBasePtr emplaceFactor(FeatureBasePtr _ftr, FrameBasePtr _frm_ref);
+
+};
+
+inline wolf::FrameBasePtr ProcessorGnssTdcp::getLastKF()
+{
+ return last_KF_;
+}
+
+} // namespace wolf
+
+#endif //WOLF_PROCESSOR_GNSS_TDCP_H
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
new file mode 100644
index 000000000..319ce9deb
--- /dev/null
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -0,0 +1,322 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef PROCESSOR_TRACKER_GNSS_H_
+#define PROCESSOR_TRACKER_GNSS_H_
+
+#include "gnss/internal/config.h"
+#include "core/common/wolf.h"
+#include "core/processor/processor_tracker_feature.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/feature/feature_gnss_satellite.h"
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/tdcp.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorTrackerGnss);
+
+struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
+{
+ GnssUtils::Options gnss_opt;
+ GnssUtils::Options fix_opt{GnssUtils::default_options};
+ GnssUtils::TdcpOptions tdcp_params;
+ double max_time_span;
+ bool remove_outliers, remove_outliers_tdcp, remove_outliers_with_fix;
+ double outlier_residual_th;
+ bool init_frames, pseudo_ranges, fix;
+ double enu_map_fix_dist;
+ int min_sbas_sats;
+ bool tdcp_enabled;
+ std::string tdcp_structure;
+
+ ParamsProcessorTrackerGnss() = default;
+ ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorTrackerFeature(_unique_name, _server)
+ {
+ remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers");
+ remove_outliers_with_fix = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers_with_fix");
+ outlier_residual_th = _server.getParam<double> (prefix + _unique_name + "/outlier_residual_th");
+ init_frames = _server.getParam<bool> (prefix + _unique_name + "/init_frames");
+ max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span");
+ enu_map_fix_dist = _server.getParam<double> (prefix + _unique_name + "/enu_map_fix_dist");
+ fix = _server.getParam<bool> (prefix + _unique_name + "/fix");
+ pseudo_ranges = _server.getParam<bool> (prefix + _unique_name + "/pseudo_ranges");
+ min_sbas_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/min_sbas_sats");
+
+ // GNSS OPTIONS (see rtklib.h)
+ gnss_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris option: EPHOPT_BRDC(0):broadcast ephemeris, EPHOPT_PREC(1): precise ephemeris, EPHOPT_SBAS(2): broadcast + SBAS, EPHOPT_SSRAPC(3): broadcast + SSR_APC, EPHOPT_SSRCOM(4): broadcast + SSR_COM, EPHOPT_LEX(5): QZSS LEX ephemeris, EPHOPT_SBAS2(6):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), EPHOPT_SBAS3(7):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_SBAS2), EPHOPT_SBAS4(8):broadcast + SBAS(EPHOPT_SBAS if possible, otherwise EPHOPT_BRDC)
+ gnss_opt.ionoopt = _server.getParam<int> (prefix + _unique_name + "/gnss/ionoopt"); // ionosphere option: IONOOPT_OFF(0):correction off, IONOOPT_BRDC(1):broadcast mode, IONOOPT_SBAS(2):SBAS model, IONOOPT_IFLC(3):L1/L2 or L1/L5, IONOOPT_EST(4):estimation, IONOOPT_TEC(5):IONEX TEC model, IONOOPT_QZS(6):QZSS broadcast, IONOOPT_LEX(7):QZSS LEX ionosphere, IONOOPT_STEC(8):SLANT TEC mode, IONOOPT_SBAS2(9):broadcast + SBAS(sats with SBAS corr and sats with BRDC eph), IONOOPT_SBAS3(10):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_SBAS2), IONOOPT_SBAS4(8):broadcast + SBAS(IONOOPT_SBAS if possible, otherwise IONOOPT_BRDC)
+ gnss_opt.tropopt = _server.getParam<int> (prefix + _unique_name + "/gnss/tropopt"); // troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction)
+ gnss_opt.sbascorr = _server.getParam<int> (prefix + _unique_name + "/gnss/sbascorr");// SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging)
+ gnss_opt.raim = _server.getParam<int> (prefix + _unique_name + "/gnss/raim"); // RAIM enabled
+ gnss_opt.elmin = _server.getParam<double>(prefix + _unique_name + "/gnss/elmin"); // min elevation (rad)
+ gnss_opt.maxgdop = _server.getParam<double>(prefix + _unique_name + "/gnss/maxgdop"); // maxgdop: reject threshold of gdop
+
+ gnss_opt.GPS = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/GPS"); /* navigation system */
+ gnss_opt.SBS = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/SBS");
+ gnss_opt.GLO = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/GLO");
+ gnss_opt.GAL = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/GAL");
+ gnss_opt.QZS = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/QZS");
+ gnss_opt.CMP = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/CMP");
+ gnss_opt.IRN = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/IRN");
+ gnss_opt.LEO = _server.getParam<bool>(prefix + _unique_name + "/gnss/constellations/LEO");
+
+ // TDCP
+ tdcp_enabled = _server.getParam<bool>(prefix + _unique_name + "/gnss/tdcp/enabled");
+ if (tdcp_enabled)
+ {
+ tdcp_structure = _server.getParam<std::string>(prefix + _unique_name + "/gnss/tdcp/structure");
+ remove_outliers_tdcp = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/remove_outliers");
+ tdcp_params.batch = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/batch");
+ gnss_opt.carrier_opt.corr_iono = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_iono");
+ gnss_opt.carrier_opt.corr_tropo = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
+ gnss_opt.carrier_opt.corr_clock = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_clock");
+ tdcp_params.loss_function = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/loss_function");
+ tdcp_params.time_window = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/time_window");
+ tdcp_params.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_atm");
+ tdcp_params.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
+ tdcp_params.multi_freq = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/multi_freq");
+ if (tdcp_params.batch)
+ {
+ tdcp_params.min_common_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/min_common_sats");
+ tdcp_params.raim_n = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/raim_n");
+ tdcp_params.max_residual_ci = _server.getParam<double> (prefix + _unique_name + "/gnss/tdcp/max_residual_ci");
+ tdcp_params.relinearize_jacobian = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/relinearize_jacobian");
+ tdcp_params.max_iterations = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/max_iterations");
+ tdcp_params.residual_opt = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/residual_opt");
+ }
+
+ if (tdcp_structure != "all-all" and tdcp_structure != "consecutive" and tdcp_structure != "first-all")
+ throw std::runtime_error("unknown value for '/gnss/tdcp/structure', should be 'all-all', 'consecutive' or 'first-all'");
+ }
+
+ // COMPUTE FIX OPTIONS (RAIM)
+ fix_opt.raim = gnss_opt.raim;
+ fix_opt.sateph = 6; //EPHOPT_SBAS2;
+ fix_opt.ionoopt = 9;//IONOPT_SBAS2;
+ fix_opt.tropopt = 2;
+ fix_opt.sbascorr = 15;
+ fix_opt.elmin = gnss_opt.elmin;
+ fix_opt.maxgdop = gnss_opt.maxgdop;
+ // same constellations
+ fix_opt.GPS = gnss_opt.GPS;
+ fix_opt.SBS = gnss_opt.SBS;
+ fix_opt.GLO = gnss_opt.GLO;
+ fix_opt.GAL = gnss_opt.GAL;
+ fix_opt.QZS = gnss_opt.QZS;
+ fix_opt.CMP = gnss_opt.CMP;
+ fix_opt.IRN = gnss_opt.IRN;
+ fix_opt.LEO = gnss_opt.LEO;
+ }
+
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorBase::print() + "\n"
+ + "remove_outliers: " + std::to_string(remove_outliers) + "\n"
+ + "outlier_residual_th: " + std::to_string(outlier_residual_th) + "\n"
+ + "init_frames: " + std::to_string(init_frames) + "\n"
+ + "enu_map_fix_dist: " + std::to_string(enu_map_fix_dist) + "\n"
+ + "keyframe_vote/max_time_span: " + std::to_string(max_time_span) + "\n"
+ + "gnss/sateph: " + std::to_string(gnss_opt.sateph) + "\n"
+ + "gnss/ionoopt: " + std::to_string(gnss_opt.ionoopt) + "\n"
+ + "gnss/tropopt: " + std::to_string(gnss_opt.tropopt) + "\n"
+ + "gnss/sbascorr: " + std::to_string(gnss_opt.sbascorr) + "\n"
+ + "gnss/raim: " + std::to_string(gnss_opt.raim) + "\n"
+ + "gnss/elmin: " + std::to_string(gnss_opt.elmin*R2D) + "\n"
+ + "gnss/maxgdop: " + std::to_string(gnss_opt.maxgdop) + "\n"
+ + "gnss/constellations/GPS: " + std::to_string(gnss_opt.GPS) + "\n"
+ + "gnss/constellations/SBS: " + std::to_string(gnss_opt.SBS) + "\n"
+ + "gnss/constellations/GLO: " + std::to_string(gnss_opt.GLO) + "\n"
+ + "gnss/constellations/GAL: " + std::to_string(gnss_opt.GAL) + "\n"
+ + "gnss/constellations/QZS: " + std::to_string(gnss_opt.QZS) + "\n"
+ + "gnss/constellations/CMP: " + std::to_string(gnss_opt.CMP) + "\n"
+ + "gnss/constellations/IRN: " + std::to_string(gnss_opt.IRN) + "\n"
+ + "gnss/constellations/LEO: " + std::to_string(gnss_opt.LEO) + "\n"
+ + "gnss/tdcp/enabled: " + std::to_string(tdcp_enabled) + "\n"
+ + "gnss/tdcp/structure: " + tdcp_structure + "\n"
+ + "gnss/tdcp/batch: " + std::to_string(tdcp_params.batch) + "\n"
+ + "gnss/tdcp/corr_iono: " + std::to_string(gnss_opt.carrier_opt.corr_iono)+ "\n"
+ + "gnss/tdcp/corr_tropo: " + std::to_string(gnss_opt.carrier_opt.corr_tropo)+ "\n"
+ + "gnss/tdcp/corr_clock: " + std::to_string(gnss_opt.carrier_opt.corr_clock)+ "\n"
+ + "gnss/tdcp/loss_function: " + std::to_string(tdcp_params.loss_function) + "\n"
+ + "gnss/tdcp/time_window: " + std::to_string(tdcp_params.time_window) + "\n"
+ + "gnss/tdcp/sigma_atm: " + std::to_string(tdcp_params.sigma_atm) + "\n"
+ + "gnss/tdcp/sigma_carrier: " + std::to_string(tdcp_params.sigma_carrier) + "\n"
+ + "gnss/tdcp/multi_freq: " + std::to_string(tdcp_params.multi_freq) + "\n";
+ }
+};
+
+
+WOLF_PTR_TYPEDEFS(ProcessorTrackerGnss);
+
+//Class
+class ProcessorTrackerGnss : public ProcessorTrackerFeature
+{
+
+ public:
+ ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss);
+ ~ProcessorTrackerGnss() override;
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(ProcessorTrackerGnss, ParamsProcessorTrackerGnss);
+
+ void configure(SensorBasePtr _sensor) override;
+
+ unsigned int getNTrackedSats() const;
+
+ unsigned int getNUntrackedSats() const;
+
+ protected:
+
+ ParamsProcessorTrackerGnssPtr params_tracker_gnss_;
+ SensorGnssPtr sensor_gnss_;
+ std::map<int, FeatureGnssSatellitePtr> untracked_incoming_features_, untracked_last_features_;
+ GnssUtils::ComputePosOutput fix_incoming_, fix_last_;
+ unsigned int outliers_pseudorange_, outliers_tdcp_, inliers_pseudorange_, inliers_tdcp_;
+ std::map<int, unsigned int> sat_outliers_;
+ Eigen::Vector3d first_pos_;
+
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
+ * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * \return the number of features tracked
+ */
+ unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences) override;
+
+ /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
+ * \param _last_feature input feature in last capture tracked
+ * \param _incoming_feature input/output feature in incoming capture to be corrected
+ * \return false if the the process discards the correspondence with origin's feature
+ */
+ bool correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) override;
+
+ /** \brief Vote for KeyFrame generation
+ *
+ * If a KeyFrame criterion is validated, this function returns true,
+ * meaning that it wants to create a KeyFrame at the \b last Capture.
+ *
+ * WARNING! This function only votes! It does not create KeyFrames!
+ */
+ bool voteForKeyFrame() const override;
+
+ /** \brief Detect new Features
+ * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
+ * \return The number of detected Features.
+ *
+ * This function detects Features that do not correspond to known Features/Landmarks in the system.
+ *
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ *
+ * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+ * the list of newly detected features of the capture last_ptr_.
+ */
+ unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
+
+ /** \brief OVERWRITTEN Emplaces tdcp and pseudorange factors
+ */
+ void establishFactors() override;
+
+ /** \brief Emplaces a new factor
+ * \param _feature_ptr pointer to the parent Feature
+ * \param _feature_other_ptr pointer to the other feature constrained.
+ *
+ * This function emplaces a factor of the appropriate type for the derived processor.
+ */
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override{return nullptr;};
+
+ /** Pre-process incoming Capture
+ *
+ * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
+ *
+ * Overload this function to prepare stuff on derived classes.
+ *
+ * Typical uses of prePrecess() are:
+ * - casting base types to derived types
+ * - initializing counters, flags, or any derived variables
+ * - initializing algorithms needed for processing the derived data
+ */
+ void preProcess() override;
+
+ /** Post-process
+ *
+ * This is called by process() after finishing the processing algorithm.
+ *
+ * Overload this function to post-process stuff on derived classes.
+ *
+ * Typical uses of postPrecess() are:
+ * - resetting and/or clearing variables and/or algorithms at the end of processing
+ * - drawing / printing / logging the results of the processing
+ */
+ void postProcess() override;
+
+ void advanceDerived() override;
+ void resetDerived() override;
+
+ void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap);
+};
+
+inline ProcessorTrackerGnss::~ProcessorTrackerGnss()
+{
+ //
+}
+
+inline void ProcessorTrackerGnss::configure(SensorBasePtr _sensor)
+{
+ sensor_gnss_ = std::dynamic_pointer_cast<SensorGnss>(_sensor);
+ assert(sensor_gnss_ != nullptr && "configured a processor tracker gnss with a wrong sensor");
+}
+
+inline bool ProcessorTrackerGnss::correctFeatureDrift(const FeatureBasePtr _origin_feature,
+ const FeatureBasePtr _last_feature,
+ FeatureBasePtr _incoming_feature)
+{
+ return true;
+}
+
+inline unsigned int ProcessorTrackerGnss::getNTrackedSats() const
+{
+ return known_features_last_.size();
+}
+
+inline unsigned int ProcessorTrackerGnss::getNUntrackedSats() const
+{
+ return untracked_last_features_.size();
+}
+
+} // namespace wolf
+
+#endif /* PROCESSOR_TRACKER_GNSS_H_ */
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 66c410926..03fc7a2d4 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
#ifndef SENSOR_GPS_H_
#define SENSOR_GPS_H_
@@ -8,86 +29,123 @@
namespace wolf {
-WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsGnss);
+static const char CLOCK_BIAS_KEY = 'T';
+static const char CLOCK_BIAS_GPS_GLO_KEY = 'G';
+static const char CLOCK_BIAS_GPS_GAL_KEY = 'E';
+static const char CLOCK_BIAS_GPS_CMP_KEY = 'M';
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorGnss);
WOLF_PTR_TYPEDEFS(SensorGnss);
-struct IntrinsicsGnss : public IntrinsicsBase
-{
- // add GNSS parameters here
- bool extrinsics_fixed_ = true;
- bool roll_fixed_ =true;
- bool pitch_fixed_=true;
- bool yaw_fixed_ =false;
- bool translation_fixed_ = false;
- virtual ~IntrinsicsGnss() = default;
- IntrinsicsGnss() = default;
- IntrinsicsGnss(std::string _unique_name, const ParamsServer& _server):
- IntrinsicsBase(_unique_name, _server)
+struct ParamsSensorGnss : public ParamsSensorBase
+{
+ // extrinsics and intrinsics
+ bool extrinsics_fixed = true;
+ bool clock_bias_GPS_GLO_dynamic = false;
+ bool clock_bias_GPS_GAL_dynamic = false;
+ bool clock_bias_GPS_CMP_dynamic = false;
+
+ // ENU
+ std::string ENU_mode = "auto";
+ bool roll_fixed = true;
+ bool pitch_fixed = true;
+ bool yaw_fixed = true;
+ bool translation_fixed = true;
+ Eigen::Vector3d ENU_latlonalt = Eigen::Vector3d::Zero();
+
+
+ ~ParamsSensorGnss() override = default;
+ ParamsSensorGnss() = default;
+ ParamsSensorGnss(std::string _unique_name, const ParamsServer& _server):
+ ParamsSensorBase(_unique_name, _server)
{
- extrinsics_fixed_ = _server.getParam<bool>(_unique_name + "/extrinsics_fixed");
- roll_fixed_ = _server.getParam<bool>(_unique_name + "/roll_fixed");
- pitch_fixed_ = _server.getParam<bool>(_unique_name + "/pitch_fixed");
- yaw_fixed_ = _server.getParam<bool>(_unique_name + "/yaw_fixed");
- translation_fixed_ = _server.getParam<bool>(_unique_name + "/translation_fixed");
+ extrinsics_fixed = _server.getParam<bool>(prefix + _unique_name + "/extrinsics_fixed");
+ clock_bias_GPS_GLO_dynamic = _server.getParam<bool>(prefix + _unique_name + "/clock_bias_GPS_GLO_dynamic");
+ clock_bias_GPS_GAL_dynamic = _server.getParam<bool>(prefix + _unique_name + "/clock_bias_GPS_GAL_dynamic");
+ clock_bias_GPS_CMP_dynamic = _server.getParam<bool>(prefix + _unique_name + "/clock_bias_GPS_CMP_dynamic");
+ ENU_mode = _server.getParam<std::string>(prefix + _unique_name + "/ENU/mode");
+ if (ENU_mode == "manual" or ENU_mode == "auto")
+ {
+ roll_fixed = _server.getParam<bool>(prefix + _unique_name + "/ENU/roll_fixed");
+ pitch_fixed = _server.getParam<bool>(prefix + _unique_name + "/ENU/pitch_fixed");
+ yaw_fixed = _server.getParam<bool>(prefix + _unique_name + "/ENU/yaw_fixed");
+ translation_fixed = _server.getParam<bool>(prefix + _unique_name + "/ENU/translation_fixed");
+ if (ENU_mode == "manual")
+ ENU_latlonalt = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/ENU_latlonalt");
+ }
+ else if (ENU_mode != "ECEF")
+ {
+ std::runtime_error("bad 'ENU_mode' value. Should be 'manual', 'auto' or 'ECEF'");
+ }
}
- std::string print() const
+ std::string print() const override
{
- return "\n" + IntrinsicsBase::print() + "\n"
- + "extrinsics_fixed: " + std::to_string(extrinsics_fixed_) + "\n"
- + "roll_fixed: " + std::to_string(roll_fixed_) + "\n"
- + "pitch_fixed: " + std::to_string(pitch_fixed_) + "\n"
- + "yaw_fixed: " + std::to_string(yaw_fixed_) + "\n"
- + "translation_fixed: " + std::to_string(translation_fixed_) + "\n";
+ return "\n" + ParamsSensorBase::print() + "\n"
+ + "extrinsics_fixed: " + std::to_string(extrinsics_fixed) + "\n"
+ + "clock_bias_GPS_GLO_dynamic: " + std::to_string(clock_bias_GPS_GLO_dynamic) + "\n"
+ + "clock_bias_GPS_GAL_dynamic: " + std::to_string(clock_bias_GPS_GAL_dynamic) + "\n"
+ + "clock_bias_GPS_CMP_dynamic: " + std::to_string(clock_bias_GPS_CMP_dynamic) + "\n"
+ + "ENU_mode: " + ENU_mode + "\n"
+ + "roll_fixed: " + std::to_string(roll_fixed) + "\n"
+ + "pitch_fixed: " + std::to_string(pitch_fixed ) + "\n"
+ + "yaw_fixed: " + std::to_string(yaw_fixed) + "\n"
+ + "translation_fixed: " + std::to_string(translation_fixed) + "\n"
+ + "ENU_latlonalt: to_string not implemented yet!" + "\n";
}
};
class SensorGnss : public SensorBase
{
protected:
- IntrinsicsGnssPtr params_;
- bool ENU_defined_, ENU_MAP_initialized_;
- Eigen::Matrix3s R_ENU_ECEF_;
- Eigen::Vector3s t_ENU_ECEF_;
+ ParamsSensorGnssPtr params_;
+ bool ENU_defined_, t_ENU_MAP_initialized_, R_ENU_MAP_initialized_;
+ Eigen::Matrix3d R_ENU_ECEF_;
+ Eigen::Vector3d t_ENU_ECEF_;
public:
- SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params);
- SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params, StateBlockPtr _t_ENU_MAP_ptr, StateBlockPtr _roll_ENU_MAP_ptr, StateBlockPtr _pitch_ENU_MAP_ptr, StateBlockPtr _yaw_ENU_MAP_ptr);
+ SensorGnss(const Eigen::VectorXd& _extrinsics,
+ const ParamsSensorGnssPtr& _ParamsSensor);
+ WOLF_SENSOR_CREATE(SensorGnss, ParamsSensorGnss, 3);
- virtual ~SensorGnss();
+ ~SensorGnss() override;
// Gets
StateBlockPtr getEnuMapTranslation() const;
StateBlockPtr getEnuMapRoll() const;
StateBlockPtr getEnuMapPitch() const;
StateBlockPtr getEnuMapYaw() const;
- const Eigen::Matrix3s& getREnuEcef() const;
- const Eigen::Vector3s& gettEnuEcef() const;
- Eigen::Matrix3s getREnuMap() const;
- Eigen::Vector3s gettEnuMap() const;
+ const Eigen::Matrix3d& getREnuEcef() const;
+ const Eigen::Vector3d& gettEnuEcef() const;
+ Eigen::Matrix3d getREnuMap() const;
+ Eigen::Vector3d gettEnuMap() const;
bool isEnuDefined() const;
bool isEnuMapInitialized() const;
+ bool isEnuMapTranslationInitialized() const;
+ bool isEnuMapRotationInitialized() const;
+ bool isEnuModeEcef() const;
+ bool isEnuModeAuto() const;
+ bool isEnuMapFixed() const;
// Sets
- void setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP);
- void setEnuMapRollState(const Scalar& roll_ENU_MAP);
- void setEnuMapPitchState(const Scalar& pitch_ENU_MAP);
- void setEnuMapYawState(const Scalar& yaw_ENU_MAP);
- void setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU);
- void setEnuEcef(const Eigen::Matrix3s& _R_ENU_ECEF, const Eigen::Vector3s& _t_ENU_ECEF);
+ void setEnuMapTranslationState(const Eigen::Vector3d& t_ENU_MAP);
+ void setEnuMapRollState(const double& roll_ENU_MAP);
+ void setEnuMapPitchState(const double& pitch_ENU_MAP);
+ void setEnuMapYawState(const double& yaw_ENU_MAP);
+ void setEcefEnu(const Eigen::Vector3d& _ENU, bool _ECEF_coordinates=true);
+ void setEnuEcef(const Eigen::Matrix3d& _R_ENU_ECEF, const Eigen::Vector3d& _t_ENU_ECEF);
+ void setEnuMapInitialized(const bool& _init);
+ void setEnuMapTranslationInitialized(const bool& _init);
+ void setEnuMapRotationInitialized(const bool& _init);
+ void fixEnuMap();
// compute
template<typename T>
Eigen::Matrix<T,3,3> computeREnuMap(const T& _r,const T& _p,const T& _y) const;
- void computeEnuEcef(const Eigen::Vector3s& _t_ECEF_ENU, Eigen::Matrix3s& R_ENU_ECEF, Eigen::Vector3s& t_ENU_ECEF) const;
- void initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, const Eigen::Vector3s& _t_ECEF_antenaENU,
- const Eigen::VectorXs& _pose_MAP_frame2, const Eigen::Vector3s& _t_ECEF_antena2);
- void initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, const Eigen::VectorXs& _pose_MAP_frame2,
- const Eigen::Vector3s& _v_ECEF_antena1_antena2);
-
- public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics);
-
-
+ void initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, const Eigen::Vector3d& _t_ECEF_antenaENU,
+ const Eigen::VectorXd& _pose_MAP_frame2, const Eigen::Vector3d& _t_ECEF_antena2);
+ void initializeEnuMapYaw(const Eigen::VectorXd& _pose_MAP_frame1, const Eigen::VectorXd& _pose_MAP_frame2,
+ const Eigen::Vector3d& _v_ECEF_antena1_antena2);
+ Eigen::Vector3d computeFrameAntennaPosEcef(const FrameBasePtr&) const;
};
inline bool SensorGnss::isEnuDefined() const
@@ -97,52 +155,109 @@ inline bool SensorGnss::isEnuDefined() const
inline bool SensorGnss::isEnuMapInitialized() const
{
- return ENU_MAP_initialized_;
+ return t_ENU_MAP_initialized_ && R_ENU_MAP_initialized_;
+}
+
+inline bool SensorGnss::isEnuMapTranslationInitialized() const
+{
+ return t_ENU_MAP_initialized_;
+}
+
+inline bool SensorGnss::isEnuMapRotationInitialized() const
+{
+ return R_ENU_MAP_initialized_;
+}
+
+inline bool SensorGnss::isEnuModeEcef() const
+{
+ return params_->ENU_mode == "ECEF";
+}
+
+inline bool SensorGnss::isEnuModeAuto() const
+{
+ return params_->ENU_mode == "auto";
}
inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
{
- return getStateBlockPtrStatic(3);
+ return getStateBlock('t');
}
inline StateBlockPtr SensorGnss::getEnuMapRoll() const
{
- return getStateBlockPtrStatic(4);
+ return getStateBlock('r');
}
inline StateBlockPtr SensorGnss::getEnuMapPitch() const
{
- return getStateBlockPtrStatic(5);
+ return getStateBlock('p');
}
inline StateBlockPtr SensorGnss::getEnuMapYaw() const
{
- return getStateBlockPtrStatic(6);
+ return getStateBlock('y');
}
-inline const Eigen::Matrix3s& SensorGnss::getREnuEcef() const
+inline const Eigen::Matrix3d& SensorGnss::getREnuEcef() const
{
return R_ENU_ECEF_;
}
-inline const Eigen::Vector3s& SensorGnss::gettEnuEcef() const
+inline const Eigen::Vector3d& SensorGnss::gettEnuEcef() const
{
return t_ENU_ECEF_;
}
+inline Eigen::Vector3d SensorGnss::gettEnuMap() const
+{
+ return getEnuMapTranslation()->getState();
+}
+
template<typename T>
inline Eigen::Matrix<T,3,3> SensorGnss::computeREnuMap(const T& _r,const T& _p,const T& _y) const
{
- Eigen::Matrix<T,3,3> R_ENU_MAP = (Eigen::AngleAxis<T>(_r, Eigen::Matrix<T,3,1>::UnitX())
- * Eigen::AngleAxis<T>(_p, Eigen::Matrix<T,3,1>::UnitY())
- * Eigen::AngleAxis<T>(_y, Eigen::Matrix<T,3,1>::UnitZ())).toRotationMatrix();
+ return Eigen::Matrix<T,3,3>(Eigen::AngleAxis<T>(_r, Eigen::Matrix<T,3,1>::UnitX()) *
+ Eigen::AngleAxis<T>(_p, Eigen::Matrix<T,3,1>::UnitY()) *
+ Eigen::AngleAxis<T>(_y, Eigen::Matrix<T,3,1>::UnitZ()));
+}
+
+inline Eigen::Matrix3d SensorGnss::getREnuMap() const
+{
+ return computeREnuMap(getEnuMapRoll() ->getState()(0),
+ getEnuMapPitch()->getState()(0),
+ getEnuMapYaw() ->getState()(0));
+}
+
+inline void SensorGnss::setEnuMapInitialized(const bool& _init)
+{
+ t_ENU_MAP_initialized_ = _init;
+ R_ENU_MAP_initialized_ = _init;
+}
+
+inline void SensorGnss::setEnuMapTranslationInitialized(const bool& _init)
+{
+ t_ENU_MAP_initialized_ = _init;
+}
+
+inline void SensorGnss::setEnuMapRotationInitialized(const bool& _init)
+{
+ R_ENU_MAP_initialized_ = _init;
+}
- return R_ENU_MAP;
+inline bool SensorGnss::isEnuMapFixed() const
+{
+ return getEnuMapTranslation()->isFixed() and
+ getEnuMapRoll()->isFixed() and
+ getEnuMapPitch()->isFixed() and
+ getEnuMapYaw()->isFixed();
+}
- // TODO: store R's when T=Scalar
- //Eigen::Matrix<T,3,3> R_roll_T, R_pitch_T, R_yaw_T;
- //if (!getRollState()->isFixed())
- // *R_roll_ptr_ = Eigen::AngleAxis<Scalar>(0.25*M_PI, Eigen::Vector3s::UnitX());
+inline void SensorGnss::fixEnuMap()
+{
+ getEnuMapTranslation()->fix();
+ getEnuMapRoll()->fix();
+ getEnuMapPitch()->fix();
+ getEnuMapYaw()->fix();
}
} // namespace wolf
diff --git a/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h b/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
new file mode 100644
index 000000000..2124f1506
--- /dev/null
+++ b/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
@@ -0,0 +1,64 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_TDCP_H_
+#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_TDCP_H_
+
+#include "core/tree_manager/tree_manager_sliding_window.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowTdcp)
+WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowTdcp)
+
+struct ParamsTreeManagerSlidingWindowTdcp : public ParamsTreeManagerSlidingWindow
+{
+ ParamsTreeManagerSlidingWindowTdcp() = default;
+ ParamsTreeManagerSlidingWindowTdcp(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsTreeManagerSlidingWindow(_unique_name, _server)
+ {
+ }
+ std::string print() const override
+ {
+ return "\n" + ParamsTreeManagerSlidingWindow::print();
+ }
+};
+
+class TreeManagerSlidingWindowTdcp : public TreeManagerSlidingWindow
+{
+ public:
+ TreeManagerSlidingWindowTdcp(ParamsTreeManagerSlidingWindowTdcpPtr _params);
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowTdcp, ParamsTreeManagerSlidingWindowTdcp)
+
+ ~TreeManagerSlidingWindowTdcp() override{}
+
+ void keyFrameCallback(FrameBasePtr _key_frame) override;
+
+ protected:
+ ParamsTreeManagerSlidingWindowTdcpPtr params_sw_sb_;
+ SensorBasePtr sensor_imu_;
+ Eigen::Matrix6d cov_bias_;
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_ */
diff --git a/internal/config.h.in b/internal/config.h.in
index b9008e196..7014ca320 100644
--- a/internal/config.h.in
+++ b/internal/config.h.in
@@ -24,8 +24,8 @@
// which will be added to the include path for compilation,
// and installed with the public wolf headers.
-#ifndef WOLF_INTERNAL_${UPPERNAME}_CONFIG_H_
-#define WOLF_INTERNAL_${UPPERNAME}_CONFIG_H_
+#ifndef WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
+#define WOLF_INTERNAL_${UPPER_NAME}_CONFIG_H_
#cmakedefine _WOLF_DEBUG
diff --git a/license_header_2022.txt b/license_header_2022.txt
new file mode 100644
index 000000000..0c987025f
--- /dev/null
+++ b/license_header_2022.txt
@@ -0,0 +1,17 @@
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
diff --git a/package.xml b/package.xml
index e94a5f327..fc94ad080 100644
--- a/package.xml
+++ b/package.xml
@@ -20,8 +20,6 @@
<!-- wolf does not REQUIRE the following, they are added here for the dependency tree -->
<build_depend>Ceres</build_depend>
- <build_depend>laser_scan_utils</build_depend>
- <build_depend>raw_gps_utils</build_depend>
<build_depend>OpenCV</build_depend>
<build_depend>YAMLCPP</build_depend>
diff --git a/src/capture/capture_gnss.cpp b/src/capture/capture_gnss.cpp
new file mode 100644
index 000000000..299f515a9
--- /dev/null
+++ b/src/capture/capture_gnss.cpp
@@ -0,0 +1,53 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss/capture/capture_gnss.h"
+
+
+namespace wolf {
+
+CaptureGnss::CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUtils::SnapshotPtr _snapshot) :
+ CaptureBase("CaptureGnss", _ts, _sensor_ptr),
+ snapshot_(_snapshot)
+{
+ // Clock bias
+ assert(_sensor_ptr->getStateBlock('T') != nullptr and _sensor_ptr->isStateBlockDynamic('T'));
+ addStateBlock('T', std::make_shared<StateBlock>(1,true), nullptr);
+
+ // interconstellation clock bias
+ assert(_sensor_ptr->getStateBlock('G') != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic('G'))
+ addStateBlock('G', std::make_shared<StateBlock>(1,true), nullptr);
+ assert(_sensor_ptr->getStateBlock('E') != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic('E'))
+ addStateBlock('E', std::make_shared<StateBlock>(1,true), nullptr);
+ assert(_sensor_ptr->getStateBlock('M') != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic('M'))
+ addStateBlock('M', std::make_shared<StateBlock>(1,true), nullptr);
+
+}
+
+CaptureGnss::~CaptureGnss()
+{
+ //std::cout << "Destroying GPS fix capture...\n";
+}
+
+} //namespace wolf
diff --git a/src/capture/capture_gnss_fix.cpp b/src/capture/capture_gnss_fix.cpp
index d7f78a9a1..5ddd2bb8a 100644
--- a/src/capture/capture_gnss_fix.cpp
+++ b/src/capture/capture_gnss_fix.cpp
@@ -1,12 +1,33 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
#include "gnss/capture/capture_gnss_fix.h"
namespace wolf {
-CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
+CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::Vector3d& _position, const Eigen::Matrix3d& _covariance, bool _ecef_coordinates) :
CaptureBase("CaptureGnssFix", _ts, _sensor_ptr),
- data_(_data),
- data_covariance_(_data_covariance)
+ position_ECEF_(_ecef_coordinates ? _position : GnssUtils::latLonAltToEcef(_position)),
+ covariance_ECEF_(_ecef_coordinates ?_covariance : GnssUtils::enuToEcefCov(_position, _covariance))
{
//
}
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 26b58a4e6..31f95fd15 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -1,17 +1,37 @@
-#include "gnss/factor/factor_gnss_fix_2D.h"
-#include "gnss/factor/factor_gnss_fix_3D.h"
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss/capture/capture_gnss.h"
+#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/factor/factor_gnss_fix_2d.h"
+#include "gnss/factor/factor_gnss_fix_3d.h"
#include "gnss/feature/feature_gnss_fix.h"
#include "gnss/processor/processor_gnss_fix.h"
-#include "gnss/capture/capture_gnss_fix.h"
namespace wolf
{
-ProcessorGnssFix::ProcessorGnssFix(ProcessorParamsGnssFixPtr _params_gnss) :
- ProcessorBase("ProcessorGnssFix", _params_gnss),
-
- params_gnss_(_params_gnss),
- first_capture_(nullptr)
+ProcessorGnssFix::ProcessorGnssFix(ParamsProcessorGnssFixPtr _params_gnss) :
+ ProcessorBase("ProcessorGnssFix", 0, _params_gnss),
+ params_gnss_(_params_gnss)
{
//
}
@@ -24,137 +44,235 @@ ProcessorGnssFix::~ProcessorGnssFix()
void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
{
// TODO: keep captures in a buffer and deal with KFpacks
- //WOLF_DEBUG("ProcessorGnssFix::process()");
- incoming_capture_ = std::static_pointer_cast<CaptureGnssFix>(_capture);
+ WOLF_DEBUG("PR ", getName()," process() capture type: ", _capture->getType());
+ incoming_capture_ = _capture;
+
+ bool KF_created = false;
+
+ // Check type of capture: GNSS raw
+ bool israw = false;
+ if (std::dynamic_pointer_cast<CaptureGnss>(_capture)!=nullptr)
+ israw = true;
+ // if not raw nor Fix, not processable
+ else if (std::dynamic_pointer_cast<CaptureGnssFix>(_capture)==nullptr)
+ return;
+
+ // Only process raw if fix_from_raw
+ if (israw and !params_gnss_->fix_from_raw)
+ return;
+
+ // Only process fix if not fix_from_raw
+ if (!israw and params_gnss_->fix_from_raw)
+ return;
FrameBasePtr new_frame = nullptr;
FactorBasePtr new_fac = nullptr;
+ // emplace features
+ if (israw)
+ {
+ auto raw_capture = std::static_pointer_cast<CaptureGnss>(incoming_capture_);
+ incoming_pos_out_ = GnssUtils::computePos(*(raw_capture->getObservations()), *(raw_capture->getNavigation()), params_gnss_->compute_pos_opt);
+ if (!incoming_pos_out_.success) // error
+ {
+ WOLF_DEBUG("computePos failed with msg: ", incoming_pos_out_.msg);
+ return;
+ }
+ }
+ else
+ {
+ WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming navsatfix. Type:", incoming_capture_->getType());
+ auto fix_capture = std::static_pointer_cast<CaptureGnssFix>(incoming_capture_);
+ incoming_pos_out_.time = fix_capture->getGpsTimeStamp().getSeconds(); // time_t time;
+ incoming_pos_out_.sec = 1e-9 * fix_capture->getGpsTimeStamp().getSeconds(); // double sec;
+ incoming_pos_out_.pos = fix_capture->getPositionEcef(); // Eigen::Vector3d pos; // position (m)
+ incoming_pos_out_.vel = Eigen::Vector3d::Zero(); // Eigen::Vector3d vel; // velocity (m/s)
+ incoming_pos_out_.pos_covar = fix_capture->getCovarianceEcef(); // Eigen::Matrix3d pos_covar; // position covariance (m^2)
+ incoming_pos_out_.type = 0; // int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
+ }
+ auto incoming_feature = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_pos_out_);
+
// ALREADY CREATED KF
- PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_gnss_->time_tolerance);
- if (KF_pack && KF_pack->key_frame != last_KF_)
+ FrameBasePtr keyframe = buffer_frame_.select( incoming_capture_->getTimeStamp(), params_gnss_->time_tolerance);
+ if (keyframe and last_KF_capture_ and keyframe == last_KF_capture_->getFrame())
+ keyframe = nullptr;
+ if (keyframe)
{
- WOLF_DEBUG( "PR ", getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
- new_frame = KF_pack->key_frame;
+ WOLF_DEBUG("PR ", getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , keyframe->id() , " TS: ", keyframe->getTimeStamp());
+ new_frame = keyframe;
}
// MAKE KF
- else if (voteForKeyFrame() && permittedKeyFrame())
+ else if (permittedKeyFrame() && voteForKeyFrame())
{
- new_frame = getProblem()->emplaceFrame(KEY, incoming_capture_->getTimeStamp());
- getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_gnss_->time_tolerance);
- WOLF_DEBUG( "PR ", getName()," - capture ", incoming_capture_->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp());
+ WOLF_DEBUG("PR ", getName()," emplacing KF TS = ", incoming_capture_->getTimeStamp());
+ new_frame = getProblem()->emplaceFrame(incoming_capture_->getTimeStamp());
+ KF_created = true;
}
+ // OTHERWISE return
+ else
+ return;
// ESTABLISH FACTOR
- if (new_frame)
- {
- // LINK CAPTURE
- incoming_capture_->link(new_frame); // Add incoming Capture to the new Frame
-
- // EMPLACE FEATURES
- WOLF_DEBUG( "PR ", getName()," - emplacing the feature...");
- auto ftr = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_capture_->getData(),incoming_capture_->getDataCovariance());
+ // link capture
+ incoming_capture_->link(new_frame);
+ // emplace factor
+ new_fac = emplaceFactor(incoming_feature);
- // EMPLACE FACTOR
- new_fac = emplaceFactor(ftr);
+ // outlier rejection (only can be evaluated if ENU defined and ENU-MAP well initialized: fixed)
+ if (params_gnss_->remove_outliers and
+ sensor_gnss_->isEnuDefined() and
+ sensor_gnss_->isEnuMapFixed() and
+ detectOutlier(new_fac))
+ {
+ new_frame->remove();
+ return;
+ }
- // outlier rejection
- if (sensor_gnss_->isEnuDefined() && sensor_gnss_->isEnuMapInitialized())
- if (rejectOutlier(new_fac))
- new_fac = nullptr;
+ // store last KF
+ last_KF_capture_ = incoming_capture_;
+ last_KF_feature_ = incoming_feature;
- // store last KF
- if (new_fac)
- last_KF_= new_frame;
+ // Define ENU (if not defined)
+ if (!sensor_gnss_->isEnuDefined())
+ {
+ WOLF_DEBUG("Defining ecef enu");
+ sensor_gnss_->setEcefEnu(incoming_feature->getMeasurement());
}
- // SET ECEF_ENU IF:
- // 1 - not initialized
- // 2 - factor established
- if (!sensor_gnss_->isEnuDefined() && new_fac != nullptr)
+ // Store the first capture that established a factor (for initializing ENU-MAP)
+ if (first_frame_state_.empty() and
+ not sensor_gnss_->isEnuMapFixed())
{
- WOLF_DEBUG("setting ecef enu");
- sensor_gnss_->setEcefEnu(incoming_capture_->getData());
-
- // Store the first capture that established a factor
- first_capture_ = incoming_capture_;
+ first_frame_state_ = incoming_capture_->getFrame()->getState();
+ first_pos_ = incoming_feature->getMeasurement();
}
-
- // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS:
- // 1 - ENU-ECEF defined
- // 2 - not initialized
- // 3 - current capture in key-frame with factor
- // 4 - two factors established (first and current) in frames separated enough ( > enu_map_init_dist_min)
- if ( sensor_gnss_->isEnuDefined() &&
- !sensor_gnss_->isEnuMapInitialized() &&
- new_fac != nullptr &&
- first_capture_ != nullptr && first_capture_->getFrame() != nullptr && incoming_capture_->getFrame() != nullptr &&
- (first_capture_->getFrame()->getState()-incoming_capture_->getFrame()->getState()).norm() > params_gnss_->enu_map_init_dist_min)
+ // 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
+ sensor_gnss_->isEnuDefined() and
+ not sensor_gnss_->isEnuMapInitialized() and
+ not sensor_gnss_->isEnuMapFixed() and
+ (first_pos_-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min)
{
- WOLF_DEBUG("initializing enu map");
- sensor_gnss_->initializeEnuMap(first_capture_->getFrame()->getState(), first_capture_->getData(),
- incoming_capture_->getFrame()->getState(), incoming_capture_->getData());
+ assert(first_frame_state_.count('P') and first_frame_state_.count('O') and incoming_capture_->getFrame() != nullptr);
+
+ sensor_gnss_->initializeEnuMap(first_frame_state_.vector("PO"),
+ first_pos_,
+ incoming_capture_->getFrame()->getState().vector("PO"),
+ incoming_feature->getMeasurement());
+ // Set as not-initialized if factors not separated enough ( < enu_map_init_dist_max)
+ if ((first_pos_ - incoming_feature->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
+ sensor_gnss_->setEnuMapInitialized(false);
}
+
+ // Fix ENU-MAP if separated enough from first pos ( > enu_map_fix_dist )
+ if ((first_pos_ - incoming_feature->getMeasurement()).norm() > params_gnss_->enu_map_fix_dist)
+ sensor_gnss_->fixEnuMap();
+
+ // Notify if KF created
+ if (KF_created)
+ getProblem()->keyFrameCallback(new_frame, shared_from_this());
}
FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
{
- // CREATE CONSTRAINT --------------------
//WOLF_DEBUG("creating the factor...");
- // 2D
+ // 2d
if (getProblem()->getDim() == 2)
- return FactorBase::emplace<FactorGnssFix2D>(_ftr, _ftr, sensor_gnss_, shared_from_this(), false, FAC_ACTIVE);
- // 3D
+ return FactorBase::emplace<FactorGnssFix2d>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
+ // 3d
else
- return FactorBase::emplace<FactorGnssFix3D>(_ftr, _ftr, sensor_gnss_, shared_from_this(), false, FAC_ACTIVE);
+ return FactorBase::emplace<FactorGnssFix3d>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
}
-bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac)
+bool ProcessorGnssFix::voteForKeyFrame() const
{
- // Cast feature
- auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
- // copy states
- Eigen::VectorXs x(gnss_ftr->getCapture()->getFrame()->getP()->getState());
- Eigen::VectorXs o(gnss_ftr->getCapture()->getFrame()->getP()->getState());
- Eigen::VectorXs x_antena(sensor_gnss_->getStateBlock(0)->getState());
- Eigen::VectorXs t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
- Eigen::VectorXs roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
- Eigen::VectorXs pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
- Eigen::VectorXs yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
- // create Scalar* array of a copy of the state
- Scalar* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(),
- pitch_ENU_map.data(), yaw_ENU_map.data()};
- // create residuals pointer
- Eigen::VectorXs residuals(3);
- // evaluate the factor in this state
- fac->evaluate(parameters, residuals.data(), nullptr);
- // discard if residual too high evaluated at the current estimation
- if (residuals.norm() > 1e3)
+ //WOLF_DEBUG("voteForKeyFrame...");
+ // Null lastKF
+ if (not last_KF_capture_)
{
- WOLF_WARN("Discarding GNSS FIX Factor, considered OUTLIER");
- WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residuals).transpose());
- fac->remove();
+ WOLF_DEBUG("KF because of null lastKF");
return true;
}
- return false;
-}
-bool ProcessorGnssFix::voteForKeyFrame() const
-{
// Depending on time since the last KF with gnssfix capture
- if (!last_KF_ || (incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
+ if (not last_KF_capture_ or
+ (incoming_capture_->getTimeStamp() - last_KF_capture_->getTimeStamp()) > params_gnss_->max_time_span)
+ {
+ WOLF_DEBUG("KF because of time since last KF");
return true;
+ }
- // Distance criterion
- if ((incoming_capture_->getFrame()->getP()->getState() - last_KF_->getP()->getState()).norm() > params_gnss_->dist_traveled)
+ // ENU not defined
+ if (not sensor_gnss_->isEnuDefined())
+ {
+ WOLF_DEBUG("KF because of enu not defined");
return true;
+ }
- // TODO: more alternatives?
+ // ENU-MAP not initialized and can be initialized
+ if (params_gnss_->init_enu_map and
+ not first_frame_state_.empty() and
+ sensor_gnss_->isEnuDefined() and
+ not sensor_gnss_->isEnuMapInitialized() and
+ not sensor_gnss_->isEnuMapFixed() and
+ (first_pos_-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min and
+ (first_pos_-incoming_pos_out_.pos).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_pos_out_.pos - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
+ {
+ WOLF_DEBUG("KF because of distance criterion: ", (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm());
+ return true;
+ }
// otherwise
return false;
}
+bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
+{
+ WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
+
+ // Cast feature
+ auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
+
+ // copy states
+ Eigen::VectorXd x(gnss_ftr->getCapture()->getFrame()->getP()->getState());
+ Eigen::VectorXd o(gnss_ftr->getCapture()->getFrame()->getO()->getState());
+ Eigen::VectorXd x_antena(sensor_gnss_->getP()->getState());
+ Eigen::VectorXd t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
+ Eigen::VectorXd roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
+ Eigen::VectorXd pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
+ Eigen::VectorXd yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
+
+ // create double* array of a copy of the state
+ double* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(),
+ pitch_ENU_map.data(), yaw_ENU_map.data()};
+ // residual
+ Eigen::Vector3d residual;
+
+ // evaluate the factor in this state
+ fac->evaluate(parameters, residual.data(), nullptr);
+
+ // evaluate if residual too high at the current estimation
+ if (residual.norm() > params_gnss_->outlier_residual_th)
+ {
+ WOLF_WARN("Discarding GNSS FIX Factor, considered OUTLIER");
+ WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),
+ "\n\tError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual).transpose(),
+ "\n\tResidual: ",residual.transpose(),
+ "\n\tResidual norm: ",residual.norm(), "(max: ", params_gnss_->outlier_residual_th, ")");
+ return true;
+ }
+ return false;
+}
+
bool ProcessorGnssFix::storeKeyFrame(FrameBasePtr _frame_ptr)
{
return true;
@@ -168,19 +286,11 @@ void ProcessorGnssFix::configure(SensorBasePtr _sensor)
sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_sensor);
};
-ProcessorBasePtr ProcessorGnssFix::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
-{
- auto prc = std::make_shared<ProcessorGnssFix>(std::static_pointer_cast<ProcessorParamsGnssFix>(_params));
- prc->setName(_unique_name);
- return prc;
-}
-
-
} // namespace wolf
-
-// Register in the SensorFactory
-#include "core/processor/processor_factory.h"
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("ProcessorGnssFix",ProcessorGnssFix)
+WOLF_REGISTER_PROCESSOR(ProcessorGnssFix)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorGnssFix);
} // namespace wolf
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index 3636ccd73..99c73f2fc 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -1,4 +1,25 @@
-#include "gnss/factor/factor_gnss_single_diff_2D.h"
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss/factor/factor_gnss_single_diff_2d.h"
#include "gnss/feature/feature_gnss_single_diff.h"
#include "gnss/processor/processor_gnss_single_diff.h"
#include "gnss/capture/capture_gnss_single_diff.h"
@@ -6,8 +27,8 @@
namespace wolf
{
-ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ProcessorParamsGnssSingleDiffPtr _params_gnss) :
- ProcessorBase("GNSS SINGLE DIFFERENCES", _params_gnss),
+ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ParamsProcessorGnssSingleDiffPtr _params_gnss) :
+ ProcessorBase("ProcessorGnssSingleDiff", 0, _params_gnss),
params_gnss_(_params_gnss)
{
//
@@ -48,7 +69,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
// MAKE KF
else if (voteForKeyFrame() && permittedKeyFrame())
{
- new_frame = getProblem()->emplaceFrame(KEY, incoming_capture_->getTimeStamp());
+ new_frame = getProblem()->emplaceKeyFrame( incoming_capture_->getTimeStamp());
getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_->time_tolerance);
WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp());
}
@@ -90,12 +111,12 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr _ftr)
{
//WOLF_DEBUG("creating the factor...");
- // 2D
+ // 2d
if (getProblem()->getDim() == 2)
- return FactorBase::emplace<FactorGnssSingleDiff2D>(_ftr, _ftr, incoming_capture_->getOriginFrame(), sensor_gnss_, shared_from_this());
- // 3D TODO
+ return FactorBase::emplace<FactorGnssSingleDiff2d>(_ftr, _ftr, incoming_capture_->getOriginFrame(), sensor_gnss_, shared_from_this(), params_->apply_loss_function);
+ // 3d TODO
else
- std::runtime_error("Single Differences in 3D not implemented yet.");
+ std::runtime_error("Single Differences in 3d not implemented yet.");
return nullptr;
}
@@ -114,9 +135,9 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame() const
// Distance criterion
std::cout << "params_gnss_->dist_traveled = " << params_gnss_->dist_traveled << std::endl;
- Eigen::Vector2s v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
+ Eigen::Vector2d v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
std::cout << "v_current_origin: " << v_current_origin.transpose() << std::endl;
- Eigen::Vector2s v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
+ Eigen::Vector2d v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
std::cout << "v_origin_last_KF: " << v_origin_last_KF.transpose() << std::endl;
std::cout << "v_current_origin + v_origin_last_KF: " << (v_current_origin + v_origin_last_KF).transpose() << std::endl;
if ((v_current_origin + v_origin_last_KF).norm() > params_gnss_->dist_traveled)
@@ -141,9 +162,9 @@ void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor)
sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_sensor);
};
-ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params)
+ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ParamsProcessorBasePtr _params)
{
- ProcessorGnssSingleDiffPtr prc = std::make_shared<ProcessorGnssSingleDiff>(std::static_pointer_cast<ProcessorParamsGnssSingleDiff>(_params));
+ ProcessorGnssSingleDiffPtr prc = std::make_shared<ProcessorGnssSingleDiff>(std::static_pointer_cast<ParamsProcessorGnssSingleDiff>(_params));
prc->setName(_unique_name);
return prc;
}
@@ -151,8 +172,8 @@ ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name
} // namespace wolf
-// Register in the SensorFactory
-#include "core/processor/processor_factory.h"
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("GNSS SINGLE DIFFERENCES",ProcessorGnssSingleDiff)
+WOLF_REGISTER_PROCESSOR(ProcessorGnssSingleDiff)
} // namespace wolf
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
new file mode 100644
index 000000000..c087f7074
--- /dev/null
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -0,0 +1,270 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "../../include/gnss/factor/factor_gnss_tdcp_2d.h"
+#include "../../include/gnss/factor/factor_gnss_tdcp_3d.h"
+#include "gnss/capture/capture_gnss_tdcp.h"
+#include "gnss/feature/feature_gnss_tdcp.h"
+#include "gnss/feature/feature_gnss_fix.h"
+#include "gnss/processor/processor_gnss_tdcp.h"
+
+namespace wolf
+{
+
+ProcessorGnssTdcp::ProcessorGnssTdcp(ParamsProcessorGnssTdcpPtr _params_gnss) :
+ ProcessorBase("ProcessorGnssTdcp", 0, _params_gnss),
+ params_tdcp_(_params_gnss)
+{
+ //
+}
+
+ProcessorGnssTdcp::~ProcessorGnssTdcp()
+{
+ //
+}
+
+void ProcessorGnssTdcp::processCapture(CaptureBasePtr _capture)
+{
+ if (std::dynamic_pointer_cast<CaptureGnss>(_capture))
+ buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
+
+ /*
+ // TODO: keep captures in a buffer and deal with KFpacks
+ WOLF_DEBUG("ProcessorGnssTdcp::process()");
+ incoming_capture_ = std::static_pointer_cast<CaptureGnssTdcp>(_capture);
+
+ // discard capture with null or non-key origin frame
+ if (incoming_capture_->getOriginFrame() == nullptr || !incoming_capture_->getOriginFrame()->isKey())
+ {
+ WOLF_WARN("process single difference with null frame origin, skipping...");
+ return;
+ }
+
+ if (last_KF_ == nullptr && incoming_capture_->getOriginFrame() != nullptr && incoming_capture_->getOriginFrame()->isKey())
+ last_KF_ = incoming_capture_->getOriginFrame();
+
+ // NEW KF? ------------------------------------------------
+ FrameBasePtr new_frame = nullptr;
+
+ // ALREADY CREATED KF
+ PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_->time_tolerance);
+ if (KF_pack && KF_pack->key_frame != incoming_capture_->getOriginFrame())
+ {
+ new_frame = KF_pack->key_frame;
+ WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
+ }
+ // MAKE KF
+ else if (voteForKeyFrame() && permittedKeyFrame())
+ {
+ new_frame = getProblem()->emplaceKeyFrame( incoming_capture_->getTimeStamp());
+ getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_->time_tolerance);
+ WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp());
+ }
+
+ // ESTABLISH FACTOR ------------------------------------------------
+ if (new_frame)
+ {
+ // LINK CAPTURE
+ _capture->link(new_frame); // Add incoming Capture to the new Frame
+
+ // EMPLACE FEATURE
+ auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(incoming_capture_, incoming_capture_->getData(),incoming_capture_->getDataCovariance());
+
+ // ADD FACTOR
+ emplaceFactor(ftr);
+
+ // store last KF
+ last_KF_ = new_frame;
+ }
+
+ // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS ------------------------------------------------
+ // 1 - ENU-ECEF defined
+ // 2 - not initialized
+ // 3 - current capture in key-frame with factor
+ // 4 - frames constained by the factor separated enough ( > enu_map_init_dist_min)
+ if ( sensor_gnss_->isEnuDefined() &&
+ !sensor_gnss_->isEnuMapRotationInitialized() &&
+ new_frame != nullptr &&
+ incoming_capture_->getFrame() != nullptr && incoming_capture_->getFrame()->isKey() &&
+ incoming_capture_->getData().norm() > params_tdcp_->enu_map_init_dist_min)
+ {
+ WOLF_INFO("initializing enu map");
+ sensor_gnss_->initializeEnuMapYaw(incoming_capture_->getOriginFrame()->getState(),
+ incoming_capture_->getFrame()->getState(),
+ incoming_capture_->getData());
+ }
+ */
+}
+
+void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe)
+{
+ WOLF_INFO("ProcessorGnssTdcp::processKeyFrame()");
+
+ // update last_KF
+ if (!last_KF_ or last_KF_->getTimeStamp() < _keyframe->getTimeStamp())
+ last_KF_ = _keyframe;
+
+ // Add GNSS capture if not already stored
+ auto cap_gnss = _keyframe->getCaptureOf(this->getSensor(), "CaptureGnss");
+ if (!cap_gnss)
+ {
+ // find capture in timestamp
+ cap_gnss = buffer_capture_.select(_keyframe->getTimeStamp(), params_tdcp_->time_tolerance);
+
+ // remove previous captures
+ buffer_capture_.removeUpTo(_keyframe->getTimeStamp());
+
+ // return if no capture GNSS found in that timestamp
+ if (!cap_gnss)
+ return;
+
+ // remove previous captures
+ buffer_capture_.removeUpTo(cap_gnss->getTimeStamp());
+
+ // Add found Capture to the new Frame
+ cap_gnss->link(_keyframe);
+ }
+
+ // Iterate over all KF of the trajectory with GNSS captures
+ for (auto KF_it = getProblem()->getTrajectory()->rbegin();
+ KF_it != getProblem()->getTrajectory()->rend();
+ KF_it++)
+ {
+ auto KF_ref = *KF_it;
+ if (_keyframe->getTimeStamp() < KF_ref->getTimeStamp())
+ continue;
+
+ // jump the same keyframe
+ if (KF_ref == _keyframe)
+ continue;
+
+ // jump KF without GNSS captures
+ auto cap_gnss_ref = KF_ref->getCaptureOf(this->getSensor(), "CaptureGnss");
+ if (!cap_gnss_ref)
+ continue;
+
+ // stop if KF_ref too old w.r.t. keyframe
+ if (_keyframe->getTimeStamp() - KF_ref->getTimeStamp() > params_tdcp_->tdcp.time_window)
+ break;
+
+ // Get feature GNSS fix
+ FeatureGnssFixPtr feat_fix = nullptr;
+ for (auto cap : KF_ref->getCaptureList())
+ {
+ for (auto ftr : cap->getFeatureList())
+ {
+ feat_fix = std::dynamic_pointer_cast<FeatureGnssFix>(ftr);
+ if (feat_fix)
+ break;
+ }
+ if (feat_fix)
+ break;
+ }
+
+ // (for now) jump KF without GNSS Fix features
+ if (!feat_fix)
+ continue; // TODO use KF state and compute in ECEF coordinates
+
+ WOLF_INFO("Past KF with feature fix found. Performing TDCP!");
+
+ // TDCP
+ std::set<int> discarded_sats;
+ auto tdcp_output = GnssUtils::Tdcp(std::static_pointer_cast<CaptureGnss>(cap_gnss_ref)->getSnapshot(),
+ std::static_pointer_cast<CaptureGnss>(cap_gnss)->getSnapshot(),
+ feat_fix->getMeasurement(),
+ Eigen::Vector4d::Zero(),
+ discarded_sats,
+ params_tdcp_->tdcp,
+ params_tdcp_->compute_pos_opt);
+ if (tdcp_output.success)
+ {
+ WOLF_INFO("TDCP successfully performed!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
+ // EMPLACE FEATURE
+ auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(cap_gnss, tdcp_output.d, tdcp_output.cov_d);
+
+ // ADD FACTOR
+ emplaceFactor(ftr, KF_ref);
+ }
+ }
+}
+
+FactorBasePtr ProcessorGnssTdcp::emplaceFactor(FeatureBasePtr _ftr, FrameBasePtr _frm_ref)
+{
+ //WOLF_DEBUG("creating the factor...");
+ // 2D
+ if (getProblem()->getDim() == 2)
+ return FactorBase::emplace<FactorGnssTdcp2d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this(), params_->apply_loss_function);
+ // 3D
+ else
+ return FactorBase::emplace<FactorGnssTdcp3d>(_ftr, _ftr, _frm_ref, sensor_gnss_, shared_from_this(), params_->apply_loss_function);
+
+ return nullptr;
+}
+
+bool ProcessorGnssTdcp::voteForKeyFrame() const
+{
+// if (last_KF_==nullptr)
+// return true;
+//
+// //std::cout << "params_tdcp_->max_time_span" << params_tdcp_->max_time_span << std::endl;
+// //std::cout << "(incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp())" << (incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) << std::endl;
+//
+// // Elapsed time criterion: From the last KF with gnssfix capture
+// if ((incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_tdcp_->max_time_span)
+// {
+// WOLF_DEBUG("voting for KF: elapsed time criterion");
+// return true;
+// }
+//
+// // Distance criterion: From the last KF with gnssfix capture
+// Eigen::Vector2d v_origin_current = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
+// Eigen::Vector2d v_lastKF_origin = incoming_capture_->getOriginFrame()->getP()->getState() - last_KF_->getP()->getState();
+// //std::cout << "params_tdcp_->dist_traveled" << params_tdcp_->dist_traveled << std::endl;
+// //std::cout << "v_origin_current: " << v_origin_current.transpose() << std::endl;
+// //std::cout << "v_lastKF_origin: " << v_lastKF_origin.transpose() << std::endl;
+// //std::cout << "v_lastKF_origin + v_origin_current: " << (v_lastKF_origin + v_origin_current).transpose() << std::endl;
+// if ((v_lastKF_origin + v_origin_current).norm() > params_tdcp_->dist_traveled)
+// {
+// WOLF_DEBUG("voting for KF: distance criterion");
+// return true;
+// }
+//
+// // TODO: more alternatives?
+
+ // otherwise
+ return false;
+}
+
+void ProcessorGnssTdcp::configure(SensorBasePtr _sensor)
+{
+ sensor_gnss_ = std::dynamic_pointer_cast<SensorGnss>(_sensor);
+ assert(sensor_gnss_ != nullptr);
+};
+
+} // namespace wolf
+
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorGnssTdcp)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorGnssTdcp)
+} // namespace wolf
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
new file mode 100644
index 000000000..ace52948c
--- /dev/null
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -0,0 +1,870 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss/processor/processor_tracker_gnss.h"
+
+#include "gnss/capture/capture_gnss.h"
+#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_pseudo_range.h"
+#include "gnss/factor/factor_gnss_fix_3d.h"
+#include "gnss_utils/utils/rcv_position.h"
+#include "gnss_utils/tdcp.h"
+
+namespace wolf
+{
+
+ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) :
+ ProcessorTrackerFeature("ProcessorTrackerGnss", "PO", 3, _params_tracker_gnss),
+ params_tracker_gnss_(_params_tracker_gnss),
+ outliers_pseudorange_(0),
+ outliers_tdcp_(0),
+ inliers_pseudorange_(0),
+ inliers_tdcp_(0),
+ first_pos_(Eigen::Vector3d::Zero())
+{
+}
+
+void ProcessorTrackerGnss::preProcess()
+{
+ WOLF_DEBUG("ProcessorTrackerGnss::preProcess");
+
+ GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot();
+ GnssUtils::Options copy_opt = params_tracker_gnss_->gnss_opt;
+ bool eph_sbas34 = params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS3 or
+ params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS4;
+ bool iono_sbas34 = params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS3 or
+ params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS4;
+
+#ifdef _WOLF_DEBUG
+ int n_initial = inc_snapshot->getObservations()->size();
+ std::string initial_str;
+ for (auto obs : inc_snapshot->getObservations()->getObservations())
+ initial_str += std::to_string(obs.sat) + " ";
+#endif
+ // overload SBAS3 and SBAS4 first chance
+ if (eph_sbas34)
+ copy_opt.sateph = EPHOPT_SBAS;
+ if (iono_sbas34)
+ copy_opt.ionoopt = IONOOPT_SBAS;
+
+ // compute satellites positions
+ inc_snapshot ->computeSatellites(copy_opt.sateph);
+
+ /* this fix is used for:
+ * - set ENU
+ * - compute azimuths and elevations
+ * - Take the eventual discarded sats by RAIM
+ * - initialize clock bias
+ **/
+ fix_incoming_ = GnssUtils::computePos(*inc_snapshot->getObservations(),
+ *inc_snapshot->getNavigation(),
+ params_tracker_gnss_->fix_opt);
+
+ // Initialize clock bias stateblocks in capture
+ if (fix_incoming_.success)
+ {
+ incoming_ptr_->getStateBlock('T') ->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(0)));
+ if (sensor_gnss_->isStateBlockDynamic('G'))
+ incoming_ptr_->getStateBlock('G')->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(1)));
+ if (sensor_gnss_->isStateBlockDynamic('E'))
+ incoming_ptr_->getStateBlock('E')->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(2)));
+ if (sensor_gnss_->isStateBlockDynamic('M'))
+ incoming_ptr_->getStateBlock('M')->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(3)));
+
+ if (first_pos_.squaredNorm() < 1e-6)
+ first_pos_ = fix_incoming_.pos;
+ }
+
+ // Set ECEF-ENU
+ if (!sensor_gnss_->isEnuDefined() and sensor_gnss_->isEnuModeAuto() )
+ {
+ if (fix_incoming_.success)
+ {
+ WOLF_DEBUG("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
+ sensor_gnss_->setEcefEnu(fix_incoming_.pos, true);
+ }
+ else
+ WOLF_WARN("Failed to compute fix to set ECEF-ENU in auto mode. Message: ", fix_incoming_.msg);
+ }
+ // Fix ENU-MAP
+ if ((fix_incoming_.pos - first_pos_).norm() > params_tracker_gnss_->enu_map_fix_dist)
+ {
+ sensor_gnss_->getEnuMapTranslation()->fix();
+ sensor_gnss_->getEnuMapRoll()->fix();
+ sensor_gnss_->getEnuMapPitch()->fix();
+ sensor_gnss_->getEnuMapYaw()->fix();
+ }
+
+ WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose());
+
+ // store initial observation before filtering
+ GnssUtils::ObservationsPtr init_obs;
+ if (eph_sbas34 or iono_sbas34)
+ init_obs = std::make_shared<GnssUtils::Observations>(*inc_snapshot->getObservations());
+
+ // filter observations (available ephemeris, constellations and elevation&SNR)
+ #ifdef _WOLF_DEBUG
+ auto discarded_gnssutils =
+ #endif
+ inc_snapshot->filterObservations(fix_incoming_.discarded_sats, // discarded sats
+ fix_incoming_.sat_azel,
+ false, // check code
+ false, // check carrier phase
+ copy_opt);
+
+ // compute corrected Ranges
+ inc_snapshot->computeRanges(fix_incoming_.sat_azel,
+ fix_incoming_.lat_lon,
+ copy_opt);
+
+ /* NOT ENOUGH SATS/RANGES in case of:
+ * eph: EPHOPT_SBAS3 or EPHOPT_SBAS4
+ * iono: IONOOPT_SBAS3 or IONOOPT_SBAS4
+ */
+ if (inc_snapshot->getRanges().size() < params_tracker_gnss_->min_sbas_sats and
+ (eph_sbas34 or iono_sbas34))
+ {
+ WOLF_DEBUG("ProcessorTrackerGnss::preProcess: with SBAS3/SBAS4, not enough ranges: ", inc_snapshot->getObservations()->size(), " Computing with SBAS2/BRDC");
+
+ if (params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS3)
+ copy_opt.sateph = EPHOPT_SBAS2;
+ if (params_tracker_gnss_->gnss_opt.sateph == EPHOPT_SBAS4)
+ copy_opt.sateph = EPHOPT_BRDC;
+
+ if (params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS3)
+ copy_opt.ionoopt = IONOOPT_SBAS2;
+ if (params_tracker_gnss_->gnss_opt.ionoopt == IONOOPT_SBAS4)
+ copy_opt.ionoopt = IONOOPT_BRDC;
+
+ // reset observations
+ inc_snapshot->setObservations(init_obs);
+ inc_snapshot->getRanges().clear();
+ // recompute satellites
+ inc_snapshot ->computeSatellites(copy_opt.sateph);
+ // filter
+ #ifdef _WOLF_DEBUG
+ discarded_gnssutils =
+ #endif
+ inc_snapshot->filterObservations(fix_incoming_.discarded_sats, // discarded sats
+ fix_incoming_.sat_azel,
+ false, // check code
+ false, // check carrier phase
+ copy_opt);
+
+
+ // recompute corrected Ranges
+ inc_snapshot->computeRanges(fix_incoming_.sat_azel,
+ fix_incoming_.lat_lon,
+ copy_opt);
+
+ WOLF_DEBUG("ProcessorTrackerGnss::preProcess: with SBAS2/BRDC, obtained ranges: ", inc_snapshot->getRanges().size());
+ }
+
+ // create features pseudorange
+ for (auto obs_sat : inc_snapshot->getObservations()->getObservations())
+ {
+ if(inc_snapshot->getSatellites().count(obs_sat.sat) == 0)
+ {
+ WOLF_DEBUG("ProcessorTrackerGnss::preProcess(): Creating features of obs_sat ", obs_sat.sat , " satellite not found");
+ continue;
+ }
+ if (inc_snapshot->getRanges().count(obs_sat.sat) == 0)
+ {
+ WOLF_DEBUG("ProcessorTrackerGnss::preProcess(): Creating features of obs_sat ", obs_sat.sat , " pseudorange not found");
+ continue;
+ }
+
+ auto feat = FeatureBase::emplace<FeatureGnssSatellite>(incoming_ptr_, obs_sat, inc_snapshot->getSatellites().at(obs_sat.sat), inc_snapshot->getRanges().at(obs_sat.sat));
+ assert(untracked_incoming_features_.count(feat->satNumber()) == 0);
+ untracked_incoming_features_[feat->satNumber()] = feat;
+ }
+
+#ifdef _WOLF_DEBUG
+ std::string used_rtklib_str, discarded_rtklib_str, detected_str, discarded_gnssutils_str;
+ for (auto sat : fix_incoming_.used_sats)
+ used_rtklib_str += std::to_string(sat) + " ";
+ for (auto sat : fix_incoming_.discarded_sats)
+ discarded_rtklib_str += std::to_string(sat) + " ";
+ for (auto ftr_pair : untracked_incoming_features_)
+ detected_str += std::to_string(ftr_pair.first) + " ";
+ for (auto sat_disc : discarded_gnssutils)
+ if (fix_incoming_.discarded_sats.count(sat_disc) == 0)
+ discarded_gnssutils_str += std::to_string(sat_disc) + " ";
+#endif
+
+ WOLF_DEBUG("ProcessorTrackerGnss::preProcess()",
+ "\n\tinitial observations: ", n_initial, " (", initial_str, ")",
+ "\n\tRTKLIB used: ", fix_incoming_.used_sats.size(), " (", used_rtklib_str, ")",
+ //"\n\tRTKLIB discarded: ", fix_incoming_.discarded_sats.size(), " (", discarded_rtklib_str, ")",
+ "\n\tgnssutils discarded: ", n_initial - untracked_incoming_features_.size() - fix_incoming_.discarded_sats.size(), " (", discarded_gnssutils_str, ")",
+ "\n\tdetected incoming features: ", untracked_incoming_features_.size(), " (", detected_str, ")");
+}
+
+unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences)
+{
+ if (_features_in.empty())
+ return 0;
+
+ WOLF_DEBUG_COND(_features_out.begin() == new_features_incoming_.begin(), "ProcessorTrackerGnss::trackFeatures tracking " , _features_in.size() , " new features...");
+ WOLF_DEBUG_COND(_features_out.begin() == known_features_incoming_.begin(), "ProcessorTrackerGnss::trackFeatures tracking " , _features_in.size() , " known features...");
+
+ assert(_capture == incoming_ptr_);
+
+ int common_sats = 0;
+
+ for (auto feat_in : _features_in)
+ {
+ auto feat_in_gnss = std::dynamic_pointer_cast<FeatureGnssSatellite>(feat_in);
+ assert(feat_in_gnss);
+
+ int sat_num = std::static_pointer_cast<FeatureGnssSatellite>(feat_in)->satNumber();
+
+ WOLF_DEBUG("tracking " , feat_in->trackId() , ", sat number ", sat_num);
+
+ if (untracked_incoming_features_.count(sat_num) != 0)
+ common_sats++;
+
+ if (untracked_incoming_features_.count(sat_num) != 0 and
+ std::abs(untracked_incoming_features_.at(sat_num)->getObservation().L[0]) > 1e-12) // Track only carrier phase valid
+ {
+ auto ftr = untracked_incoming_features_[sat_num];
+ untracked_incoming_features_.erase(sat_num);
+ _features_out.push_back(ftr);
+ _feature_correspondences[ftr] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in,0}));
+
+ WOLF_DEBUG("\ttracked: " , feat_in->trackId() , " last: " , feat_in->id() , " inc: " , ftr->id() , " !" );
+ }
+ else
+ {
+ WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 0, "\tnot tracked, missing satellite" );
+ WOLF_DEBUG_COND(untracked_incoming_features_.count(sat_num) == 1, "\tnot tracked, wrong carrier phase value" );
+ }
+ }
+ WOLF_WARN_COND(_features_out.empty() and
+ _features_out.begin() == known_features_incoming_.begin(),
+ "ProcessorTrackerGnss::trackFeatures: LOST TRACK OF ALL SATELLITES of ", _features_in.size(),
+ " - ", common_sats, " due to wrong Carrier Phase data.");
+ WOLF_DEBUG_COND(_features_out.begin() == new_features_incoming_.begin(),
+ "ProcessorTrackerGnss::trackFeatures: tracked " ,
+ _features_out.size(), " new features (of ", _features_in.size(), ")");
+ WOLF_DEBUG_COND(_features_out.begin() == known_features_incoming_.begin(),
+ "ProcessorTrackerGnss::trackFeatures: tracked " ,
+ _features_out.size(), " known features (of ", _features_in.size(), ")");
+
+ return _features_out.size();
+}
+
+bool ProcessorTrackerGnss::voteForKeyFrame() const
+{
+ //WOLF_DEBUG("ProcessorTrackerGnss::voteForKeyFrame");
+
+ // too old origin
+ if (origin_ptr_== nullptr or (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > params_tracker_gnss_->max_time_span )
+ {
+ WOLF_DEBUG( "Vote for KF because of time span or null origin" );
+ return true;
+ }
+
+ /* KNOWN FEATURES
+ * If tracked sats are lower than minimum (min_features_for_keyframe), a KF in last will be useful
+ * if it allows to add more satellites (untracked features in last is not empty)
+ */
+ WOLF_DEBUG("Nbr. of active feature tracks: " , known_features_incoming_.size() );
+ if (known_features_incoming_.size() < params_tracker_feature_->min_features_for_keyframe
+ and not untracked_last_features_.empty() )
+ {
+ WOLF_DEBUG( "Vote for KF because of too less known_features_incoming and not empty untracked in last" );
+ return true;
+ }
+
+ // TODO: more alternatives?
+
+ // otherwise
+ return false;
+}
+
+unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
+{
+ WOLF_DEBUG("ProcessorTrackerGnss::detectNewFeatures");
+
+ assert(_capture == last_ptr_);
+
+ for (auto feat_pair : untracked_last_features_)
+ {
+ if (_features_out.size() == _max_new_features)
+ break;
+
+ // discard non-valid carrier phase
+ if (std::abs(std::static_pointer_cast<FeatureGnssSatellite>(feat_pair.second)->getObservation().L[0]) < 1e-12)
+ continue;
+
+ _features_out.push_back(feat_pair.second);
+ WOLF_DEBUG("feature " , feat_pair.second->id() , " detected!" );
+ }
+ WOLF_DEBUG(_features_out.size() , " new features detected!");
+
+ return _features_out.size();
+}
+
+void ProcessorTrackerGnss::establishFactors()
+{
+ WOLF_DEBUG("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_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
+
+ FactorBasePtrList new_factors;
+
+ // FIX FACTOR
+ if (params_tracker_gnss_->fix)
+ {
+ GnssUtils::SnapshotPtr last_snapshot = std::static_pointer_cast<CaptureGnss>(last_ptr_)->getSnapshot();
+
+
+ auto fix = GnssUtils::computePos(*last_snapshot->getObservations(),
+ *last_snapshot->getNavigation(),
+ params_tracker_gnss_->gnss_opt);
+ if (fix.success)
+ {
+ FeatureBasePtr ftr = FeatureBase::emplace<FeatureGnssFix>(last_ptr_, fix);
+
+ FactorBase::emplace<FactorGnssFix3d>(ftr,
+ ftr,
+ sensor_gnss_,
+ shared_from_this(),
+ params_tracker_gnss_->apply_loss_function,
+ FAC_ACTIVE);
+ }
+ }
+
+ // PSEUDO RANGE FACTORS (all sats)
+ if (params_tracker_gnss_->pseudo_ranges)
+ for (auto ftr : last_ptr_->getFeatureList())
+ {
+ auto ftr_sat = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr);
+ if (ftr_sat == nullptr)
+ continue;
+
+ // Check valid measurement
+ if (std::abs(ftr_sat->getMeasurement()(0)) < 1e-12)
+ {
+ WOLF_WARN("Ignoring feature with not valid pseudorange: ", ftr_sat->getMeasurement()(0));
+ continue;
+ }
+
+ // unfix clock bias
+ last_ptr_->getStateBlock('T')->unfix();
+ // unfix clock bias inter-constellation (if observed)
+ if (ftr_sat->getSatellite().sys == SYS_GLO)
+ last_ptr_->getStateBlock('G')->unfix();
+ if (ftr_sat->getSatellite().sys == SYS_GAL)
+ last_ptr_->getStateBlock('E')->unfix();
+ if (ftr_sat->getSatellite().sys == SYS_CMP)
+ last_ptr_->getStateBlock('M')->unfix();
+
+ // Emplace a pseudo range factor
+ auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
+ ftr_sat,
+ sensor_gnss_,
+ shared_from_this(),
+ params_->apply_loss_function);
+ new_factors.push_back(new_fac);
+
+ WOLF_DEBUG("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
+ }
+
+ // TDCP FACTORS (tracked sats)
+ if (origin_ptr_ != last_ptr_ and params_tracker_gnss_->tdcp_enabled)
+ {
+ // Displacement factor from batch TDCP (FactorGnssTdcp3d)
+ if (params_tracker_gnss_->tdcp_params.batch)
+ {
+ WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
+ FactorBasePtr last_fac_ptr = nullptr;
+
+ for (auto KF_rit = getProblem()->getTrajectory()->rbegin();
+ KF_rit != getProblem()->getTrajectory()->rend();
+ KF_rit++)
+ {
+ FrameBasePtr KF = (*KF_rit);
+
+ WOLF_DEBUG("TDCP BATCH ref frame ", KF->id());
+
+ // discard non-key frames, last-last pair and frames without CaptureGnss
+ if (KF == last_ptr_->getFrame() or
+ KF->getCaptureOf(getSensor(),"CaptureGnss") == nullptr)
+ continue;
+
+ // static cast
+ auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>((*KF_rit)->getCaptureOf(getSensor(),"CaptureGnss"));
+ auto last_cap_gnss = std::static_pointer_cast<CaptureGnss>(last_ptr_);
+
+ // dt
+ double dt = last_ptr_->getTimeStamp() - ref_cap_gnss->getTimeStamp();
+ WOLF_DEBUG("TDCP BATCH dt = ", dt);
+
+ // discard strange cases
+ if (dt <= 0)
+ continue;
+
+ // within time window
+ if (dt > params_tracker_gnss_->tdcp_params.time_window)
+ break;
+
+ // discard removing Frame/capture/feature
+ if (ref_cap_gnss->isRemoving() or ref_cap_gnss->getFrame()->isRemoving())
+ continue;
+
+ // get common sats from tracking
+ auto matches = track_matrix_.matches(ref_cap_gnss, last_cap_gnss);
+ WOLF_DEBUG("TDCP BATCH matches ", matches.size());
+ std::set<int> common_sats;
+ for (auto match : matches)
+ {
+ assert(std::dynamic_pointer_cast<FeatureGnssSatellite>(match.second.first)->satNumber());
+ assert(std::dynamic_pointer_cast<FeatureGnssSatellite>(match.second.second)->satNumber());
+ assert(std::static_pointer_cast<FeatureGnssSatellite>(match.second.first)->satNumber() ==
+ std::static_pointer_cast<FeatureGnssSatellite>(match.second.second)->satNumber());
+ common_sats.insert(std::static_pointer_cast<FeatureGnssSatellite>(match.second.first)->satNumber());
+ }
+ WOLF_DEBUG("TDCP BATCH common_sats: ", common_sats.size());
+ //for (auto sat : common_sats)
+ // std::cout << sat << " ";
+ //std::cout << std::endl;
+
+ // DEBUG: FIND COMMON SATELLITES OBSERVATIONS
+ //std::set<int> common_sats_debug = GnssUtils::Observations::findCommonObservations(*ref_cap_gnss->getSnapshot()->getObservations(),
+ // *last_cap_gnss->getSnapshot()->getObservations());
+ //WOLF_DEBUG("TDCP BATCH common_sats_debug: ", common_sats_debug.size());
+ //for (auto sat : common_sats_debug)
+ // std::cout << sat << " ";
+ //std::cout << std::endl;
+
+ // reference ECEF position
+ Eigen::Vector3d x_r = sensor_gnss_->computeFrameAntennaPosEcef(KF);
+
+ // compute TDCP batch
+ auto tdcp_output = GnssUtils::Tdcp(ref_cap_gnss->getSnapshot(),
+ last_cap_gnss->getSnapshot(),
+ x_r,
+ common_sats,
+ Eigen::Vector4d::Zero(),
+ params_tracker_gnss_->tdcp_params);
+ if (tdcp_output.success)
+ {
+ WOLF_DEBUG("TDCP BATCH d = ", tdcp_output.d.transpose());
+ WOLF_DEBUG("TDCP BATCH cov =\n", tdcp_output.cov_d);
+
+ // EMPLACE FEATURE
+ auto factor_status = FAC_ACTIVE;
+ if (params_tracker_gnss_->tdcp_structure == "first-all")
+ factor_status = FAC_INACTIVE;
+
+ auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss,
+ tdcp_output.d,
+ tdcp_output.cov_d);
+
+ // EMPLACE FACTOR
+ last_fac_ptr = FactorBase::emplace<FactorGnssTdcpBatch>(ftr,
+ ftr,
+ ref_cap_gnss,
+ sensor_gnss_,
+ shared_from_this(),
+ params_->apply_loss_function,
+ factor_status);
+
+ WOLF_DEBUG("TDCP BATCH factor ", last_fac_ptr->id() , " emplaced as ", (last_fac_ptr->getStatus() == FAC_ACTIVE ? "ACTIVE" : "INACTIVE"));
+
+ // only first (with previous frame) factor in "consecutive" structure
+ if (params_tracker_gnss_->tdcp_structure == "consecutive")
+ break;
+ }
+ else
+ {
+ WOLF_DEBUG("TDCP BATCH failed with msg: ", tdcp_output.msg);
+ }
+ }
+ // only last (with first frame) factor in "first-all" structure
+ if (params_tracker_gnss_->tdcp_structure == "first-all" and last_fac_ptr)
+ {
+ last_fac_ptr->setStatus(FAC_ACTIVE);
+ WOLF_DEBUG("TDCP BATCH factor ", last_fac_ptr->id() , " changed to ", (last_fac_ptr->getStatus() == FAC_ACTIVE ? "ACTIVE" : "INACTIVE"));
+ }
+ }
+ // FACTOR per SATELLITE (FactorGnssTdcp)
+ else
+ {
+ // iterate over tracked features of last
+ for (auto ftr_k_pair : track_matrix_.snapshot(last_ptr_))
+ {
+ // current feature
+ auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
+ assert(ftr_k != nullptr);
+
+ // check valid measurement
+ assert(std::abs(ftr_k->getObservation().L[0]) > 1e-12);
+
+ WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
+
+ // unfix clock bias
+ last_ptr_->getStateBlock('T')->unfix();
+
+ // emplace a tdcp factor from last to each KF
+ auto ts_ftr_r_map = track_matrix_.trackAtKeyframes(ftr_k_pair.first);
+ for (auto ts_ftr_r_it = ts_ftr_r_map.rbegin(); ts_ftr_r_it != ts_ftr_r_map.rend(); ts_ftr_r_it++)
+ {
+ // cast reference feature
+ auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ts_ftr_r_it->second);
+ assert(ftr_r != nullptr);
+
+ // dt
+ double dt = ftr_k->getCapture()->getTimeStamp() - ts_ftr_r_it->first;
+
+ // discard incomming-last and last-last
+ if (dt < 0 or ftr_k == ftr_r)
+ continue;
+
+ // within time window
+ if (dt > params_tracker_gnss_->tdcp_params.time_window)
+ break;
+
+ // discard removing Frame/capture/feature
+ if (ftr_r->isRemoving() or ftr_r->getCapture()->isRemoving() or ftr_r->getCapture()->getFrame()->isRemoving())
+ continue;
+
+ // check valid measurement
+ assert(std::abs(ftr_r->getObservation().L[0]) > 1e-12);
+
+ WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
+
+ // emplace tdcp factor
+ auto factor_status = FAC_ACTIVE;
+ if (params_tracker_gnss_->tdcp_structure == "first-all")
+ factor_status = FAC_INACTIVE;
+ double var_tdcp = dt * std::pow(params_tracker_gnss_->tdcp_params.sigma_atm,2) + std::pow(params_tracker_gnss_->tdcp_params.sigma_carrier,2);
+ auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
+ sqrt(var_tdcp),
+ ftr_r,
+ ftr_k,
+ sensor_gnss_,
+ shared_from_this(),
+ params_tracker_gnss_->tdcp_params.loss_function,
+ factor_status);
+ new_factors.push_back(new_fac);
+
+ // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
+ // " origin: " , feature_in_origin->id() ,
+ // " from last: " , feature_in_last->id() );
+
+ // only first (with previous frame) factor in "consecutive" structure
+ if (params_tracker_gnss_->tdcp_structure == "consecutive")
+ break;
+ }
+ // only last (with first frame) factor in "first-all" structure
+ if (params_tracker_gnss_->tdcp_structure == "first-all")
+ new_factors.back()->setStatus(FAC_ACTIVE);
+
+ WOLF_DEBUG("All TDCP factors emplaced!");
+ }
+ }
+ }
+
+ // remove outliers
+ if (!new_factors.empty() and (params_tracker_gnss_->remove_outliers or params_tracker_gnss_->remove_outliers_tdcp))
+ removeOutliers(new_factors, last_ptr_);
+
+ //getProblem()->print(4, 1, 1, 1);
+
+ WOLF_DEBUG("ProcessorTrackerGnss::establishFactors done");
+}
+
+void ProcessorTrackerGnss::advanceDerived()
+{
+ ProcessorTrackerFeature::advanceDerived();
+
+ untracked_last_features_ = std::move(untracked_incoming_features_);
+ fix_last_ = fix_incoming_;
+}
+
+void ProcessorTrackerGnss::resetDerived()
+{
+ ProcessorTrackerFeature::resetDerived();
+
+ untracked_last_features_ = std::move(untracked_incoming_features_);
+ fix_last_ = fix_incoming_;
+}
+
+void ProcessorTrackerGnss::postProcess()
+{
+ // Fix orientation of last frame if no other processors
+ if (getSensor()->getHardware()->getSensorList().size() == 1)
+ getProblem()->getLastFrame()->getO()->fix();
+}
+
+void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap)
+{
+ WOLF_DEBUG("ProcessorTrackerGnss::removeOutliers");
+
+ FactorBasePtrList remove_fac;
+
+ //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
+
+ // PseudoRange states
+ Eigen::Vector3d x;
+ Eigen::Vector4d o;
+ Eigen::Vector1d clock_bias;
+ Eigen::Vector1d clock_bias_glo;
+ Eigen::Vector1d clock_bias_gal;
+ Eigen::Vector1d clock_bias_cmp;
+ Eigen::Vector3d x_antena_pr;
+
+
+ // state for pseudoranges is fix solution if OK
+ if (cap == last_ptr_ and fix_last_.stat != 0 and params_tracker_gnss_->remove_outliers_with_fix)
+ {
+ WOLF_DEBUG("OUTLIERS COMPUTED USING fix_last");
+ x = sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap());
+ o << 0,0,0,1;
+ clock_bias << CLIGHT * fix_last_.rcv_bias(0);
+ clock_bias_glo << CLIGHT * fix_last_.rcv_bias(1);
+ clock_bias_gal << CLIGHT * fix_last_.rcv_bias(2);
+ clock_bias_cmp << CLIGHT * fix_last_.rcv_bias(3);
+ x_antena_pr = Eigen::Vector3d::Zero();
+
+ //std::cout << "x: " << x.transpose() << std::endl;
+ //std::cout << "o: " << o.transpose() << std::endl;
+ //std::cout << "clock_bias: " << clock_bias << std::endl;
+ //std::cout << "clock_bias_glo: " << clock_bias_glo << std::endl;
+ //std::cout << "clock_bias_gal: " << clock_bias_gal << std::endl;
+ //std::cout << "clock_bias_cmp: " << clock_bias_cmp << std::endl;
+ //std::cout << "x_antena_pr: " << x_antena_pr.transpose() << std::endl;
+ //std::cout << "frame p: " << cap->getFrame()->getP()->getState().transpose() << std::endl;
+ //std::cout << "frame o: " << cap->getFrame()->getO()->getState().transpose() << std::endl;
+ //std::cout << "sb clock: " << cap->getStateBlock('T')->getState() << std::endl;
+ //std::cout << "sb clock glo: " << cap->getStateBlock('G')->getState() << std::endl;
+ //std::cout << "sb clock gal: " << cap->getStateBlock('E')->getState() << std::endl;
+ //std::cout << "sb clock cmp: " << cap->getStateBlock('M')->getState() << std::endl;
+ //std::cout << "sb antena: " << sensor_gnss_->getP()->getState().transpose() << std::endl;
+ }
+ else
+ {
+ x = cap->getFrame()->getP()->getState();
+ o = cap->getFrame()->getO()->getState();
+ clock_bias = cap->getStateBlock('T')->getState();
+ clock_bias_glo = cap->getStateBlock('G')->getState();
+ clock_bias_gal = cap->getStateBlock('E')->getState();
+ clock_bias_cmp = cap->getStateBlock('M')->getState();
+ x_antena_pr = sensor_gnss_->getP()->getState();
+ }
+
+ // TDCP states
+ Eigen::Vector3d x_k(cap->getFrame()->getP()->getState());
+ Eigen::Vector4d o_k(cap->getFrame()->getO()->getState());
+ Eigen::Vector3d x_r(cap->getFrame()->getP()->getState());
+ Eigen::Vector4d o_r(cap->getFrame()->getO()->getState());
+ Eigen::Vector1d clock_bias_k(cap->getStateBlock('T')->getState());
+ Eigen::Vector1d clock_bias_r(cap->getStateBlock('T')->getState());
+ Eigen::Vector3d x_antena(sensor_gnss_->getP()->getState());
+
+ // Common states
+ Eigen::Vector3d t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
+ Eigen::Vector1d roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
+ Eigen::Vector1d pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
+ Eigen::Vector1d yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
+
+ //std::cout << "t_ENU_map: " << t_ENU_map.transpose() << std::endl;
+ //std::cout << "roll_ENU_map: " << roll_ENU_map << std::endl;
+ //std::cout << "pitch_ENU_map: " << pitch_ENU_map << std::endl;
+ //std::cout << "yaw_ENU_map: " << yaw_ENU_map << std::endl;
+
+ // create double* array of a copy of the state for pseudo range factors
+ double* parameters_glo_pr[9] = {x.data(),
+ o.data(),
+ clock_bias.data(),
+ clock_bias_glo.data(),
+ x_antena_pr.data(),
+ t_ENU_map.data(),
+ roll_ENU_map.data(),
+ pitch_ENU_map.data(),
+ yaw_ENU_map.data()};
+ double* parameters_gal_pr[9] = {x.data(),
+ o.data(),
+ clock_bias.data(),
+ clock_bias_gal.data(),
+ x_antena_pr.data(),
+ t_ENU_map.data(),
+ roll_ENU_map.data(),
+ pitch_ENU_map.data(),
+ yaw_ENU_map.data()};
+ double* parameters_cmp_pr[9] = {x.data(),
+ o.data(),
+ clock_bias.data(),
+ clock_bias_cmp.data(),
+ x_antena_pr.data(),
+ t_ENU_map.data(),
+ roll_ENU_map.data(),
+ pitch_ENU_map.data(),
+ yaw_ENU_map.data()};
+ double* parameters_tdcp[11] = {x_r.data(),
+ o_r.data(),
+ clock_bias_r.data(),
+ x_k.data(),
+ o_k.data(),
+ clock_bias_k.data(),
+ x_antena.data(),
+ t_ENU_map.data(),
+ roll_ENU_map.data(),
+ pitch_ENU_map.data(),
+ yaw_ENU_map.data()};
+
+ // create residuals
+ double residual;
+
+ for (auto fac : fac_list)
+ {
+ // PSEUDO RANGE FACTORS
+ auto fac_pr = std::dynamic_pointer_cast<FactorGnssPseudoRange>(fac);
+ auto ftr_sat = std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature());
+ int sat = ftr_sat->satNumber();
+
+ if (fac_pr)
+ {
+ // evaluate the factor in this state
+ switch (ftr_sat->getSatellite().sys)
+ {
+ case SYS_GLO:
+ fac_pr->evaluate(parameters_glo_pr, &residual, nullptr);
+ break;
+ case SYS_GAL:
+ fac_pr->evaluate(parameters_gal_pr, &residual, nullptr);
+ break;
+ case SYS_CMP:
+ fac_pr->evaluate(parameters_cmp_pr, &residual, nullptr);
+ break;
+ default:
+ fac_pr->evaluate(parameters_glo_pr, &residual, nullptr);
+ break;
+ }
+
+ // RTKLIB FIX error
+ //int sat = std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature())->getSatellite().sat;
+ //assert(fix_last_.prange_residuals.count(sat) && "sat not used when computing fix!");
+ //WOLF_DEBUG("FactorGnssPseudoRange error = ", fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
+ //WOLF_DEBUG("RTKLIB pntpos error = ", fix_last_.prange_residuals.at(sat));
+
+ // discard if residual too high evaluated at the current estimation
+ if (std::abs(residual) > params_tracker_gnss_->outlier_residual_th)
+ {
+ WOLF_WARN("Discarding FactorGnssPseudoRange, considered OUTLIER");
+ WOLF_TRACE("Feature: ", fac->getMeasurement(),"\nError: ",fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
+ remove_fac.push_back(fac_pr);
+ // store for statistics
+ outliers_pseudorange_++;
+ if (not sat_outliers_.count(sat))
+ sat_outliers_.emplace(sat, 1);
+ else
+ sat_outliers_[sat]++;
+
+ }
+ else
+ inliers_pseudorange_++;
+ }
+ // TDCP FACTORS
+ auto fac_tdcp = std::dynamic_pointer_cast<FactorGnssTdcp>(fac);
+ if (fac_tdcp and params_tracker_gnss_->remove_outliers_tdcp)
+ {
+ // update ref frame
+ x_r = fac_tdcp->getCaptureOther()->getFrame()->getP()->getState();
+ o_r = fac_tdcp->getCaptureOther()->getFrame()->getO()->getState();
+ clock_bias_r = fac_tdcp->getCaptureOther()->getStateBlock('T')->getState();
+ parameters_tdcp[0] = x_r.data();
+ parameters_tdcp[1] = o_r.data();
+ parameters_tdcp[2] = clock_bias_r.data();
+
+ // evaluate
+ fac_tdcp->evaluate(parameters_tdcp, &residual, nullptr);
+
+ //WOLF_DEBUG("FactorGnssTdcp with residual = ", residual);
+
+ // discard if residual too high evaluated at the current estimation
+ if (std::abs(residual) > params_tracker_gnss_->outlier_residual_th)
+ {
+ WOLF_WARN("Discarding FactorGnssTdcp, considered OUTLIER");
+ WOLF_TRACE("Residual: ", residual,"\nError: ",fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
+ remove_fac.push_back(fac_tdcp);
+ // store for statistics
+ outliers_tdcp_++;
+ if (not sat_outliers_.count(sat))
+ sat_outliers_.emplace(sat, 1);
+ else
+ sat_outliers_[sat]++;
+ }
+ else
+ inliers_tdcp_++;
+ }
+ }
+
+ // remove outliers
+ for (auto fac : remove_fac)
+ {
+ //assert(false);
+ fac->remove();
+ }
+ WOLF_INFO_COND(not remove_fac.empty(),
+ "ProcessorTrackerGnss::removeOutliers:",
+ "\n\tPseudorange: ", outliers_pseudorange_,
+ "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)");
+ if (not remove_fac.empty() and
+ params_tracker_gnss_->tdcp_enabled and
+ params_tracker_gnss_->remove_outliers_tdcp)
+ std::cout << "\tTDCP: "
+ << outliers_tdcp_
+ << "\t( "
+ << (100.0 * outliers_tdcp_)/(outliers_tdcp_+inliers_tdcp_)
+ << " \%)\n";
+ //std::cout << "\tsats:";
+ //for (auto sat_out : sat_outliers_)
+ //{
+ // const int& sat = sat_out.first;
+ // int sys = satsys(sat,NULL);
+ // const unsigned int& outliers = sat_out.second;
+ // std::cout << "\t\tsat " << sat << "(" << sys << "): " << outliers << std::endl;
+ //}
+}
+
+} // namespace wolf
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorTrackerGnss)
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorTrackerGnss)
+} // namespace wolf
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 5beaf5097..6e8d73c39 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -1,59 +1,89 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
#include "gnss/sensor/sensor_gnss.h"
#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
+#include "core/state_block/state_angle.h"
+#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/utils/transformations.h"
namespace wolf {
-// Geodetic system parameters
-static Scalar kSemimajorAxis = 6378137;
-static Scalar kSemiminorAxis = 6356752.3142;
-static Scalar kFirstEccentricitySquared = 6.69437999014 * 0.001;
-static Scalar kSecondEccentricitySquared = 6.73949674228 * 0.001;
-//static Scalar kFlattening = 1 / 298.257223563;
-
-SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D position with respect to the car frame (base frame)
- StateBlockPtr _bias_ptr, //GNSS sensor time bias
- IntrinsicsGnssPtr params) //sensor params
+SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
+ const ParamsSensorGnssPtr& _params)
:
- SensorBase("SensorGnss", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS
- params_(params),
+ SensorBase("SensorGnss",
+ std::make_shared<StateBlock>(_extrinsics,_params->extrinsics_fixed),
+ nullptr, // antena orientation has no sense in GNSS
+ nullptr, // SensorParams are set afterwards
+ 0),
+ params_(_params),
ENU_defined_(false),
- ENU_MAP_initialized_(false)
+ t_ENU_MAP_initialized_(false),
+ R_ENU_MAP_initialized_(false),
+ R_ENU_ECEF_(Eigen::Matrix3d::Identity()),
+ t_ENU_ECEF_(Eigen::Vector3d::Zero())
{
- assert(_p_ptr->getSize() == 3 && "Bad extrinsics size");
- assert((_bias_ptr == nullptr || _bias_ptr->getSize() == 1 )&& "Bad bias size");
-
- getStateBlockVec().resize(7);
- setStateBlockPtrStatic(3, std::make_shared<StateBlock>(3, params_->translation_fixed_));
- setStateBlockPtrStatic(4, std::make_shared<StateBlock>(1, params_->roll_fixed_));
- setStateBlockPtrStatic(5, std::make_shared<StateBlock>(1, params_->pitch_fixed_));
- setStateBlockPtrStatic(6, std::make_shared<StateBlock>(1, params_->yaw_fixed_));
-}
-
-SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position with respect to the car frame (base frame)
- StateBlockPtr _bias_ptr, //GNSS sensor time bias
- IntrinsicsGnssPtr params, //sensor params
- StateBlockPtr _t_ENU_MAP_ptr, //ENU_MAP translation
- StateBlockPtr _roll_ENU_MAP_ptr, //ENU_MAP Roll
- StateBlockPtr _pitch_ENU_MAP_ptr, //ENU_MAP Pitch
- StateBlockPtr _yaw_ENU_MAP_ptr) //ENU_MAP Yaw
- :
- SensorBase("SensorGnss", _p_ptr, nullptr, _bias_ptr, 0),// antena orientation has no sense in GNSS
- params_(params),
- ENU_defined_(false),
- ENU_MAP_initialized_(false)
-{
- assert(_p_ptr->getSize() == 3 && "Bad extrinsics size");
- assert((_bias_ptr == nullptr || _bias_ptr->getSize() == 1 )&& "Bad bias size");
- assert(_roll_ENU_MAP_ptr->getSize() == 1 && "Bad ENU_MAP roll size");
- assert(_pitch_ENU_MAP_ptr->getSize() == 1 && "Bad ENU_MAP pitch size");
- assert(_yaw_ENU_MAP_ptr->getSize() == 1 && "Bad ENU_MAP yaw size");
-
- getStateBlockVec().resize(7);
- setStateBlockPtrStatic(3, _t_ENU_MAP_ptr);
- setStateBlockPtrStatic(4, _roll_ENU_MAP_ptr);
- setStateBlockPtrStatic(5, _pitch_ENU_MAP_ptr);
- setStateBlockPtrStatic(6, _yaw_ENU_MAP_ptr);
+ assert(_extrinsics.size() == 3 && "Bad extrinsics size");
+
+ // STATE BLOCKS
+ // ENU-MAP
+ addStateBlock('t', std::make_shared<StateBlock>(3, params_->translation_fixed), false);
+ addStateBlock('r', std::make_shared<StateAngle>(0.0, params_->roll_fixed), false);
+ addStateBlock('p', std::make_shared<StateAngle>(0.0, params_->pitch_fixed), false);
+ addStateBlock('y', std::make_shared<StateAngle>(0.0, params_->yaw_fixed), false);
+ // clock bias
+ addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateBlock>(1,false), true); // receiver clock bias
+ addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
+ addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
+ addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
+
+ // ENU-ECEF
+ // Mode "manual": ENU provided via params
+ if (params_->ENU_mode == "manual")
+ setEcefEnu(params_->ENU_latlonalt, false);
+ // Mode "ECEF": ENU = ECEF (disabling this coordinates system)
+ if (params_->ENU_mode == "ECEF")
+ setEnuEcef(Eigen::Matrix3d::Identity(),Eigen::Vector3d::Zero());
+
+ // PRIORS
+ // prior to ENU-MAP (avoid non-observable problem)
+ if (params_->ENU_mode != "ECEF")
+ {
+ if (not params_->translation_fixed)
+ addPriorParameter('t', Eigen::Vector3d::Zero(), 10*Eigen::Matrix3d::Identity());
+ if (not params_->roll_fixed)
+ addPriorParameter('r', Eigen::Vector1d::Zero(), M_PI*Eigen::Matrix1d::Identity());
+ if (not params_->pitch_fixed)
+ addPriorParameter('p', Eigen::Vector1d::Zero(), M_PI*Eigen::Matrix1d::Identity());
+ if (not params_->yaw_fixed)
+ addPriorParameter('y', Eigen::Vector1d::Zero(), M_PI*Eigen::Matrix1d::Identity());
+ }
+ // prior to inter-constellation clock bias (avoid non-observable problem)
+ if (not params_->clock_bias_GPS_GLO_dynamic)
+ addPriorParameter(CLOCK_BIAS_GPS_GLO_KEY, Eigen::Vector1d::Zero(), Eigen::Matrix1d::Identity());
+ if (not params_->clock_bias_GPS_GAL_dynamic)
+ addPriorParameter(CLOCK_BIAS_GPS_GAL_KEY, Eigen::Vector1d::Zero(), Eigen::Matrix1d::Identity());
+ if (not params_->clock_bias_GPS_CMP_dynamic)
+ addPriorParameter(CLOCK_BIAS_GPS_CMP_KEY, Eigen::Vector1d::Zero(), Eigen::Matrix1d::Identity());
}
SensorGnss::~SensorGnss()
@@ -61,100 +91,75 @@ SensorGnss::~SensorGnss()
//
}
-void SensorGnss::computeEnuEcef(const Eigen::Vector3s& _t_ECEF_ENU, Eigen::Matrix3s& R_ENU_ECEF, Eigen::Vector3s& t_ENU_ECEF) const
+void SensorGnss::setEcefEnu(const Eigen::Vector3d& _ENU, bool _ECEF_coordinates)
{
- // Convert ECEF coordinates to geodetic coordinates.
- // J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
- // to geodetic coordinates," IEEE Transactions on Aerospace and
- // Electronic Systems, vol. 30, pp. 957-961, 1994.
-
- Scalar r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
- Scalar Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
- Scalar F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
- Scalar G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq;
- Scalar C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
- Scalar S = cbrt(1 + C + sqrt(C * C + 2 * C));
- Scalar P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
- Scalar Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
- Scalar r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
- + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
- - P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * r * r);
- Scalar V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2));
- Scalar Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
-
- Scalar latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
- Scalar longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
-
- Scalar sLat = sin(latitude);
- Scalar cLat = cos(latitude);
- Scalar sLon = sin(longitude);
- Scalar cLon = cos(longitude);
-
- R_ENU_ECEF(0,0) = -sLon;
- R_ENU_ECEF(0,1) = cLon;
- R_ENU_ECEF(0,2) = 0.0;
-
- R_ENU_ECEF(1,0) = -sLat*cLon;
- R_ENU_ECEF(1,1) = -sLat * sLon;
- R_ENU_ECEF(1,2) = cLat;
-
- R_ENU_ECEF(2,0) = cLat * cLon;
- R_ENU_ECEF(2,1) = cLat * sLon;
- R_ENU_ECEF(2,2) = sLat;
-
- t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
-}
+ WOLF_WARN_COND(ENU_defined_,"setEnuEcef: ENU already defined!");
+ assert(!(ENU_defined_ && params_->ENU_mode=="ECEF") && "ENU cannot be redefined if set to ECEF");
+
+ if (_ECEF_coordinates)
+ GnssUtils::computeEnuEcefFromEcef(_ENU, R_ENU_ECEF_, t_ENU_ECEF_);
+ else
+ GnssUtils::computeEnuEcefFromEcef(GnssUtils::latLonAltToEcef(_ENU), R_ENU_ECEF_, t_ENU_ECEF_);
-void SensorGnss::setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU)
-{
- computeEnuEcef(_t_ECEF_ENU, R_ENU_ECEF_, t_ENU_ECEF_);
ENU_defined_ = true;
- WOLF_INFO("SensorGnss: ECEF-ENU initialized.")
+ WOLF_INFO("SensorGnss: ECEF-ENU set.")
}
-void SensorGnss::setEnuEcef(const Eigen::Matrix3s& _R_ENU_ECEF, const Eigen::Vector3s& _t_ENU_ECEF)
+void SensorGnss::setEnuEcef(const Eigen::Matrix3d& _R_ENU_ECEF, const Eigen::Vector3d& _t_ENU_ECEF)
{
+ WOLF_WARN_COND(ENU_defined_,"setEnuEcef: ENU already defined!");
+ assert(!(ENU_defined_ && params_->ENU_mode=="ECEF") && "ENU cannot be redefined if set to ECEF");
+
R_ENU_ECEF_ = _R_ENU_ECEF;
t_ENU_ECEF_ = _t_ENU_ECEF;
ENU_defined_ = true;
}
-void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, const Eigen::Vector3s& _t_ECEF_antenaENU,
- const Eigen::VectorXs& _pose_MAP_frame2, const Eigen::Vector3s& _t_ECEF_antena2)
+void SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, const Eigen::Vector3d& _t_ECEF_antenaENU,
+ const Eigen::VectorXd& _pose_MAP_frame2, const Eigen::Vector3d& _t_ECEF_antena2)
{
- Eigen::Matrix3s R_ENU_ECEF;//, R_ENU2_ECEF;
- Eigen::Vector3s t_ENU_ECEF;//, t_ENU2_ECEF;
- computeEnuEcef(_t_ECEF_antenaENU, R_ENU_ECEF, t_ENU_ECEF);
- //computeENU_ECEF(_t_ECEF_antena2, R_ENU2_ECEF, t_ENU2_ECEF);
+ assert(ENU_defined_ && "initializing ENU-MAP before setting ENU");
// compute fix vector (from 1 to 2) in ENU coordinates
- Eigen::Vector3s v_ENU = R_ENU_ECEF * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
+ Eigen::Vector3d v_ENU = R_ENU_ECEF_ * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
- // 2D
+ // 2d
if (getProblem()->getDim() == 2)
{
+ WOLF_INFO("SensorGnss: initializing ENU-MAP 2D case....");
+
// compute antena vector (from 1 to 2) in MAP
- Eigen::Vector2s t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frameENU(2)) * getP()->getState().head<2>();
- Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
- Eigen::Vector2s v_MAP = t_MAP_antena2 - t_MAP_antenaENU;
-
- // initialize yaw
- Scalar theta_ENU = atan2(v_ENU(1),v_ENU(0));
- Scalar theta_MAP = atan2(v_MAP(1),v_MAP(0));
- Scalar yaw_ENU_MAP = theta_ENU-theta_MAP;
- setEnuMapYawState(yaw_ENU_MAP);
-
- // initialize translation
- Eigen::Vector3s t_ENU_MAP(Eigen::Vector3s::Zero());
- Eigen::Matrix2s R_ENU_MAP = computeREnuMap(getEnuMapRoll()->getState()(0),
- getEnuMapPitch()->getState()(0),
- getEnuMapYaw()->getState()(0)).topLeftCorner<2,2>();
+ Eigen::Vector2d t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frameENU(2)) * getP()->getState().head<2>();
+ Eigen::Vector2d t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
+ Eigen::Vector2d v_MAP = t_MAP_antena2 - t_MAP_antenaENU;
+
+ // ENU-MAP Rotation
+ Eigen::Matrix2d R_ENU_MAP;
+ // get it if already initialized
+ if (R_ENU_MAP_initialized_)
+ R_ENU_MAP = getREnuMap().topLeftCorner<2,2>();
+ // initialize yaw if not initialized
+ else
+ {
+ double theta_ENU = atan2(v_ENU(1),v_ENU(0));
+ double theta_MAP = atan2(v_MAP(1),v_MAP(0));
+ double yaw_ENU_MAP = theta_ENU-theta_MAP;
+ setEnuMapYawState(yaw_ENU_MAP);
+
+ R_ENU_MAP = computeREnuMap(getEnuMapRoll()->getState()(0),
+ getEnuMapPitch()->getState()(0),
+ getEnuMapYaw()->getState()(0)).topLeftCorner<2,2>();
+
+ WOLF_INFO("SensorGnss: ENU-MAP rotation initialized.");
+ }
+ // ENU-MAP translation
+ Eigen::Vector3d t_ENU_MAP(Eigen::Vector3d::Zero());
t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
-
- // set translation state
setEnuMapTranslationState(t_ENU_MAP);
+ WOLF_INFO("SensorGnss: ENU-MAP translation initialized.");
+
//std::cout << "-----------------------------------------\n";
//std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl;
//std::cout << "t_ECEF_antena2: " << _t_ECEF_antena2.transpose() << std::endl;
@@ -167,111 +172,87 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
//std::cout << "yaw set: " << yaw << std::endl;
//std::cout << "t_ENU1_origin: " << t_ENU_MAP.transpose() << std::endl;
//std::cout << "-----checks\n";
- //std::cout << "R(-yaw) * v_ENU: " << (Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>()).transpose() << std::endl;
+ //std::cout << "R(-yaw) * v_ENU: " << (Eigen::Rotation2D<double>(-yaw) * v_ENU.head<2>()).transpose() << std::endl;
//std::cout << "should be: " << v_MAP.transpose() << std::endl;
- //std::cout << "atan2(R(-yaw) * v_ENU): " << atan2((Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>())(1), (Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>())(0)) << std::endl;
+ //std::cout << "atan2(R(-yaw) * v_ENU): " << atan2((Eigen::Rotation2D<double>(-yaw) * v_ENU.head<2>())(1), (Eigen::Rotation2D<double>(-yaw) * v_ENU.head<2>())(0)) << std::endl;
//std::cout << "should be: " << atan2(v_MAP(1),v_MAP(0)) << std::endl;
- //std::cout << "R(yaw) * v_MAP: " << (Eigen::Rotation2D<Scalar>(yaw) * v_MAP).transpose() << std::endl;
+ //std::cout << "R(yaw) * v_MAP: " << (Eigen::Rotation2D<double>(yaw) * v_MAP).transpose() << std::endl;
//std::cout << "should be: " << v_ENU.head<2>().transpose() << std::endl;
- //std::cout << "atan2(R(yaw) * v_MAP): " << atan2((Eigen::Rotation2D<Scalar>(yaw) * v_MAP)(1),(Eigen::Rotation2D<Scalar>(yaw) * v_MAP)(0)) << std::endl;
+ //std::cout << "atan2(R(yaw) * v_MAP): " << atan2((Eigen::Rotation2D<double>(yaw) * v_MAP)(1),(Eigen::Rotation2D<double>(yaw) * v_MAP)(0)) << std::endl;
//std::cout << "should be: " << atan2(v_ENU(1),v_ENU(0)) << std::endl;
//std::cout << "v_ENU.norm(): " << v_ENU.norm() << std::endl;
//std::cout << "v_MAP.norm(): " << v_MAP.norm() << std::endl;
-
- WOLF_INFO("SensorGnss: ENU-MAP initialized.")
}
- // 3D
+ // 3d
else
{
//TODO
- std::runtime_error("not implemented yet");
+ WOLF_WARN("initialization of ENU-MAP not implemented in 3D")
+ //throw std::runtime_error("not implemented yet");
}
}
-void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, const Eigen::VectorXs& _pose_MAP_frame2,
- const Eigen::Vector3s& _v_ECEF_antena1_antena2)
+void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXd& _pose_MAP_frame1, const Eigen::VectorXd& _pose_MAP_frame2,
+ const Eigen::Vector3d& _v_ECEF_antena1_antena2)
{
assert(ENU_defined_ && "initializing ENU-MAP yaw without ENU defined");
- Eigen::Vector2s t_MAP_antena1 = _pose_MAP_frame1.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame1(2)) * getP()->getState().head<2>();
- Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
- Eigen::Vector2s v_MAP_antena1_antena2 = t_MAP_antena2 - t_MAP_antena1;
- Eigen::Vector3s v_ENU_antena1_antena2 = R_ENU_ECEF_ * _v_ECEF_antena1_antena2;
+ Eigen::Vector2d t_MAP_antena1 = _pose_MAP_frame1.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame1(2)) * getP()->getState().head<2>();
+ Eigen::Vector2d t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
+ Eigen::Vector2d v_MAP_antena1_antena2 = t_MAP_antena2 - t_MAP_antena1;
+ Eigen::Vector3d v_ENU_antena1_antena2 = R_ENU_ECEF_ * _v_ECEF_antena1_antena2;
- Scalar theta_ENU = atan2(v_ENU_antena1_antena2(1),v_ENU_antena1_antena2(0));
- Scalar theta_MAP = atan2(v_MAP_antena1_antena2(1),v_MAP_antena1_antena2(0));
- Scalar yaw_ENU_MAP = theta_ENU-theta_MAP;
+ double theta_ENU = atan2(v_ENU_antena1_antena2(1),v_ENU_antena1_antena2(0));
+ double theta_MAP = atan2(v_MAP_antena1_antena2(1),v_MAP_antena1_antena2(0));
+ double yaw_ENU_MAP = theta_ENU-theta_MAP;
setEnuMapYawState(yaw_ENU_MAP);
+
+ WOLF_INFO("SensorGnss: ENU-MAP Rotation initialized.")
}
-void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP)
+void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3d& t_ENU_MAP)
{
getEnuMapTranslation()->setState(t_ENU_MAP);
- ENU_MAP_initialized_ = true;
+ t_ENU_MAP_initialized_ = true;
}
-void SensorGnss::setEnuMapRollState(const Scalar& roll_ENU_MAP)
+void SensorGnss::setEnuMapRollState(const double& roll_ENU_MAP)
{
- getEnuMapRoll()->setState(Eigen::Vector1s(roll_ENU_MAP));
+ getEnuMapRoll()->setState(Eigen::Vector1d(roll_ENU_MAP));
- ENU_MAP_initialized_ = true;
+ R_ENU_MAP_initialized_ = true;
}
-void SensorGnss::setEnuMapPitchState(const Scalar& pitch_ENU_MAP)
+void SensorGnss::setEnuMapPitchState(const double& pitch_ENU_MAP)
{
- getEnuMapPitch()->setState(Eigen::Vector1s(pitch_ENU_MAP));
+ getEnuMapPitch()->setState(Eigen::Vector1d(pitch_ENU_MAP));
- ENU_MAP_initialized_ = true;
+ R_ENU_MAP_initialized_ = true;
}
-void SensorGnss::setEnuMapYawState(const Scalar& yaw_ENU_MAP)
+void SensorGnss::setEnuMapYawState(const double& yaw_ENU_MAP)
{
- getEnuMapYaw()->setState(Eigen::Vector1s(yaw_ENU_MAP));
+ getEnuMapYaw()->setState(Eigen::Vector1d(yaw_ENU_MAP));
- ENU_MAP_initialized_ = true;
-}
-
-Eigen::Matrix3s SensorGnss::getREnuMap() const
-{
- return Eigen::Matrix3s(Eigen::AngleAxis<Scalar>(getEnuMapRoll() ->getState()(0), Eigen::Vector3s::UnitX()) *
- Eigen::AngleAxis<Scalar>(getEnuMapPitch()->getState()(0), Eigen::Vector3s::UnitY()) *
- Eigen::AngleAxis<Scalar>(getEnuMapYaw() ->getState()(0), Eigen::Vector3s::UnitZ()));
+ R_ENU_MAP_initialized_ = true;
}
-// Define the factory method
-SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
- const IntrinsicsBasePtr _intrinsics)
+Eigen::Vector3d SensorGnss::computeFrameAntennaPosEcef(const FrameBasePtr& frm) const
{
- assert((_extrinsics.size() == 3 || _extrinsics.size() == 9) && "Bad extrinsics vector length. Should be 3 (antena position) or 9 (antena position and MAP-ENU initialization: position XYZ and orientation RPY)");
-
- SensorBasePtr sen = nullptr;
- IntrinsicsGnssPtr intrinsics_gnss_ptr = std::static_pointer_cast<IntrinsicsGnss>(_intrinsics);
- StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head(3), intrinsics_gnss_ptr->extrinsics_fixed_);
-
- if (_extrinsics.size() == 3)
- sen = std::make_shared<SensorGnss>(p_ptr, nullptr, intrinsics_gnss_ptr);
- else
- {
- StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head<3>(), intrinsics_gnss_ptr->extrinsics_fixed_);
- StateBlockPtr translation_ptr = std::make_shared<StateBlock>(_extrinsics.segment<3>(3), intrinsics_gnss_ptr->translation_fixed_);
- StateBlockPtr roll_ptr = std::make_shared<StateBlock>(_extrinsics.segment<1>(6), intrinsics_gnss_ptr->roll_fixed_);
- StateBlockPtr pitch_ptr = std::make_shared<StateBlock>(_extrinsics.segment<1>(7), intrinsics_gnss_ptr->pitch_fixed_);
- StateBlockPtr yaw_ptr = std::make_shared<StateBlock>(_extrinsics.segment<1>(8), intrinsics_gnss_ptr->yaw_fixed_);
-
- sen = std::make_shared<SensorGnss>(p_ptr, nullptr, intrinsics_gnss_ptr, translation_ptr, roll_ptr, pitch_ptr, yaw_ptr);
- }
+ assert(isEnuDefined());
+ assert(frm and frm->getP() and frm->getO());
- sen->setName(_unique_name);
- return sen;
+ return R_ENU_ECEF_.transpose() * (-t_ENU_ECEF_ + gettEnuMap() + getREnuMap() * (frm->getP()->getState() + Eigen::Quaterniond(Eigen::Vector4d(frm->getO()->getState())) * this->getP()->getState()));
}
} // namespace wolf
-// Register in the SensorFactory
-#include "core/sensor/sensor_factory.h"
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
namespace wolf {
-WOLF_REGISTER_SENSOR("SensorGnss",SensorGnss)
+WOLF_REGISTER_SENSOR(SensorGnss)
+WOLF_REGISTER_SENSOR_AUTO(SensorGnss)
} // namespace wolf
diff --git a/src/tree_manager/tree_manager_sliding_window_tdcp.cpp b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
new file mode 100644
index 000000000..6d97ad40a
--- /dev/null
+++ b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
@@ -0,0 +1,66 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include "gnss/tree_manager/tree_manager_sliding_window_tdcp.h"
+
+namespace wolf
+{
+
+TreeManagerSlidingWindowTdcp::TreeManagerSlidingWindowTdcp(ParamsTreeManagerSlidingWindowTdcpPtr _params) :
+ TreeManagerSlidingWindow(_params),
+ params_sw_sb_(_params)
+{
+ NodeBase::setType("TreeManagerSlidingWindowTdcp");
+};
+
+void TreeManagerSlidingWindowTdcp::keyFrameCallback(FrameBasePtr _key_frame)
+{
+ assert(getProblem() && "TreeManagerSlidingWindowTdcp::keyFrameCallback: problem not set.");
+
+ // store first frame
+ auto first_frame = getProblem()->getTrajectory()->getFirstFrame();
+
+ // call base sliding window tree manager
+ TreeManagerSlidingWindow::keyFrameCallback(_key_frame);
+
+ // if first frame was removed, activate all factors of new first frame
+ if (first_frame != getProblem()->getTrajectory()->getFirstFrame())
+ {
+ assert(first_frame->isRemoving());
+ for (auto fac : getProblem()->getTrajectory()->getFirstFrame()->getConstrainedByList())
+ if (fac and not fac->isRemoving() and
+ (fac->getType() == "FactorGnssTdcp" or
+ fac->getType() == "FactorGnssTdcp2d" or
+ fac->getType() == "FactorGnssTdcp3d" or
+ fac->getType() == "FactorGnssTdcpBatch"))
+ fac->setStatus(FAC_ACTIVE);
+ }
+}
+
+} /* namespace wolf */
+
+// Register in the FactoryTreeManager
+#include "core/tree_manager/factory_tree_manager.h"
+namespace wolf {
+WOLF_REGISTER_TREE_MANAGER(TreeManagerSlidingWindowTdcp);
+WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerSlidingWindowTdcp);
+} // namespace wolf
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 726d98fd4..fc829135d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,22 +1,26 @@
# Retrieve googletest from github & compile
add_subdirectory(gtest)
-
# Include gtest directory.
include_directories(${GTEST_INCLUDE_DIRS})
############# USE THIS TEST AS AN EXAMPLE #################
# #
# Create a specific test executable for gtest_example #
-wolf_add_gtest(gtest_example gtest_example.cpp) #
-target_link_libraries(gtest_example ${PLUGIN_NAME}) #
+# wolf_add_gtest(gtest_example gtest_example.cpp) #
+# target_link_libraries(gtest_example ${PLUGIN_NAME}) #
# #
###########################################################
-# FactorGnssFix2D test
-wolf_add_gtest(gtest_factor_gnss_fix_2D gtest_factor_gnss_fix_2D.cpp)
-target_link_libraries(gtest_factor_gnss_fix_2D ${PLUGIN_NAME} ${wolf_LIBRARY})
+# FactorGnssFix2d test
+wolf_add_gtest(gtest_factor_gnss_fix_2d gtest_factor_gnss_fix_2d.cpp)
+target_link_libraries(gtest_factor_gnss_fix_2d ${PLUGIN_NAME})
+
+# FactorGnssPseudoRange test
+wolf_add_gtest(gtest_factor_gnss_pseudo_range gtest_factor_gnss_pseudo_range.cpp)
+target_link_libraries(gtest_factor_gnss_pseudo_range ${PLUGIN_NAME})
+
+# FactorGnssTdcp test
+wolf_add_gtest(gtest_factor_gnss_tdcp gtest_factor_gnss_tdcp.cpp)
+target_link_libraries(gtest_factor_gnss_tdcp ${PLUGIN_NAME})
-# FactorGnssSingleDiff2D test
-wolf_add_gtest(gtest_factor_gnss_single_diff_2D gtest_factor_gnss_single_diff_2D.cpp)
-target_link_libraries(gtest_factor_gnss_single_diff_2D ${PLUGIN_NAME} ${wolf_LIBRARY})
\ No newline at end of file
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index 140792d53..c73292612 100644
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
@@ -1,3 +1,24 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
#include <core/utils/utils_gtest.h>
TEST(TestTest, DummyTestExample)
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2d.cpp
similarity index 51%
rename from test/gtest_factor_gnss_fix_2D.cpp
rename to test/gtest_factor_gnss_fix_2d.cpp
index 762e61fb4..a95688fd9 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2d.cpp
@@ -1,34 +1,53 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
/**
- * \file gtest_factor_gnss_fix_2D.cpp
+ * \file gtest_factor_gnss_fix_2d.cpp
*
* Created on: Aug 1, 2018
* \author: jvallve
*/
-
-#include "gnss/factor/factor_gnss_fix_2D.h"
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
#include "core/problem/problem.h"
#include "gnss/sensor/sensor_gnss.h"
#include "gnss/processor/processor_gnss_fix.h"
-
-#include "core/ceres_wrapper/ceres_manager.h"
-
+#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/factor/factor_gnss_fix_2d.h"
using namespace Eigen;
using namespace wolf;
void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr,
- const Vector1s& o_enu_map,
- const Vector3s& t_base_antena,
- const Vector3s& t_enu_map,
- const Vector3s& t_map_base,
- const Vector1s& o_map_base)
+ const Vector1d& o_enu_map,
+ const Vector3d& t_base_antena,
+ const Vector3d& t_enu_map,
+ const Vector3d& t_map_base,
+ const Vector1d& o_map_base)
{
// ENU-MAP
- gnss_sensor_ptr->getEnuMapRoll()->setState(Eigen::Vector1s::Zero());
- gnss_sensor_ptr->getEnuMapPitch()->setState(Eigen::Vector1s::Zero());
+ gnss_sensor_ptr->getEnuMapRoll()->setState(Eigen::Vector1d::Zero());
+ gnss_sensor_ptr->getEnuMapPitch()->setState(Eigen::Vector1d::Zero());
gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map);
gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map);
// Antena
@@ -52,52 +71,53 @@ void fixAllStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr)
frame_ptr->getO()->fix();
}
-void computeParamSizes(const CeresManagerPtr& ceres_mgr_ptr, int& num_params_reduced, int& num_param_blocks_reduced )
+void computeParamSizes(const SolverCeresPtr& solver_ceres, int& num_params_reduced, int& num_param_blocks_reduced )
{
num_param_blocks_reduced = 0;
num_params_reduced = 0;
- std::vector<Scalar*> param_blocks;
- ceres_mgr_ptr->getCeresProblem()->GetParameterBlocks(¶m_blocks);
+ std::vector<double*> param_blocks;
+ solver_ceres->getCeresProblem()->GetParameterBlocks(¶m_blocks);
for (auto pb : param_blocks)
{
std::vector<ceres::ResidualBlockId> residual_blocks;
- ceres_mgr_ptr->getCeresProblem()->GetResidualBlocksForParameterBlock(pb,&residual_blocks);
+ solver_ceres->getCeresProblem()->GetResidualBlocksForParameterBlock(pb,&residual_blocks);
- if (!ceres_mgr_ptr->getCeresProblem()->IsParameterBlockConstant(pb) && !residual_blocks.empty())
+ if (not solver_ceres->getCeresProblem()->IsParameterBlockConstant(pb) and
+ not residual_blocks.empty())
{
- num_param_blocks_reduced ++;
- num_params_reduced += ceres_mgr_ptr->getCeresProblem()->ParameterBlockLocalSize(pb);
+ num_param_blocks_reduced ++;
+ num_params_reduced += solver_ceres->getCeresProblem()->ParameterBlockLocalSize(pb);
}
}
}
// groundtruth transformations
-Vector1s o_enu_map = (Vector1s() << 2.6).finished();
-Vector3s t_enu_map = (Vector3s() << 12, -34, 4).finished();
-Vector3s t_map_base = (Vector3s() << 32, -34, 0).finished(); // z=0
-Vector1s o_map_base = (Vector1s() << -0.4).finished();
-Vector3s t_base_antena = (Vector3s() << 3,-2,8).finished();// Antena extrinsics
-Vector3s t_ecef_enu = (Vector3s() << 100,30,50).finished();
-Matrix3s R_ecef_enu = (AngleAxis<Scalar>(1.3, Vector3s::UnitX()) *
- AngleAxis<Scalar>(-2.2, Vector3s::UnitY()) *
- AngleAxis<Scalar>(-1.8, Vector3s::UnitZ())).toRotationMatrix();
-Matrix3s R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
-Matrix3s R_map_base = AngleAxis<Scalar>(o_map_base(0), Vector3s::UnitZ()).toRotationMatrix();
-Vector3s t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+Vector1d o_enu_map = (Vector1d() << 2.6).finished();
+Vector3d t_enu_map = (Vector3d() << 12, -34, 4).finished();
+Vector3d t_map_base = (Vector3d() << 32, -34, 0).finished(); // z=0
+Vector1d o_map_base = (Vector1d() << -0.4).finished();
+Vector3d t_base_antena = (Vector3d() << 3,-2,8).finished();// Antena extrinsics
+Vector3d t_ecef_enu = (Vector3d() << 100,30,50).finished();
+Matrix3d R_ecef_enu = (AngleAxis<double>(1.3, Vector3d::UnitX()) *
+ AngleAxis<double>(-2.2, Vector3d::UnitY()) *
+ AngleAxis<double>(-1.8, Vector3d::UnitZ())).toRotationMatrix();
+Matrix3d R_enu_map = AngleAxis<double>(o_enu_map(0), Vector3d::UnitZ()).toRotationMatrix();
+Matrix3d R_map_base = AngleAxis<double>(o_map_base(0), Vector3d::UnitZ()).toRotationMatrix();
+Vector3d t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
// WOLF
ProblemPtr problem_ptr = Problem::create("PO", 2);
-CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
-SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<IntrinsicsGnss>()));
+SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr);
+SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
FrameBasePtr frame_ptr;
////////////////////////////////////////////////////////
-TEST(FactorGnssFix2DTest, configure_tree)
+TEST(FactorGnssFix2dTest, configure_tree)
{
- ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
+ solver_ceres->getSolverOptions().max_num_iterations = 100;
// Configure sensor and processor
gnss_sensor_ptr->setEnuMapTranslationState(t_enu_map);
@@ -106,23 +126,23 @@ TEST(FactorGnssFix2DTest, configure_tree)
gnss_sensor_ptr->getEnuMapPitch()->fix();
gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
- ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>();
+ ParamsProcessorGnssFixPtr gnss_params_ptr = std::make_shared<ParamsProcessorGnssFix>();
gnss_params_ptr->time_tolerance = 1.0;
gnss_params_ptr->voting_active = true;
+ gnss_params_ptr->apply_loss_function = false;
problem_ptr->installProcessor("ProcessorGnssFix", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
// Emplace a frame (FIXED)
- Vector3s frame_pose = (Vector3s() << t_map_base(0), t_map_base(1), o_map_base(0)).finished();
- frame_ptr = problem_ptr->emplaceFrame(KEY, frame_pose, TimeStamp(0));
- problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0);
+ Vector3d frame_pose = (Vector3d() << t_map_base(0), t_map_base(1), o_map_base(0)).finished();
+ frame_ptr = problem_ptr->emplaceFrame( TimeStamp(0), frame_pose);
+ problem_ptr->keyFrameCallback(frame_ptr, nullptr);
// Create & process GNSS Fix capture
- CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity());
+ CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3d::Identity());
gnss_sensor_ptr->process(cap_gnss_ptr);
// Checks
- ASSERT_TRUE(problem_ptr->check(0));
- ASSERT_TRUE(frame_ptr->isKey());
+ EXPECT_TRUE(problem_ptr->check(1));
}
/*
@@ -131,7 +151,7 @@ TEST(FactorGnssFix2DTest, configure_tree)
* Estimating: MAP_BASE position.
* Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
*/
-TEST(FactorGnssFix2DTest, gnss_1_map_base_position)
+TEST(FactorGnssFix2dTest, gnss_1_map_base_position)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
@@ -141,34 +161,34 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_position)
frame_ptr->getP()->unfix();
// --------------------------- distort: map base position
- Vector3s frm_dist = frame_ptr->getState();
+ Vector3d frm_dist = frame_ptr->getState().vector("PO");
frm_dist(0) += 0.18;
frm_dist(1) += -0.58;
- frame_ptr->setState(frm_dist);
+ frame_ptr->setState(frm_dist, "PO", {2,1});
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 2);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 2);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 2);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 2);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(frame_ptr->getState().head(2), t_map_base.head(2), 1e-6);
+ EXPECT_MATRIX_APPROX(frame_ptr->getState().at('P'), t_map_base.head(2), 1e-6);
}
/*
@@ -177,7 +197,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_position)
* Estimating: MAP_BASE orientation.
* Fixed: ENU_MAP, MAP_BASE position, BASE_ANTENA.
*/
-TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation)
+TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
@@ -187,31 +207,31 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation)
frame_ptr->getO()->unfix();
// --------------------------- distort: map base orientation
- Vector1s frm_dist = frame_ptr->getO()->getState();
+ Vector1d frm_dist = frame_ptr->getO()->getState();
frm_dist(0) += 0.18;
frame_ptr->getO()->setState(frm_dist);
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 1);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 1);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET);
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6);
+ EXPECT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6);
}
/*
@@ -220,7 +240,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation)
* Estimating: ENU_MAP yaw.
* Fixed: ENU_MAP position, MAP_BASE and BASE_ANTENA.
*/
-TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
+TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
@@ -230,31 +250,34 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
gnss_sensor_ptr->getEnuMapYaw()->unfix();
// --------------------------- distort: yaw enu_map
- Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
+ Vector1d o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
o_enu_map_dist(0) += 0.18;
gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map_dist);
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
+ EXPECT_TRUE(solver_ceres->check());
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 1);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 1);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6);
+ EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6);
+
+ problem_ptr->print(4,1,1,1);
}
/*
@@ -263,7 +286,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
* Estimating: ENU_MAP position.
* Fixed: ENU_MAP yaw, MAP_BASE and BASE_ANTENA.
*/
-TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
+TEST(FactorGnssFix2dTest, gnss_1_enu_map_position)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
@@ -273,32 +296,32 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
gnss_sensor_ptr->getEnuMapTranslation()->unfix();// enu-map yaw translation
// --------------------------- distort: position enu_map
- Vector3s t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslation()->getState();
+ Vector3d t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslation()->getState();
t_enu_map_dist(0) += 0.86;
t_enu_map_dist(1) += -0.34;
gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map_dist);
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 3);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 3);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET);
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 3);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslation()->getState().head(2), t_enu_map.head(2), 1e-6);
+ EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslation()->getState().head(2), t_enu_map.head(2), 1e-6);
}
/*
@@ -307,7 +330,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
* Estimating: BASE_ANTENA (2 first components that are observable).
* Fixed: ENU_MAP, MAP_BASE.
*/
-TEST(FactorGnssFix2DTest, gnss_1_base_antena)
+TEST(FactorGnssFix2dTest, gnss_1_base_antena)
{
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);
@@ -317,32 +340,32 @@ TEST(FactorGnssFix2DTest, gnss_1_base_antena)
gnss_sensor_ptr->getP()->unfix();
// --------------------------- distort: base_antena
- Vector3s base_antena_dist = gnss_sensor_ptr->getP()->getState();
+ Vector3d base_antena_dist = gnss_sensor_ptr->getP()->getState();
base_antena_dist(0) += 1.68;
base_antena_dist(0) += -0.48;
gnss_sensor_ptr->getP()->setState(base_antena_dist);
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 3);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 3);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET);
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 3);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6);
+ EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6);
}
int main(int argc, char **argv)
@@ -350,4 +373,3 @@ int main(int argc, char **argv)
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-
diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp
new file mode 100644
index 000000000..f6691f178
--- /dev/null
+++ b/test/gtest_factor_gnss_pseudo_range.cpp
@@ -0,0 +1,322 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "core/problem/problem.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/capture/capture_gnss.h"
+#include "gnss/factor/factor_gnss_pseudo_range.h"
+
+using namespace Eigen;
+using namespace wolf;
+using namespace GnssUtils;
+
+// groundtruth transformations
+Vector3d t_ecef_enu, t_enu_map, t_map_base, t_base_antena; // Antena extrinsics
+Vector3d t_ecef_antena;
+Vector3d rpy_enu_map;
+Matrix3d R_ecef_enu, R_enu_map;
+Quaterniond q_map_base;
+Vector1d clock_drift;
+
+GnssUtils::Satellite sat1({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat2({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat3({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat4({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+
+GnssUtils::Range range1, range2, range3, range4;
+
+
+// WOLF
+ProblemPtr prb = Problem::create("PO", 3);
+SolverCeresPtr solver = std::make_shared<SolverCeres>(prb);
+SensorGnssPtr gnss_sensor = std::static_pointer_cast<SensorGnss>(prb->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
+FrameBasePtr frm;
+CaptureGnssPtr cap;
+FeatureGnssSatellitePtr ftr1, ftr2, ftr3, ftr4;
+FactorGnssPseudoRangePtr fac1, fac2, fac3, fac4;
+
+void randomGroundtruth()
+{
+ // ECEF-ENU
+ t_ecef_enu = Vector3d::Random() * 1e3;
+ Vector3d rpy_ecef_enu = M_PI*Vector3d::Random();
+ R_ecef_enu = (AngleAxis<double>(rpy_ecef_enu(0), Vector3d::UnitX()) *
+ AngleAxis<double>(rpy_ecef_enu(1), Vector3d::UnitY()) *
+ AngleAxis<double>(rpy_ecef_enu(2), Vector3d::UnitZ())).toRotationMatrix();
+
+ // ENU-MAP
+ t_enu_map = Vector3d::Random() * 1e3;
+ rpy_enu_map = M_PI*Vector3d::Random();
+ R_enu_map = (AngleAxis<double>(rpy_enu_map(0), Vector3d::UnitX()) *
+ AngleAxis<double>(rpy_enu_map(1), Vector3d::UnitY()) *
+ AngleAxis<double>(rpy_enu_map(2), Vector3d::UnitZ())).toRotationMatrix();
+
+ // MAP-BASE
+ t_map_base = Vector3d::Random() * 1e2;
+ q_map_base = Quaterniond::UnitRandom();
+
+ // BASE-ANTENA (EXTRINSICS)
+ t_base_antena = Vector3d::Random();
+
+ // composition
+ t_ecef_antena = R_ecef_enu * (R_enu_map * (q_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+
+ // SATELLITES
+ sat1.pos = Vector3d::Random() * 1e4;
+ sat2.pos = Vector3d::Random() * 1e4;
+ sat3.pos = Vector3d::Random() * 1e4;
+ sat4.pos = Vector3d::Random() * 1e4;
+
+ // clock drift
+ clock_drift = Vector1d::Random() * 1e2;
+
+ // pseudo ranges
+ range1.P_corrected = (sat1.pos-t_ecef_antena).norm() + clock_drift(0);
+ range2.P_corrected = (sat2.pos-t_ecef_antena).norm() + clock_drift(0);
+ range3.P_corrected = (sat3.pos-t_ecef_antena).norm() + clock_drift(0);
+ range4.P_corrected = (sat4.pos-t_ecef_antena).norm() + clock_drift(0);
+ range1.P_var = 1.0;
+ range2.P_var = 1.0;
+ range3.P_var = 1.0;
+ range4.P_var = 1.0;
+}
+
+void setUpProblem()
+{
+ // remove previous setup
+ if (frm)
+ frm->remove();
+
+ solver->getSolverOptions().max_num_iterations = 100;
+
+ // Sensor
+ // ENU-MAP
+ gnss_sensor->setEnuMapTranslationState(t_enu_map);
+ gnss_sensor->setEnuMapRollState(rpy_enu_map(0));
+ gnss_sensor->setEnuMapPitchState(rpy_enu_map(1));
+ gnss_sensor->setEnuMapYawState(rpy_enu_map(2));
+ // ECEF-ENU
+ gnss_sensor->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
+ // Extrinsics
+ gnss_sensor->getP()->setState(t_base_antena);
+
+ // Frame
+ Vector7d frm_state;
+ frm_state.head<3>() = t_map_base;
+ frm_state.tail<4>() = q_map_base.coeffs();
+ frm = prb->emplaceFrame( TimeStamp(0), frm_state);
+
+ // capture
+ cap = CaptureBase::emplace<CaptureGnss>(frm, TimeStamp(0), gnss_sensor, nullptr);
+ cap->getStateBlock('T')->unfix();
+ cap->getStateBlock('T')->setState(clock_drift);
+
+ // features
+ ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat1, range1); // obsd_t data is not used in pseudo range factors
+ ftr2 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat2, range2); // obsd_t data is not used in pseudo range factors
+ ftr3 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat3, range3); // obsd_t data is not used in pseudo range factors
+ ftr4 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat4, range4); // obsd_t data is not used in pseudo range factors
+
+ // factors
+ fac1 = FactorBase::emplace<FactorGnssPseudoRange>(ftr1, ftr1, gnss_sensor, nullptr, false);
+ fac2 = FactorBase::emplace<FactorGnssPseudoRange>(ftr2, ftr2, gnss_sensor, nullptr, false);
+ fac3 = FactorBase::emplace<FactorGnssPseudoRange>(ftr3, ftr3, gnss_sensor, nullptr, false);
+ fac4 = FactorBase::emplace<FactorGnssPseudoRange>(ftr4, ftr4, gnss_sensor, nullptr, false);
+
+ // ASSERTS
+ // ENU-MAP
+ ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapRoll()->getState()(0), rpy_enu_map(0));
+ ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapPitch()->getState()(0), rpy_enu_map(1));
+ ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapYaw()->getState()(0), rpy_enu_map(2));
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-3);
+ // extrinsics
+ ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena, 1e-3);
+ // Frame
+ ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-3);
+ ASSERT_MATRIX_APPROX(frm->getO()->getState(), q_map_base.coeffs(), 1e-3);
+ // clock drift
+ ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock('T')->getState(), 1e-3);
+ // composition
+ ASSERT_MATRIX_APPROX(gnss_sensor->computeFrameAntennaPosEcef(frm), t_ecef_antena, 1e-3);
+}
+
+void fixAllStates()
+{
+ // ENU-MAP
+ gnss_sensor->getEnuMapRoll()->fix();
+ gnss_sensor->getEnuMapPitch()->fix();
+ gnss_sensor->getEnuMapYaw()->fix();
+ gnss_sensor->getEnuMapTranslation()->fix();
+ // Antena
+ gnss_sensor->getP()->fix();
+ // Frame
+ frm->fix();
+ // clock drift
+ cap->fix();
+ }
+
+////////////////////////////////////////////////////////
+TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ cap->getStateBlock('T')->unfix();
+
+ // perturb
+ cap->getStateBlock('T')->perturb(1e2);
+
+ // Only 1 factor
+ fac2->setStatus(FAC_INACTIVE);
+ fac3->setStatus(FAC_INACTIVE);
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock('T')->getState(), 1e-3);
+ }
+}
+
+TEST(FactorGnssPreusoRangeTest, observe_frame_p)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ frm->getP()->unfix();
+
+ // perturb
+ frm->getP()->perturb(1);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-3);
+ }
+}
+
+TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ frm->getP()->unfix();
+ cap->getStateBlock('T')->unfix();
+
+ // perturb
+ frm->getP()->perturb(1);
+ cap->getStateBlock('T')->perturb(1e2);
+
+ // all 4 factors
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock('T')->getState(), 1e-3);
+ }
+}
+
+TEST(FactorGnssPreusoRangeTest, observe_enumap_p)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ gnss_sensor->getEnuMapTranslation()->unfix();
+
+ // perturb
+ gnss_sensor->getEnuMapTranslation()->perturb(1e2);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-3);
+ }
+}
+
+TEST(FactorGnssPreusoRangeTest, observe_enumap_o)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ gnss_sensor->getEnuMapRoll()->unfix();
+ gnss_sensor->getEnuMapPitch()->unfix();
+ gnss_sensor->getEnuMapYaw()->unfix();
+
+ // perturb
+ gnss_sensor->getEnuMapRoll()->perturb(1);
+ gnss_sensor->getEnuMapPitch()->perturb(1);
+ gnss_sensor->getEnuMapYaw()->perturb(1);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-3);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp
deleted file mode 100644
index dcabb00df..000000000
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ /dev/null
@@ -1,267 +0,0 @@
-/**
- * \file gtest_factor_gnss_single_diff_2D.cpp
- *
- * Created on: Aug 28, 2018
- * \author: jvallve
- */
-
-
-#include "gnss/factor/factor_gnss_single_diff_2D.h"
-#include <core/utils/utils_gtest.h>
-
-#include "core/capture/capture_motion.h"
-#include "core/problem/problem.h"
-#include "gnss/sensor/sensor_gnss.h"
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_odom_2D.h"
-#include "gnss/processor/processor_gnss_single_diff.h"
-
-#include "core/ceres_wrapper/ceres_manager.h"
-
-
-using namespace Eigen;
-using namespace wolf;
-using std::cout;
-using std::endl;
-
-
-class FactorGnssSingleDiff2DTest : public testing::Test
-{
- public:
-
- // groundtruth transformations
- Vector3s t_ecef_enu;
- Matrix3s R_ecef_enu, R_enu_map, R_map_base1, R_map_base2;
- Vector3s t_base_antena, t_ecef_antena1, t_ecef_antena2;
- Vector1s o_enu_map;
- Vector3s t_map_base1, t_map_base2;
- Vector1s o_map_base1, o_map_base2;
-
- // WOLF
- ProblemPtr problem_ptr;
- CeresManagerPtr ceres_mgr_ptr;
- SensorGnssPtr gnss_sensor_ptr;
- SensorOdom2DPtr odom_sensor_ptr;
- FrameBasePtr prior_frame_ptr;
-
- FactorGnssSingleDiff2DTest()
- {
- o_enu_map << 2.6;
- t_map_base1 << 32, -34, 0; // z=0
- o_map_base1 << -0.4;
- t_map_base2 << -76, 63, 0; // z=0
- o_map_base2 << 0.7;
- t_base_antena << 3,-2,8;// Antena extrinsics
- t_ecef_enu << 100,30,50;
- R_ecef_enu = (AngleAxis<Scalar>(1.3, Vector3s::UnitX())
- *AngleAxis<Scalar>(-2.2, Vector3s::UnitY())
- *AngleAxis<Scalar>(-1.8, Vector3s::UnitZ())).toRotationMatrix();
-
- // ----------------------------------------------------
- // Problem and solver
- problem_ptr = Problem::create("PO", 2);
- ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
- ceres_mgr_ptr->getSolverOptions().max_num_iterations = 10;
-
- // gnss sensor
- gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
- gnss_sensor_ptr->getEnuMapRoll()->fix();
- gnss_sensor_ptr->getEnuMapPitch()->fix();
- gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
-
- // gnss processor
- ProcessorParamsGnssSingleDiffPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssSingleDiff>();
- gnss_params_ptr->time_tolerance = 1.0;
- gnss_params_ptr->voting_active = true;
- gnss_params_ptr->voting_aux_active = false;
- gnss_params_ptr->time_th = 0;
- gnss_params_ptr->dist_traveled = 0;
- gnss_params_ptr->enu_map_init_dist_min = 0;
- problem_ptr->installProcessor("GNSS SINGLE DIFFERENCES", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
-
- // odom sensor & processor
- IntrinsicsOdom2DPtr odom_intrinsics_ptr = std::make_shared<IntrinsicsOdom2D>();
- odom_intrinsics_ptr->k_disp_to_disp = 0.1;
- odom_intrinsics_ptr->k_rot_to_rot = 0.1;
- odom_sensor_ptr = std::static_pointer_cast<SensorOdom2D>(problem_ptr->installSensor("SensorOdom2D", "odometer", VectorXs::Zero(3), odom_intrinsics_ptr));
- ProcessorParamsOdom2DPtr odom_params_ptr = std::make_shared<ProcessorParamsOdom2D>();
- odom_params_ptr->voting_active = true;
- odom_params_ptr->dist_traveled = 1;
- odom_params_ptr->angle_turned = 2.0;
- odom_params_ptr->max_time_span = 1.0;
- odom_params_ptr->time_tolerance = 1.0;
- problem_ptr->installProcessor("ProcessorOdom2D", "main odometry", odom_sensor_ptr, odom_params_ptr);
- //problem_ptr->setProcessorMotion("main odometry");
-
- // set prior (FIXED)
- Vector3s frame1state = t_map_base1;
- frame1state(2) = o_map_base1(0);
- prior_frame_ptr = problem_ptr->setPrior(frame1state, Matrix3s::Identity(), TimeStamp(0), Scalar(0.1));
- prior_frame_ptr->fix();
- };
-
- virtual void SetUp()
- {
- // reset grountruth parameters
- R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
- R_map_base1 = Matrix3s::Identity();
- R_map_base1.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base1(0)).matrix();
- R_map_base2 = Matrix3s::Identity();
- R_map_base2.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base2(0)).matrix();
-
- t_ecef_antena1 = R_ecef_enu * R_enu_map * (R_map_base1 * t_base_antena + t_map_base1) + t_ecef_enu;
- t_ecef_antena2 = R_ecef_enu * R_enu_map * (R_map_base2 * t_base_antena + t_map_base2) + t_ecef_enu;
-
- // Reset antena extrinsics
- gnss_sensor_ptr->getP()->setState(t_base_antena);
-
- // Reset ENU_ECEF
- gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
- }
-};
-
-////////////////////////////////////////////////////////
-
-TEST_F(FactorGnssSingleDiff2DTest, check_tree)
-{
- ASSERT_TRUE(problem_ptr->check(0));
- ASSERT_TRUE(prior_frame_ptr != nullptr);
-}
-
-/*
- * Test with one GNSS Single Difference capture.
- *
- * Estimating: MAP_BASE2 position.
- * Fixed: MAP_BASE1, ENU_MAP fixed, MAP_BASE2 orientation, BASE_ANTENA.
- */
-TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
-{
- // Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
- gnss_sensor_ptr->process(cap_gnss_ptr);
-
- // fixing things
- gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0)); // enu-map yaw orientation
- gnss_sensor_ptr->getEnuMapYaw()->fix();
- cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
- cap_gnss_ptr->getFrame()->getO()->fix();
-
- std::cout << "frame1: " << prior_frame_ptr->getState().transpose() << std::endl;
-
- // distort frm position
- Vector3s frm_dist = cap_gnss_ptr->getFrame()->getState();
- frm_dist(0) += 18;
- frm_dist(1) += -58;
- cap_gnss_ptr->getFrame()->setState(frm_dist);
-
- // solve for frm
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
- ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFrame()->getState().head(2), t_map_base2.head(2), 1e-6);
-}
-
-/*
- * Test with one GNSS Single Difference capture.
- *
- * Estimating: MAP_BASE2 orientation.
- * Fixed: MAP_BASE1, ENU_MAP fixed, MAP_BASE2 position, BASE_ANTENA.
- */
-TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
-{
- ASSERT_TRUE(prior_frame_ptr != nullptr);
- // Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
- ASSERT_TRUE(cap_gnss_ptr->getOriginFrame() != nullptr);
- gnss_sensor_ptr->process(cap_gnss_ptr);
-
- // fixing things
- gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0)); // enu-map yaw orientation
- gnss_sensor_ptr->getEnuMapYaw()->fix();
- cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
- cap_gnss_ptr->getFrame()->getP()->fix();
- // distort frm position
- Vector1s frm_dist = cap_gnss_ptr->getFrame()->getO()->getState();
- frm_dist(0) += 1.8;
- cap_gnss_ptr->getFrame()->getO()->setState(frm_dist);
-
- // solve for frm
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
-
- ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
- ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFrame()->getO()->getState(), o_map_base2, 1e-6);
-}
-
-/*
- * Test with one GNSS Single Difference capture.
- *
- * Estimating: ENU_MAP yaw.
- * Fixed: MAP_BASE1, MAP_BASE2 and BASE_ANTENA.
- */
-TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
-{
- // Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
- gnss_sensor_ptr->process(cap_gnss_ptr);
-
- // unfixing things
- gnss_sensor_ptr->getEnuMapYaw()->unfix();
- // fixing things
- cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
- cap_gnss_ptr->getFrame()->getP()->fix();
- cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
- cap_gnss_ptr->getFrame()->getO()->fix();
-
- // distort yaw enu_map
- Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
- o_enu_map_dist(0) += 1.8;
- gnss_sensor_ptr->setEnuMapYawState(o_enu_map_dist(0));
-
- // solve for frm
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
-
- ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6);
-}
-
-/*
- * Test with one GNSS Single Difference capture.
- *
- * Estimating: BASE_ANTENA (2 first components obsevable).
- * Fixed: MAP_BASE1, ENU_MAP, MAP_BASE2.
- */
-TEST_F(FactorGnssSingleDiff2DTest, gnss_1_base_antena)
-{
- // Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
- gnss_sensor_ptr->process(cap_gnss_ptr);
-
- // unfixing things
- gnss_sensor_ptr->getP()->unfix();
- // fixing things
- gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0)); // enu-map yaw orientation
- gnss_sensor_ptr->getEnuMapYaw()->fix();
- cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
- cap_gnss_ptr->getFrame()->getP()->fix();
- cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
- cap_gnss_ptr->getFrame()->getO()->fix();
-
- // distort base_antena
- Vector3s base_antena_dist = gnss_sensor_ptr->getP()->getState();
- base_antena_dist(0) += 16.8;
- base_antena_dist(0) += -40.8;
- gnss_sensor_ptr->getP()->setState(base_antena_dist);
-
- // solve for frm
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
-
- ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp
new file mode 100644
index 000000000..7818ab4a9
--- /dev/null
+++ b/test/gtest_factor_gnss_tdcp.cpp
@@ -0,0 +1,440 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program 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 Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <core/ceres_wrapper/solver_ceres.h>
+
+#include "gnss/factor/factor_gnss_tdcp.h"
+#include <core/utils/utils_gtest.h>
+
+#include "core/problem/problem.h"
+#include "gnss/sensor/sensor_gnss.h"
+#include "gnss/capture/capture_gnss.h"
+
+
+using namespace Eigen;
+using namespace wolf;
+using namespace GnssUtils;
+
+// groundtruth transformations
+Vector3d t_ecef_enu, t_enu_map, t_map_base_r, t_map_base_k, t_base_antena; // Antena extrinsics
+Vector3d t_ecef_antena_r, t_ecef_antena_k;
+Vector3d rpy_enu_map;
+Matrix3d R_ecef_enu, R_enu_map;
+Quaterniond q_map_base_r, q_map_base_k;
+//double range1_r, range2_r, range3_r, range4_r, range1_k, range2_k, range3_k, range4_k;
+Vector1d clock_drift_r, clock_drift_k;
+
+GnssUtils::Satellite sat1_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat2_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat3_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat4_r({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat1_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat2_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat3_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Satellite sat4_k({SYS_GPS, 1,Eigen::Vector3d::Zero(),Eigen::Vector3d::Zero(),0,0,0});
+GnssUtils::Range range1_r, range2_r, range3_r, range4_r, range1_k, range2_k, range3_k, range4_k;
+
+// WOLF
+ProblemPtr prb = Problem::create("PO", 3);
+SolverCeresPtr solver = std::make_shared<SolverCeres>(prb);
+SensorGnssPtr gnss_sensor = std::static_pointer_cast<SensorGnss>(prb->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
+FrameBasePtr frm_r, frm_k;
+CaptureGnssPtr cap_r, cap_k;
+FeatureGnssSatellitePtr ftr1_r, ftr2_r, ftr3_r, ftr4_r, ftr1_k, ftr2_k, ftr3_k, ftr4_k;
+FactorGnssTdcpPtr fac1, fac2, fac3, fac4;
+
+void randomGroundtruth()
+{
+ // ECEF-ENU
+ t_ecef_enu = Vector3d::Random() * 1e3;
+ Vector3d rpy_ecef_enu = M_PI*Vector3d::Random();
+ R_ecef_enu = (AngleAxis<double>(rpy_ecef_enu(0), Vector3d::UnitX()) *
+ AngleAxis<double>(rpy_ecef_enu(1), Vector3d::UnitY()) *
+ AngleAxis<double>(rpy_ecef_enu(2), Vector3d::UnitZ())).toRotationMatrix();
+
+ // ENU-MAP
+ t_enu_map = Vector3d::Random() * 1e3;
+ rpy_enu_map = M_PI*Vector3d::Random();
+ R_enu_map = (AngleAxis<double>(rpy_enu_map(0), Vector3d::UnitX()) *
+ AngleAxis<double>(rpy_enu_map(1), Vector3d::UnitY()) *
+ AngleAxis<double>(rpy_enu_map(2), Vector3d::UnitZ())).toRotationMatrix();
+
+ // MAP-BASE
+ t_map_base_k = Vector3d::Random() * 1e2;
+ q_map_base_k = Quaterniond::UnitRandom();
+ t_map_base_r = Vector3d::Random() * 1e2;
+ q_map_base_r = Quaterniond::UnitRandom();
+
+ // BASE-ANTENA (EXTRINSICS)
+ t_base_antena = Vector3d::Random();
+
+ // composition
+ t_ecef_antena_r = R_ecef_enu * (R_enu_map * (q_map_base_r * t_base_antena + t_map_base_r) + t_enu_map ) + t_ecef_enu;
+ t_ecef_antena_k = R_ecef_enu * (R_enu_map * (q_map_base_k * t_base_antena + t_map_base_k) + t_enu_map ) + t_ecef_enu;
+
+ // SATELLITES
+ sat1_r.pos = Vector3d::Random() * 1e4;
+ sat2_r.pos = Vector3d::Random() * 1e4;
+ sat3_r.pos = Vector3d::Random() * 1e4;
+ sat4_r.pos = Vector3d::Random() * 1e4;
+ sat1_k.pos = Vector3d::Random() * 1e4;
+ sat2_k.pos = Vector3d::Random() * 1e4;
+ sat3_k.pos = Vector3d::Random() * 1e4;
+ sat4_k.pos = Vector3d::Random() * 1e4;
+
+ // clock drift
+ clock_drift_r = Vector1d::Random() * 1e2;
+ clock_drift_k = Vector1d::Random() * 1e2;
+
+ // ranges
+ range1_r.L_corrected = (sat1_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+ range2_r.L_corrected = (sat2_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+ range3_r.L_corrected = (sat3_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+ range4_r.L_corrected = (sat4_r.pos-t_ecef_antena_r).norm() + clock_drift_r(0);
+ range1_k.L_corrected = (sat1_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+ range2_k.L_corrected = (sat2_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+ range3_k.L_corrected = (sat3_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+ range4_k.L_corrected = (sat4_k.pos-t_ecef_antena_k).norm() + clock_drift_k(0);
+}
+
+void setUpProblem()
+{
+ // remove previous setup
+ if (frm_r)
+ frm_r->remove();
+ if (frm_k)
+ frm_k->remove();
+
+ solver->getSolverOptions().max_num_iterations = 100;
+
+ // Sensor
+ // ENU-MAP
+ gnss_sensor->setEnuMapTranslationState(t_enu_map);
+ gnss_sensor->setEnuMapRollState(rpy_enu_map(0));
+ gnss_sensor->setEnuMapPitchState(rpy_enu_map(1));
+ gnss_sensor->setEnuMapYawState(rpy_enu_map(2));
+ // ECEF-ENU
+ gnss_sensor->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
+ // Extrinsics
+ gnss_sensor->getP()->setState(t_base_antena);
+
+ // Frame r
+ Vector7d frm_r_state;
+ frm_r_state.head<3>() = t_map_base_r;
+ frm_r_state.tail<4>() = q_map_base_r.coeffs();
+ frm_r = prb->emplaceFrame( TimeStamp(0), frm_r_state);
+
+ // Frame k
+ Vector7d frm_k_state;
+ frm_k_state.head<3>() = t_map_base_k;
+ frm_k_state.tail<4>() = q_map_base_k.coeffs();
+ frm_k = prb->emplaceFrame( TimeStamp(1), frm_k_state);
+
+ // capture r
+ cap_r = CaptureBase::emplace<CaptureGnss>(frm_r, TimeStamp(0), gnss_sensor, nullptr);
+ cap_r->getStateBlock('T')->unfix();
+ cap_r->getStateBlock('T')->setState(clock_drift_r);
+
+ // capture k
+ cap_k = CaptureBase::emplace<CaptureGnss>(frm_k, TimeStamp(1), gnss_sensor, nullptr);
+ cap_k->getStateBlock('T')->unfix();
+ cap_k->getStateBlock('T')->setState(clock_drift_k);
+
+ // features r
+ ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obsd_t(), sat1_r, range1_r);
+ ftr2_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obsd_t(), sat2_r, range2_r);
+ ftr3_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obsd_t(), sat3_r, range3_r);
+ ftr4_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obsd_t(), sat4_r, range4_r);
+
+ // features k
+ ftr1_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obsd_t(), sat1_k, range1_k);
+ ftr2_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obsd_t(), sat2_k, range2_k);
+ ftr3_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obsd_t(), sat3_k, range3_k);
+ ftr4_k = FeatureBase::emplace<FeatureGnssSatellite>(cap_k, obsd_t(), sat4_k, range4_k);
+
+ // factors
+ fac1 = FactorBase::emplace<FactorGnssTdcp>(ftr1_r, 0.1, ftr1_r, ftr1_k, gnss_sensor, nullptr, false);
+ fac2 = FactorBase::emplace<FactorGnssTdcp>(ftr2_r, 0.1, ftr2_r, ftr2_k, gnss_sensor, nullptr, false);
+ fac3 = FactorBase::emplace<FactorGnssTdcp>(ftr3_r, 0.1, ftr3_r, ftr3_k, gnss_sensor, nullptr, false);
+ fac4 = FactorBase::emplace<FactorGnssTdcp>(ftr4_r, 0.1, ftr4_r, ftr4_k, gnss_sensor, nullptr, false);
+
+ // ASSERTS
+ // ENU-MAP
+ ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapRoll()->getState()(0), rpy_enu_map(0));
+ ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapPitch()->getState()(0), rpy_enu_map(1));
+ ASSERT_FLOAT_EQ(gnss_sensor->getEnuMapYaw()->getState()(0), rpy_enu_map(2));
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-3);
+ // Antena
+ ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena, 1e-3);
+ // Frame r
+ ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-3);
+ ASSERT_MATRIX_APPROX(frm_r->getO()->getState(), q_map_base_r.coeffs(), 1e-3);
+ // Frame k
+ ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-3);
+ ASSERT_MATRIX_APPROX(frm_k->getO()->getState(), q_map_base_k.coeffs(), 1e-3);
+ // clock drift
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), 1e-3);
+}
+
+void fixAllStates()
+{
+ // ENU-MAP
+ gnss_sensor->getEnuMapRoll()->fix();
+ gnss_sensor->getEnuMapPitch()->fix();
+ gnss_sensor->getEnuMapYaw()->fix();
+ gnss_sensor->getEnuMapTranslation()->fix();
+ // Antena
+ gnss_sensor->getP()->fix();
+ // Frames
+ frm_r->fix();
+ frm_k->fix();
+ // clock drift
+ cap_r->fix();
+ cap_k->fix();
+ }
+
+////////////////////////////////////////////////////////
+TEST(FactorGnssTdcpTest, observe_clock_drift_r)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ cap_r->getStateBlock('T')->unfix();
+
+ // perturb
+ cap_r->getStateBlock('T')->perturb(1e2);
+
+ // Only 1 factor
+ fac2->setStatus(FAC_INACTIVE);
+ fac3->setStatus(FAC_INACTIVE);
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), 1e-3);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_clock_drift_k)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ cap_k->getStateBlock('T')->unfix();
+
+ // perturb
+ cap_k->getStateBlock('T')->perturb(1e2);
+
+ // Only 1 factor
+ fac2->setStatus(FAC_INACTIVE);
+ fac3->setStatus(FAC_INACTIVE);
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), 1e-3);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_frame_p_r)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ frm_r->getP()->unfix();
+
+ // perturb
+ frm_r->getP()->perturb(1);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-3);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_frame_p_k)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ frm_k->getP()->unfix();
+
+ // perturb
+ frm_k->getP()->perturb(1);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-3);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_frame_p_clock_k)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ frm_k->getP()->unfix();
+ cap_k->getStateBlock('T')->unfix();
+
+ // perturb
+ frm_k->getP()->perturb(1);
+ cap_k->getStateBlock('T')->perturb(1e2);
+
+ // all 4 factors
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), 1e-3);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_frame_p_clock_r)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ frm_r->getP()->unfix();
+ cap_r->getStateBlock('T')->unfix();
+
+ // perturb
+ frm_r->getP()->perturb(1);
+ cap_r->getStateBlock('T')->perturb(1e2);
+
+ // all 4 factors
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), 1e-3);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_enumap_p)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ gnss_sensor->getEnuMapTranslation()->unfix();
+
+ // perturb
+ gnss_sensor->getEnuMapTranslation()->perturb(1e2);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-3);
+ }
+}
+
+TEST(FactorGnssTdcpTest, observe_enumap_o)
+{
+ for (auto i = 0; i < 100; i++)
+ {
+ // setup random problem
+ randomGroundtruth();
+ setUpProblem();
+
+ // fix/unfix
+ fixAllStates();
+ gnss_sensor->getEnuMapRoll()->unfix();
+ gnss_sensor->getEnuMapPitch()->unfix();
+ gnss_sensor->getEnuMapYaw()->unfix();
+
+ // perturb
+ gnss_sensor->getEnuMapRoll()->perturb(1);
+ gnss_sensor->getEnuMapPitch()->perturb(1);
+ gnss_sensor->getEnuMapYaw()->perturb(1);
+
+ // Only 3 factors
+ fac4->setStatus(FAC_INACTIVE);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, 1e-3);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
--
GitLab