Commit eaf0f864 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Merge branch 'devel' into 'main'

After cmake and const refactor

See merge request !35
parents 881730fb a90d683d
......@@ -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
......@@ -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
......
# 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)
......
#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
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
......@@ -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();
}
......
......@@ -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_;
}
......
//--------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
......@@ -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_;
}
......
......@@ -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');
}
......