Merge branch 'devel' into 'main'

After cmake and const refactor

See merge request !35

.gitignore

@@ -2,9 +2,11 @@
 .settings/language.settings.xml
 .project
 .cproject
-build/
 bin/
+build*/
 lib/
 gnss.found
 .vscode
 .vscode/c_cpp_properties.json
+/CMakeCache.txt
+/CMakeFiles/cmake.check_cache

.gitlab-ci.yml

@@ -3,6 +3,13 @@ stages:
   - build_and_test
 
 ############ YAML ANCHORS ############
+.print_variables_template: &print_variables_definition
+  # Print variables
+  - echo $WOLF_CORE_BRANCH
+  - echo $CI_COMMIT_BRANCH
+  - echo $WOLF_GNSS_BRANCH
+  - echo $GNSSUTILS_BRANCH
+
 .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)
@@ -85,9 +92,12 @@ stages:
   - if [ -d gnss_utils ]; then
   -   echo "directory gnss_utils exists"
   -   cd gnss_utils
+  -   git checkout main
+  -   git pull
+  -   git checkout ${GNSSUTILS_BRANCH}
   -   git pull
   - else
-  -   git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils.git
+  -   git clone -b ${GNSSUTILS_BRANCH} ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/gauss_project/gnss_utils.git
   -   cd gnss_utils
   -   git submodule update --init
   - fi
@@ -116,6 +126,7 @@ license_headers:
       paths:
       - ci_deps/wolf/
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
   script:
@@ -133,6 +144,7 @@ build_and_test:bionic:
       paths:
       - ci_deps/gnss_utils/
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_gnssutils_definition
@@ -152,6 +164,7 @@ build_and_test:focal:
       paths:
       - ci_deps/gnss_utils/
   before_script:
+    - *print_variables_definition
     - *preliminaries_definition
     - *install_wolf_definition
     - *install_gnssutils_definition

CMakeLists.txt

 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
 
 if(COMMAND cmake_policy)
   cmake_policy(SET CMP0005 NEW)
@@ -16,7 +16,8 @@ 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)
+set(INCLUDE_INSTALL_DIR include/iri-algorithms/wolf)
+set(LIB_INSTALL_DIR lib/)
 
 IF (NOT CMAKE_BUILD_TYPE)
   SET(CMAKE_BUILD_TYPE "DEBUG")
@@ -67,10 +68,6 @@ if(BUILD_TESTS)
     enable_testing()
 endif()
 
-#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)
@@ -79,12 +76,13 @@ ENDIF()
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
 # ============ DEPENDENCIES ============ 
-FIND_PACKAGE(wolfcore REQUIRED)
+FIND_PACKAGE(wolfcore REQUIRED CONFIG)
 FIND_PACKAGE(gnss_utils REQUIRED)
 
 # ============ CONFIG.H ============ 
-string(TOUPPER ${PROJECT_NAME} UPPER_NAME)
 set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+# variable used to compile the config.h.in file
+string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
 
 # Define the directory where will be the configured config.h
 SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal)
@@ -104,8 +102,6 @@ message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}")
 include_directories("${PROJECT_BINARY_DIR}/conf")
 
 # ============ INCLUDES ============ 
-INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(${gnss_utils_INCLUDE_DIRS})
 INCLUDE_DIRECTORIES(BEFORE "include")
 
 # ============ HEADERS ============ 
@@ -118,7 +114,6 @@ SET(HDRS_FACTOR
   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
@@ -180,8 +175,8 @@ endif()
 
 #Link the created libraries
 #===============EXAMPLE=========================
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES})
-TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${gnss_utils_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} wolfcore)
+TARGET_LINK_LIBRARIES(${PLUGIN_NAME} gnss_utils)
 
 #Build tests
 #===============EXAMPLE=========================
@@ -193,38 +188,56 @@ ENDIF(BUILD_TESTS)
 #install library
 #=============================================================
 INSTALL(TARGETS ${PLUGIN_NAME} EXPORT ${PLUGIN_NAME}Targets
-      RUNTIME DESTINATION bin
-      LIBRARY DESTINATION lib
-      ARCHIVE DESTINATION lib)
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION ${LIB_INSTALL_DIR}
+  ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
+)
+install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/${PLUGIN_NAME}/cmake)
+
+# Configure the package installation
+include(CMakePackageConfigHelpers)
+configure_package_config_file(
+  ${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake.in
+  ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+  INSTALL_DESTINATION ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+  PATH_VARS INCLUDE_INSTALL_DIR LIB_INSTALL_DIR
+)
+
+install(
+  FILES 
+  ${CMAKE_CURRENT_BINARY_DIR}/${PLUGIN_NAME}Config.cmake
+  DESTINATION 
+  ${LIB_INSTALL_DIR}/${PLUGIN_NAME}/cmake
+)
+
+# Specifies include directories to use when compiling the plugin target
+# This way, include_directories does not need to be called in plugins depending on this one
+target_include_directories(${PLUGIN_NAME} INTERFACE
+  $<INSTALL_INTERFACE:${INCLUDE_INSTALL_DIR}>
+)
 
-install(EXPORT ${PLUGIN_NAME}Targets DESTINATION lib/cmake/${PLUGIN_NAME})
 #install headers
 INSTALL(FILES ${HDRS_CAPTURE}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/capture)
 INSTALL(FILES ${HDRS_FACTOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/factor)
 INSTALL(FILES ${HDRS_FEATURE}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/feature)
 INSTALL(FILES ${HDRS_PROCESSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/processor)
 INSTALL(FILES ${HDRS_SENSOR}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/sensor)
 INSTALL(FILES ${HDRS_TREE_MANAGER}
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/tree_manager)
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/tree_manager)
 
-FILE(WRITE ${PROJECT_NAME}.found "")
-INSTALL(FILES ${PROJECT_NAME}.found
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME})
 INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
-  DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal)
-
-INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
+  DESTINATION ${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/internal)
 
 INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
 
 export(PACKAGE ${PLUGIN_NAME})
 
-FIND_PACKAGE(Doxygen)
+FIND_PACKAGE(Doxygen MODULE)
 
 FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
 IF (IRI_DOC_DIR)

cmake_modules/wolfgnssConfig.cmake

-#edit the following line to add the librarie's header files
-FIND_PATH(
-    wolfgnss_INCLUDE_DIRS
-    NAMES gnss.found
-    PATHS /usr/local/include/iri-algorithms/wolf/plugin_gnss)
-IF(wolfgnss_INCLUDE_DIRS)
-  MESSAGE("Found gnss include dirs: ${wolfgnss_INCLUDE_DIRS}")
-ELSE(wolfgnss_INCLUDE_DIRS)
-  MESSAGE("Couldn't find gnss include dirs")
-ENDIF(wolfgnss_INCLUDE_DIRS)
-
-FIND_LIBRARY(
-    wolfgnss_LIBRARIES
-    NAMES libwolfgnss.so
-    PATHS /usr/local/lib)
-IF(wolfgnss_LIBRARIES)
-  MESSAGE("Found gnss lib: ${wolfgnss_LIBRARIES}")
-ELSE(wolfgnss_LIBRARIES)
-  MESSAGE("Couldn't find wolf gnss lib")
-ENDIF(wolfgnss_LIBRARIES)
-
-IF (wolfgnss_INCLUDE_DIRS AND wolfgnss_LIBRARIES)
-   SET(wolfgnss_FOUND TRUE)
- ELSE(wolfgnss_INCLUDE_DIRS AND wolfgnss_LIBRARIES)
-   set(wolfgnss_FOUND FALSE)
-ENDIF (wolfgnss_INCLUDE_DIRS AND wolfgnss_LIBRARIES)
-
-IF (wolfgnss_FOUND)
-   IF (NOT wolfgnss_FIND_QUIETLY)
-      MESSAGE(STATUS "Found gnss: ${wolfgnss_LIBRARIES}")
-   ENDIF (NOT wolfgnss_FIND_QUIETLY)
-ELSE (wolfgnss_FOUND)
-   IF (wolfgnss_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find wolf gnss")
-   ENDIF (wolfgnss_FIND_REQUIRED)
-ENDIF (wolfgnss_FOUND)
-
-
-macro(wolf_report_not_found REASON_MSG)
-  set(wolfgnss_FOUND FALSE)
-  unset(wolfgnss_INCLUDE_DIRS)
-  unset(wolfgnss_LIBRARIES)
-
-  # Reset the CMake module path to its state when this script was called.
-  set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
-
-  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
-  # FindPackage() use the camelcase library name, not uppercase.
-  if (wolfgnss_FIND_QUIETLY)
-    message(STATUS "Failed to find wolf gnss- " ${REASON_MSG} ${ARGN})
-  elseif(wolfgnss_FIND_REQUIRED)
-    message(FATAL_ERROR "Failed to find wolf gnss - " ${REASON_MSG} ${ARGN})
-  else()
-    # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
-    # that prevents generation, but continues configuration.
-    message(SEND_ERROR "Failed to find wolf gnss - " ${REASON_MSG} ${ARGN})
-  endif ()
-  return()
-endmacro(wolf_report_not_found)
-
-if(NOT wolfgnss_FOUND)
-  wolf_report_not_found("Something went wrong while setting up wolf gnss.")
-endif(NOT wolfgnss_FOUND)
-# Set the include directories for wolf (itself).
-set(wolfgnss_FOUND TRUE)
-
-# Now we gather all the required dependencies for Wolf Laser
-
-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(wolfcore REQUIRED)
-
-  #We reverse in order to insert at the start
-  list(REVERSE wolfgnss_INCLUDE_DIRS)
-  list(APPEND wolfgnss_INCLUDE_DIRS ${wolfcore_INCLUDE_DIRS})
-  list(REVERSE wolfgnss_INCLUDE_DIRS)
-
-  list(REVERSE wolfgnss_LIBRARIES)
-  list(APPEND wolfgnss_LIBRARIES ${wolfcore_LIBRARIES})
-  list(REVERSE wolfgnss_LIBRARIES)
-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

cmake_modules/wolfgnssConfig.cmake.in

+set(@PLUGIN_NAME@_VERSION 0.0.1)
+
+
+@PACKAGE_INIT@
+
+set_and_check(@PLUGIN_NAME@_INCLUDE_DIR "@PACKAGE_INCLUDE_INSTALL_DIR@")
+set(@PLUGIN_NAME@_INCLUDE_DIRS @PLUGIN_NAME@_INCLUDE_DIR)
+set_and_check(@PLUGIN_NAME@_LIB_INSTALL_DIR "@PACKAGE_LIB_INSTALL_DIR@")
+set(@PLUGIN_NAME@_LIB_INSTALL_DIRS @PLUGIN_NAME@_LIB_INSTALL_DIR)
+
+# forwards the correct parameters given to FIND_DEPENDENCIES
+include(CMakeFindDependencyMacro)
+FIND_DEPENDENCY(gnss_utils REQUIRED)
+
+include("${CMAKE_CURRENT_LIST_DIR}/@PLUGIN_NAME@Targets.cmake")
+
+check_required_components(@PLUGIN_NAME@)
\ No newline at end of file

include/gnss/capture/capture_gnss.h

@@ -43,25 +43,43 @@ class CaptureGnss : public CaptureBase
       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();
+      GnssUtils::SnapshotConstPtr getSnapshot() const;
+      GnssUtils::SnapshotPtr getSnapshot();
+      GnssUtils::ObservationsConstPtr getObservations() const;
+      GnssUtils::ObservationsPtr getObservations();
+      GnssUtils::NavigationConstPtr getNavigation() const;
+      GnssUtils::NavigationPtr getNavigation();
       const GnssUtils::Satellites& getSatellites() const;
+      GnssUtils::Satellites& getSatellites();
 
 };
 
-inline GnssUtils::SnapshotPtr CaptureGnss::getSnapshot() const
+inline GnssUtils::SnapshotConstPtr CaptureGnss::getSnapshot() const
 {
   return snapshot_;
 }
 
-inline GnssUtils::ObservationsPtr CaptureGnss::getObservations() const
+inline GnssUtils::SnapshotPtr CaptureGnss::getSnapshot()
+{
+  return snapshot_;
+}
+
+inline GnssUtils::ObservationsConstPtr CaptureGnss::getObservations() const
+{
+  return snapshot_->getObservations();
+}
+
+inline GnssUtils::ObservationsPtr CaptureGnss::getObservations()
 {
   return snapshot_->getObservations();
 }
 
-inline GnssUtils::NavigationPtr CaptureGnss::getNavigation() const
+inline GnssUtils::NavigationConstPtr CaptureGnss::getNavigation() const
+{
+  return snapshot_->getNavigation();
+}
+
+inline GnssUtils::NavigationPtr CaptureGnss::getNavigation()
 {
   return snapshot_->getNavigation();
 }

include/gnss/capture/capture_gnss_tdcp.h

@@ -56,7 +56,8 @@ class CaptureGnssTdcp : public CaptureBase
         const Eigen::Vector3d& getData() const;
         const Eigen::Matrix3d& getDataCovariance() const;
         void getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const;
-        FrameBasePtr getOriginFrame() const;
+        FrameBaseConstPtr getOriginFrame() const;
+        FrameBasePtr getOriginFrame();
         const TimeStamp& getGpsTimeStamp() const;
         void setGpsTimeStamp(const TimeStamp &_ts_GPST);
 };
@@ -77,12 +78,17 @@ inline void CaptureGnssTdcp::getDataAndCovariance(Eigen::Vector3d& data, Eigen::
     data_cov = data_covariance_;
 }
 
-inline FrameBasePtr CaptureGnssTdcp::getOriginFrame() const
+inline FrameBaseConstPtr CaptureGnssTdcp::getOriginFrame() const
 {
     return origin_frame_ptr_;
 }
 
-inline const wolf::TimeStamp& CaptureGnssTdcp::getGpsTimeStamp() const
+inline FrameBasePtr CaptureGnssTdcp::getOriginFrame()
+{
+    return origin_frame_ptr_;
+}
+
+inline const TimeStamp& CaptureGnssTdcp::getGpsTimeStamp() const
 {
     return ts_GPST_;
 }

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--------
-
-#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

include/gnss/processor/processor_gnss_tdcp.h

@@ -96,6 +96,7 @@ class ProcessorGnssTdcp : public ProcessorBase
 
         WOLF_PROCESSOR_CREATE(ProcessorGnssTdcp, ParamsProcessorGnssTdcp);
 
+        FrameBaseConstPtr getLastKF() const;
         FrameBasePtr getLastKF();
 
     protected:
@@ -144,7 +145,12 @@ class ProcessorGnssTdcp : public ProcessorBase
 
 };
 
-inline wolf::FrameBasePtr ProcessorGnssTdcp::getLastKF()
+inline FrameBaseConstPtr ProcessorGnssTdcp::getLastKF() const
+{
+    return last_KF_;
+}
+
+inline FrameBasePtr ProcessorGnssTdcp::getLastKF()
 {
     return last_KF_;
 }

include/gnss/sensor/sensor_gnss.h

@@ -110,10 +110,14 @@ class SensorGnss : public SensorBase
         ~SensorGnss() override;
 
         // Gets
-        StateBlockPtr getEnuMapTranslation() const;
-        StateBlockPtr getEnuMapRoll() const;
-        StateBlockPtr getEnuMapPitch() const;
-        StateBlockPtr getEnuMapYaw() const;
+        StateBlockConstPtr getEnuMapTranslation() const;
+        StateBlockPtr getEnuMapTranslation();
+        StateBlockConstPtr getEnuMapRoll() const;
+        StateBlockPtr getEnuMapRoll();
+        StateBlockConstPtr getEnuMapPitch() const;
+        StateBlockPtr getEnuMapPitch();
+        StateBlockConstPtr getEnuMapYaw() const;
+        StateBlockPtr getEnuMapYaw();
         const Eigen::Matrix3d& getREnuEcef() const;
         const Eigen::Vector3d& gettEnuEcef() const;
         Eigen::Matrix3d getREnuMap() const;
@@ -178,22 +182,42 @@ inline bool SensorGnss::isEnuModeAuto() const
     return params_->ENU_mode == "auto";
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
+inline StateBlockConstPtr SensorGnss::getEnuMapTranslation() const
 {
     return getStateBlock('t');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapRoll() const
+inline StateBlockPtr SensorGnss::getEnuMapTranslation()
+{
+    return getStateBlock('t');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapRoll() const
+{
+    return getStateBlock('r');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapRoll()
 {
     return getStateBlock('r');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapPitch() const
+inline StateBlockConstPtr SensorGnss::getEnuMapPitch() const
 {
     return getStateBlock('p');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapYaw() const
+inline StateBlockPtr SensorGnss::getEnuMapPitch()
+{
+    return getStateBlock('p');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapYaw() const
+{
+    return getStateBlock('y');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapYaw()
 {
     return getStateBlock('y');
 }

internal/config.h.in

@@ -24,13 +24,13 @@