diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index a47a0acd2c5f81ed46bf9f9f3ec642b750abfe97..3696d52a048db65e2b09cd3a4a4aa42db9313b29 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,4 +1,69 @@
+stages:
+ - license
+ - build_and_test
+ - deploy
+ - final
+
+############ 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
+
+ # update apt
+ - apt-get update
+
+.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}
+ - cd wolf_scripts
+ - 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..."
+ - ./license_manager.sh --add --path=${CI_PROJECT_DIR} --license-header=license_header_${CURRENT_YEAR}.txt
+ - 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
+ - ./license_manager.sh --update --path=${CI_PROJECT_DIR} --license-header=license_header_${CURRENT_YEAR}.txt
+ - fi
+ - cd ..
+
+ # 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 "No changes, nothing to commit!"
+ - fi
+
.build_and_test_template: &build_and_test_definition
+ - cd $CI_PROJECT_DIR
- mkdir -pv build
- cd build
- cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
@@ -9,16 +74,93 @@
- ../bin/hello_wolf_autoconf
- make install
+############ LICENSE HEADERS ############
+license_header:
+ stage: license
+ image: labrobotica/wolf_deps:16.04
+ before_script:
+ - *preliminaries_definition
+ script:
+ - *license_header_definition
+
+############ UBUNTU 16.04 TESTS ############
build_and_test:xenial:
+ stage: build_and_test
image: labrobotica/wolf_deps:16.04
except:
- master
script:
- *build_and_test_definition
+############ UBUNTU 18.04 TESTS ############
build_and_test:bionic:
+ stage: build_and_test
image: labrobotica/wolf_deps:18.04
except:
- master
script:
- - *build_and_test_definition
\ No newline at end of file
+ - *build_and_test_definition
+
+############ UBUNTU 20.04 TESTS ############
+build_and_test:focal:
+ stage: build_and_test
+ image: labrobotica/wolf_deps:20.04
+ except:
+ - master
+ script:
+ - *build_and_test_definition
+
+############ DEPLOY PLUGINS ############
+deploy_imu:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/imu
+ strategy: depend
+
+deploy_gnss:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
+ strategy: depend
+
+deploy_vision:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/vision
+ strategy: depend
+
+deploy_laser:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/laser
+ strategy: depend
+
+deploy_apriltag:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag
+ strategy: depend
+
+deploy_bodydynamics:
+ stage: deploy
+ variables:
+ WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
+ trigger:
+ project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
+ strategy: depend
+
+############ WAIT FOR PLUGINS ############
+final_all:
+ stage: final
+ script:
+ - echo "ALL PLUGINS PIPELINES SUCCEED!!!"
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7780c9a0f13d8ee17aa84dcb6fb1c76f1912d2ac..59fa36ee80881e3c5e21a83c93673be4d9cf32cb 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,7 +1,8 @@
-# Pre-requisites about cmake itself
#Start WOLF build
MESSAGE("Starting WOLF CMakeLists ...")
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+
+# Pre-requisites about cmake itself
+CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
@@ -11,7 +12,6 @@ endif(COMMAND cmake_policy)
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
-
# The project name
PROJECT(core)
set(PLUGIN_NAME "wolf${PROJECT_NAME}")
@@ -32,18 +32,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)
@@ -84,36 +80,21 @@ 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_DEMOS OR BUILD_TESTS)
- set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
-ENDIF(BUILD_DEMOS OR BUILD_TESTS)
-
-
-#START_SRC --------------------------------------------------------------------------------------------------------------------------------
-
#CMAKE modules
-
SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules")
MESSAGE(STATUS "Cmake modules at: " ${CMAKE_MODULE_PATH})
-
-#find dependencies.
-
+# ============ DEPENDENCIES ============
FIND_PACKAGE(Threads REQUIRED)
-
-FIND_PACKAGE(Ceres REQUIRED) #Ceres is required
-
+FIND_PACKAGE(Ceres REQUIRED)
FIND_PACKAGE(Eigen3 3.3 REQUIRED)
if(${EIGEN3_VERSION_STRING} VERSION_LESS 3.3)
message(FATAL_ERROR "Wolf requires Eigen >= 3.3. Found Eigen ${EIGEN3_VERSION_STRING}")
endif()
-
-# YAML with yaml-cpp
FIND_PACKAGE(YamlCpp REQUIRED)
+# ============ config.h ============
+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/core/internal)
@@ -141,72 +122,12 @@ ELSE (SPDLOG_INCLUDE_DIR)
MESSAGE(FATAL_ERROR "Could not find spdlog")
ENDIF (SPDLOG_INCLUDE_DIR)
-
-# Includes
+# ============ INCLUDES ============
INCLUDE_DIRECTORIES("include") # In this same project
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(${YAMLCPP_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
-
-#HEADERS
-
-SET(HDRS_COMMON
- include/core/common/factory.h
- include/core/common/node_base.h
- include/core/common/time_stamp.h
- include/core/common/wolf.h
- include/core/common/params_base.h
- )
-SET(HDRS_MATH
- include/core/math/SE2.h
- include/core/math/SE3.h
- include/core/math/rotations.h
- include/core/math/covariance.h
- )
-SET(HDRS_UTILS
- include/core/utils/check_log.h
- include/core/utils/converter.h
- include/core/utils/eigen_assert.h
- include/core/utils/eigen_predicates.h
- include/core/utils/graph_search.h
- include/core/utils/loader.h
- include/core/utils/logging.h
- include/core/utils/make_unique.h
- include/core/utils/params_server.h
- include/core/utils/singleton.h
- include/core/utils/utils_gtest.h
- include/core/utils/converter_utils.h
- )
-SET(HDRS_PROBLEM
- include/core/problem/problem.h
- )
-SET(HDRS_HARDWARE
- include/core/hardware/hardware_base.h
- )
-SET(HDRS_TRAJECTORY
- include/core/trajectory/trajectory_base.h
- )
-SET(HDRS_MAP
- include/core/map/factory_map.h
- include/core/map/map_base.h
- )
-SET(HDRS_FRAME
- include/core/frame/frame_base.h
- )
-SET(HDRS_STATE_BLOCK
- include/core/state_block/factory_state_block.h
- include/core/state_block/has_state_blocks.h
- include/core/state_block/local_parametrization_angle.h
- include/core/state_block/local_parametrization_base.h
- include/core/state_block/local_parametrization_homogeneous.h
- include/core/state_block/local_parametrization_quaternion.h
- include/core/state_block/state_angle.h
- include/core/state_block/state_block.h
- include/core/state_block/state_composite.h
- include/core/state_block/state_homogeneous_3d.h
- include/core/state_block/state_quaternion.h
- )
-
+# ============ HEADERS ============
SET(HDRS_CAPTURE
include/core/capture/capture_base.h
include/core/capture/capture_motion.h
@@ -216,6 +137,13 @@ SET(HDRS_CAPTURE
include/core/capture/capture_void.h
include/core/capture/capture_diff_drive.h
)
+SET(HDRS_COMMON
+ include/core/common/factory.h
+ include/core/common/node_base.h
+ include/core/common/time_stamp.h
+ include/core/common/wolf.h
+ include/core/common/params_base.h
+ )
SET(HDRS_FACTOR
include/core/factor/factor_analytic.h
include/core/factor/factor_autodiff.h
@@ -235,23 +163,42 @@ SET(HDRS_FACTOR
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
+ include/core/feature/feature_diff_drive.h
include/core/feature/feature_match.h
include/core/feature/feature_motion.h
include/core/feature/feature_odom_2d.h
include/core/feature/feature_pose.h
)
+SET(HDRS_FRAME
+ include/core/frame/frame_base.h
+ )
+SET(HDRS_HARDWARE
+ include/core/hardware/hardware_base.h
+ )
SET(HDRS_LANDMARK
include/core/landmark/landmark_base.h
include/core/landmark/landmark_match.h
)
+SET(HDRS_MATH
+ include/core/math/SE2.h
+ include/core/math/SE3.h
+ include/core/math/rotations.h
+ include/core/math/covariance.h
+ )
+SET(HDRS_MAP
+ include/core/map/factory_map.h
+ include/core/map/map_base.h
+ )
+SET(HDRS_PROBLEM
+ include/core/problem/problem.h
+ )
SET(HDRS_PROCESSOR
- include/core/processor/is_motion.h
include/core/processor/motion_buffer.h
+ include/core/processor/motion_provider.h
include/core/processor/processor_base.h
include/core/processor/processor_diff_drive.h
- include/core/processor/processor_fix_wing_model.h
+ include/core/processor/processor_fixed_wing_model.h
include/core/processor/factory_processor.h
- include/core/processor/processor_logging.h
include/core/processor/processor_loop_closure.h
include/core/processor/processor_motion.h
include/core/processor/processor_odom_2d.h
@@ -266,7 +213,7 @@ SET(HDRS_SENSOR
include/core/sensor/sensor_base.h
include/core/sensor/sensor_diff_drive.h
include/core/sensor/factory_sensor.h
- include/core/sensor/sensor_model.h
+ include/core/sensor/sensor_motion_model.h
include/core/sensor/sensor_odom_2d.h
include/core/sensor/sensor_odom_3d.h
include/core/sensor/sensor_pose.h
@@ -275,54 +222,46 @@ SET(HDRS_SOLVER
include/core/solver/solver_manager.h
include/core/solver/factory_solver.h
)
+SET(HDRS_STATE_BLOCK
+ include/core/state_block/factory_state_block.h
+ include/core/state_block/has_state_blocks.h
+ include/core/state_block/local_parametrization_angle.h
+ include/core/state_block/local_parametrization_base.h
+ include/core/state_block/local_parametrization_homogeneous.h
+ include/core/state_block/local_parametrization_quaternion.h
+ include/core/state_block/state_angle.h
+ include/core/state_block/state_block.h
+ include/core/state_block/state_composite.h
+ include/core/state_block/state_homogeneous_3d.h
+ include/core/state_block/state_quaternion.h
+ )
+SET(HDRS_TRAJECTORY
+ include/core/trajectory/trajectory_base.h
+ )
SET(HDRS_TREE_MANAGER
include/core/tree_manager/factory_tree_manager.h
include/core/tree_manager/tree_manager_base.h
include/core/tree_manager/tree_manager_sliding_window.h
include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
)
+SET(HDRS_UTILS
+ include/core/utils/check_log.h
+ include/core/utils/converter.h
+ include/core/utils/eigen_assert.h
+ include/core/utils/graph_search.h
+ include/core/utils/loader.h
+ include/core/utils/logging.h
+ include/core/utils/params_server.h
+ include/core/utils/singleton.h
+ include/core/utils/utils_gtest.h
+ include/core/utils/converter_utils.h
+ )
SET(HDRS_YAML
include/core/yaml/parser_yaml.h
include/core/yaml/yaml_conversion.h
)
-#SOURCES
-SET(SRCS_PROBLEM
- src/problem/problem.cpp
- )
-SET(SRCS_HARDWARE
- src/hardware/hardware_base.cpp
- )
-SET(SRCS_TRAJECTORY
- src/trajectory/trajectory_base.cpp
- )
-SET(SRCS_MAP
- src/map/map_base.cpp
- )
-SET(SRCS_FRAME
- src/frame/frame_base.cpp
- )
-SET(SRCS_STATE_BLOCK
- src/state_block/has_state_blocks.cpp
- src/state_block/local_parametrization_base.cpp
- src/state_block/local_parametrization_homogeneous.cpp
- src/state_block/local_parametrization_quaternion.cpp
- src/state_block/state_block.cpp
- src/state_block/state_composite.cpp
- )
-SET(SRCS_COMMON
- src/common/node_base.cpp
- src/common/time_stamp.cpp
- )
-SET(SRCS_MATH
- )
-SET(SRCS_UTILS
- src/utils/check_log.cpp
- src/utils/converter_utils.cpp
- src/utils/graph_search.cpp
- src/utils/loader.cpp
- src/utils/params_server.cpp
- )
-
+
+# ============ SOURCES ============
SET(SRCS_CAPTURE
src/capture/capture_base.cpp
src/capture/capture_motion.cpp
@@ -332,25 +271,42 @@ SET(SRCS_CAPTURE
src/capture/capture_void.cpp
src/capture/capture_diff_drive.cpp
)
+SET(SRCS_COMMON
+ src/common/node_base.cpp
+ src/common/time_stamp.cpp
+ )
SET(SRCS_FACTOR
src/factor/factor_analytic.cpp
src/factor/factor_base.cpp
)
SET(SRCS_FEATURE
src/feature/feature_base.cpp
+ src/feature/feature_diff_drive.cpp
src/feature/feature_motion.cpp
src/feature/feature_odom_2d.cpp
src/feature/feature_pose.cpp
)
+SET(SRCS_FRAME
+ src/frame/frame_base.cpp
+ )
+SET(SRCS_HARDWARE
+ src/hardware/hardware_base.cpp
+ )
SET(SRCS_LANDMARK
src/landmark/landmark_base.cpp
)
+SET(SRCS_MAP
+ src/map/map_base.cpp
+ )
+SET(SRCS_PROBLEM
+ src/problem/problem.cpp
+ )
SET(SRCS_PROCESSOR
- src/processor/is_motion.cpp
src/processor/motion_buffer.cpp
+ src/processor/motion_provider.cpp
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
- src/processor/processor_fix_wing_model.cpp
+ src/processor/processor_fixed_wing_model.cpp
src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
@@ -364,7 +320,7 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
- src/sensor/sensor_model.cpp
+ src/sensor/sensor_motion_model.cpp
src/sensor/sensor_odom_2d.cpp
src/sensor/sensor_odom_3d.cpp
src/sensor/sensor_pose.cpp
@@ -372,10 +328,28 @@ SET(SRCS_SENSOR
SET(SRCS_SOLVER
src/solver/solver_manager.cpp
)
+SET(SRCS_STATE_BLOCK
+ src/state_block/has_state_blocks.cpp
+ src/state_block/local_parametrization_base.cpp
+ src/state_block/local_parametrization_homogeneous.cpp
+ src/state_block/local_parametrization_quaternion.cpp
+ src/state_block/state_block.cpp
+ src/state_block/state_composite.cpp
+ )
+SET(SRCS_TRAJECTORY
+ src/trajectory/trajectory_base.cpp
+ )
SET(SRCS_TREE_MANAGER
src/tree_manager/tree_manager_sliding_window.cpp
src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
)
+SET(SRCS_UTILS
+ src/utils/check_log.cpp
+ src/utils/converter_utils.cpp
+ src/utils/graph_search.cpp
+ src/utils/loader.cpp
+ src/utils/params_server.cpp
+ )
SET(SRCS_YAML
src/yaml/parser_yaml.cpp
src/yaml/processor_odom_3d_yaml.cpp
@@ -383,10 +357,10 @@ SET(SRCS_YAML
src/yaml/sensor_odom_3d_yaml.cpp
src/yaml/sensor_pose_yaml.cpp
)
-#OPTIONALS
-#optional HDRS and SRCS
+
+# ============ OPTIONALS ============
IF (Ceres_FOUND)
- SET(HDRS_WRAPPER
+ SET(HDRS_CERES_WRAPPER
#ceres_wrapper/qr_manager.h
include/core/ceres_wrapper/cost_function_wrapper.h
include/core/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -396,32 +370,26 @@ IF (Ceres_FOUND)
include/core/solver/solver_manager.h
include/core/solver_suitesparse/sparse_utils.h
)
- SET(SRCS_WRAPPER
+ SET(SRCS_CERES_WRAPPER
#ceres_wrapper/qr_manager.cpp
src/ceres_wrapper/solver_ceres.cpp
src/ceres_wrapper/local_parametrization_wrapper.cpp
src/solver/solver_manager.cpp
)
ELSE(Ceres_FOUND)
- SET(HDRS_WRAPPER)
- SET(SRCS_WRAPPER)
+ SET(HDRS_CERES_WRAPPER)
+ SET(SRCS_CERES_WRAPPER)
ENDIF(Ceres_FOUND)
-IF (cereal_FOUND)
-ADD_SUBDIRECTORY(serialization/cereal)
-ENDIF(cereal_FOUND)
-
IF (Suitesparse_FOUND)
- #DOES NOTHING?!
#ADD_SUBDIRECTORY(solver_suitesparse)
ENDIF(Suitesparse_FOUND)
# create the shared library
ADD_LIBRARY(${PLUGIN_NAME}
SHARED
- ${SRCS}
- ${SRCS_BASE}
${SRCS_CAPTURE}
+ ${SRCS_CERES_WRAPPER}
${SRCS_COMMON}
${SRCS_FACTOR}
${SRCS_FEATURE}
@@ -429,7 +397,6 @@ ADD_LIBRARY(${PLUGIN_NAME}
${SRCS_HARDWARE}
${SRCS_LANDMARK}
${SRCS_MAP}
- ${SRCS_MATH}
${SRCS_PROBLEM}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
@@ -438,7 +405,6 @@ ADD_LIBRARY(${PLUGIN_NAME}
${SRCS_TRAJECTORY}
${SRCS_TREE_MANAGER}
${SRCS_UTILS}
- ${SRCS_WRAPPER}
${SRCS_YAML}
)
@@ -462,30 +428,33 @@ IF (Ceres_FOUND)
TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES})
ENDIF(Ceres_FOUND)
+#Build tests
+#===============EXAMPLE=========================
IF(BUILD_TESTS)
MESSAGE(STATUS "Will build tests.")
add_subdirectory(test)
ENDIF(BUILD_TESTS)
+#Build demos
+#===============EXAMPLE=========================
IF(BUILD_DEMOS)
- #Build demos
MESSAGE(STATUS "Will build demos.")
ADD_SUBDIRECTORY(demos)
ENDIF(BUILD_DEMOS)
-
#install library
-
#=============================================================
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_CAPTURE}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/capture)
+INSTALL(FILES ${HDRS_CERES_WRAPPER}
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
INSTALL(FILES ${HDRS_COMMON}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/common)
INSTALL(FILES ${HDRS_FACTOR}
@@ -508,12 +477,10 @@ INSTALL(FILES ${HDRS_PROCESSOR}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/processor)
INSTALL(FILES ${HDRS_SENSOR}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/sensor)
-INSTALL(FILES ${HDRS_SERIALIZATION}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/serialization)
INSTALL(FILES ${HDRS_SOLVER}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver)
-INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
+#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE}
+# DESTINATION include/iri-algorithms/wolf/plugin_core/core/solver_suitesparse)
INSTALL(FILES ${HDRS_STATE_BLOCK}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/state_block)
INSTALL(FILES ${HDRS_TRAJECTORY}
@@ -522,14 +489,14 @@ INSTALL(FILES ${HDRS_TREE_MANAGER}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/tree_manager)
INSTALL(FILES ${HDRS_UTILS}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/utils)
-INSTALL(FILES ${HDRS_WRAPPER}
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/ceres_wrapper)
INSTALL(FILES ${HDRS_YAML}
DESTINATION include/iri-algorithms/wolf/plugin_core/core/yaml)
FILE(WRITE ${PLUGIN_NAME}.found "")
INSTALL(FILES ${PLUGIN_NAME}.found
DESTINATION include/iri-algorithms/wolf/plugin_core)
+INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
+ DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal)
#install Find*.cmake
configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake"
@@ -538,9 +505,6 @@ configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake"
configure_file("${CMAKE_SOURCE_DIR}/cmake_modules/FindYamlCpp.cmake"
"${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" @ONLY)
-INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h"
- DESTINATION include/iri-algorithms/wolf/plugin_core/core/internal)
-
INSTALL(FILES "${CMAKE_BINARY_DIR}/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
INSTALL(FILES "${CMAKE_BINARY_DIR}/FindYamlCpp.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}")
@@ -548,8 +512,6 @@ INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/")
export(PACKAGE ${PLUGIN_NAME})
-#-END_SRC --------------------------------------------------------------------------------------------------------------------------------
-
FIND_PACKAGE(Doxygen)
FIND_PATH(IRI_DOC_DIR doxygen.conf ${CMAKE_SOURCE_DIR}/doc/iri_doc/)
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..281d399f13dfd23087acc56303dd38d68162587c
--- /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/Testing/Temporary/.gitignore b/Testing/Temporary/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..6ccdef7c09c93207bc46508bb9fd8c873e94e6c9
--- /dev/null
+++ b/Testing/Temporary/.gitignore
@@ -0,0 +1,2 @@
+/CTestCostData.txt
+/LastTest.log
diff --git a/cmake_modules/wolfcoreConfig.cmake b/cmake_modules/wolfcoreConfig.cmake
index b4df7e15bab20bc8d1090ffbd5428ac7a3ab5cd6..5204cd493ca0740f38d99aef942a7df32ad7f71d 100644
--- a/cmake_modules/wolfcoreConfig.cmake
+++ b/cmake_modules/wolfcoreConfig.cmake
@@ -12,7 +12,7 @@ ENDIF(wolfcore_INCLUDE_DIRS)
FIND_LIBRARY(
wolfcore_LIBRARIES
NAMES libwolfcore.so libwolfcore.dylib
- PATHS /usr/local/lib/iri-algorithms)
+ PATHS /usr/local/lib)
IF(wolfcore_LIBRARIES)
MESSAGE("Found wolf core lib: ${wolfcore_LIBRARIES}")
ELSE(wolfcore_LIBRARIES)
@@ -97,3 +97,8 @@ else (NOT wolfcore_FOUND)
list(APPEND wolfcore_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIRS})
endif(NOT wolfcore_FOUND)
SET(CMAKE_MODULE_PATH ${BACKUP_MODULE_PATH})
+
+# provide both INCLUDE_DIR and INCLUDE_DIRS
+SET(wolfcore_INCLUDE_DIR ${wolfcore_INCLUDE_DIRS})
+# provide both LIBRARY and LIBRARIES
+SET(wolfcore_LIBRARY ${wolfcore_LIBRARIES})
diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index 0fd8d42cc5280016d7473e23cb2624615221fda7..0625a61b5fce5bd66d240e3ac1da89e5d584e455 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.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--------
// Testing creating wolf tree from imported .graph file
//C includes for sleep, time and main args
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index 386d803138f33b89bee401aceda047fe2199f104..037ff094a158592c24adee70c28ddfb8cddb1d35 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.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--------
// Testing creating wolf tree from imported .graph file
//C includes for sleep, time and main args
diff --git a/demos/hello_wolf/capture_range_bearing.cpp b/demos/hello_wolf/capture_range_bearing.cpp
index 9ead1c183b8b08a64f709faea5236d5dbb056231..f5756924bd63369adb1e37fab7e785e9590929d5 100644
--- a/demos/hello_wolf/capture_range_bearing.cpp
+++ b/demos/hello_wolf/capture_range_bearing.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--------
/*
* CaptureRangeBearing2d.cpp
*
diff --git a/demos/hello_wolf/capture_range_bearing.h b/demos/hello_wolf/capture_range_bearing.h
index 10fb8fa05f14fc4801effce4fb734c90a9ea2e90..03803089d8a68d9ab14fd31aee123c6bf04144f3 100644
--- a/demos/hello_wolf/capture_range_bearing.h
+++ b/demos/hello_wolf/capture_range_bearing.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--------
/*
* capture_range_bearing.h
*
diff --git a/demos/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h
index d3902daadbda071ae5a08e5d74148c877d44b93e..d06ace3a796002e87d33a2e9546ad280bea49416 100644
--- a/demos/hello_wolf/factor_bearing.h
+++ b/demos/hello_wolf/factor_bearing.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--------
/*
* FactorBearing.h
*
diff --git a/demos/hello_wolf/factor_range_bearing.h b/demos/hello_wolf/factor_range_bearing.h
index 596278b2f70d362ba3c1debeec12fb9d8830795e..4495682647b7b1ddcac24726e2bfbb86df1e343a 100644
--- a/demos/hello_wolf/factor_range_bearing.h
+++ b/demos/hello_wolf/factor_range_bearing.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--------
/**
* \file factor_range_bearing.h
*
diff --git a/demos/hello_wolf/feature_range_bearing.cpp b/demos/hello_wolf/feature_range_bearing.cpp
index ea584a419aa21c1d7e462824d6872f7d57bd818c..5ae7e031e8d057f8a8a0fe5df0b5deefe479d4b4 100644
--- a/demos/hello_wolf/feature_range_bearing.cpp
+++ b/demos/hello_wolf/feature_range_bearing.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--------
/*
* FeatureRangeBearing2d.cpp
*
diff --git a/demos/hello_wolf/feature_range_bearing.h b/demos/hello_wolf/feature_range_bearing.h
index b924d29eea0a22a80267ace90718f9a66f54dac1..24123e4df893acc29337713877bc342db39563bc 100644
--- a/demos/hello_wolf/feature_range_bearing.h
+++ b/demos/hello_wolf/feature_range_bearing.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--------
/*
* FeatureRangeBearing2d.h
*
diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 2b2d03d43f1a7d315efb0ae07a734ec022f9e4c6..5500b724a95cf12984d12b1327749833a5ff8acc 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.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--------
/**
* \file hello_wolf.cpp
*
@@ -140,7 +161,7 @@ int main()
VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P = Matrix3d::Identity() * 0.1;
VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- FrameBasePtr KF1 = problem->setPriorFactor(x, P, t, 0.5); // KF1 : (0,0,0)
+ FrameBasePtr KF1 = problem->setPriorFactor(x, P, t); // KF1 : (0,0,0)
std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index edeb6b1f04f1f673ad3af9b94493b4e67e46ab9f..db934ca13174443de5053e3a427206de41799c18 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.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--------
/**
* \file hello_wolf_autoconf.cpp
*
@@ -95,7 +116,7 @@ int main()
using namespace wolf;
- WOLF_TRACE("======== CONFIGURE PROBLEM =======");
+ WOLF_INFO("======== CONFIGURE PROBLEM =======");
// Config file to parse. Here is where all the problem is defined:
std::string config_file = "demos/hello_wolf/yaml/hello_wolf_config.yaml";
@@ -123,7 +144,7 @@ int main()
// APPLY PRIOR and SET PROCESSOR ODOM ORIGIN ===================================================
TimeStamp t(0.0);
FrameBasePtr KF1 = problem->applyPriorOptions(t);
- std::static_pointer_cast<ProcessorMotion>(problem->getProcessorIsMotion())->setOrigin(KF1);
+// std::static_pointer_cast<ProcessorMotion>(problem->getMotionProviderMap().begin()->second)->setOrigin(KF1);
// SELF CALIBRATION ===================================================
// These few lines control whether we calibrate some sensor parameters or not.
@@ -148,12 +169,16 @@ int main()
// SET OF EVENTS: make things happen =======================================================
std::cout << std::endl;
- WOLF_TRACE("======== START ROBOT MOVE AND SLAM =======")
+ WOLF_INFO("======== START ROBOT MOVE AND SLAM =======")
// We'll do 3 steps of motion and landmark observations.
// STEP 1 --------------------------------------------------------------
+ // move zero motion to accept the first keyframe and initialize the processor
+ CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, 0*motion_data, 0*motion_cov);
+ cap_motion ->process(); // KF1 : (0,0,0)
+
// observe lmks
ids.resize(1); ranges.resize(1); bearings.resize(1);
ids << 1; // will observe Lmk 1
@@ -166,7 +191,7 @@ int main()
t += 1.0; // t : 1.0
// motion
- CaptureOdom2dPtr cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
+ cap_motion = std::make_shared<CaptureOdom2d>(t, sensor_odo, motion_data, motion_cov);
cap_motion ->process(); // KF2 : (1,0,0)
// observe lmks
@@ -199,40 +224,40 @@ int main()
// SOLVE ================================================================
// SOLVE with exact initial guess
- WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
+ WOLF_INFO("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_TRACE(report); // should show a very low iteration number (possibly 1)
+ WOLF_INFO(report); // should show a very low iteration number (possibly 1)
problem->print(1,0,1,0);
// PERTURB initial guess
- WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
+ WOLF_INFO("======== PERTURB PROBLEM PRIORS =======")
problem->perturb(0.5); // Perturb all state blocks that are not fixed
problem->print(1,0,1,0);
// SOLVE again
- WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
+ WOLF_INFO("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
- WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
+ WOLF_INFO(report); // should show a very high iteration number (more than 10, or than 100!)
problem->print(1,0,1,0);
// GET COVARIANCES of all states
- WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
+ WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
for (auto& kf : *problem->getTrajectory())
{
Eigen::MatrixXd cov;
kf->getCovariance(cov);
- WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+ WOLF_INFO("KF", kf->id(), "_cov = \n", cov);
}
for (auto& lmk : problem->getMap()->getLandmarkList())
{
Eigen::MatrixXd cov;
lmk->getCovariance(cov);
- WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
+ WOLF_INFO("L", lmk->id(), "_cov = \n", cov);
}
std::cout << std::endl;
- WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
+ WOLF_INFO("======== FINAL PRINT FOR INTERPRETATION =======")
problem->print(4,1,1,1);
/*
diff --git a/demos/hello_wolf/landmark_point_2d.cpp b/demos/hello_wolf/landmark_point_2d.cpp
index 8b2ad4c0e8a9b576f67e9d8d56e41d932abedef8..c2c11187eedd016c49116f84ee14141067d7fa47 100644
--- a/demos/hello_wolf/landmark_point_2d.cpp
+++ b/demos/hello_wolf/landmark_point_2d.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--------
/**
* \file landmark_point_2d.cpp
*
diff --git a/demos/hello_wolf/landmark_point_2d.h b/demos/hello_wolf/landmark_point_2d.h
index 79287af069fb5b5da0ef7186333b90fc18664bbb..78aeafe9bf28b7abbd81f937db60d96da8f8c601 100644
--- a/demos/hello_wolf/landmark_point_2d.h
+++ b/demos/hello_wolf/landmark_point_2d.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--------
/**
* \file landmark_point_2d.h
*
diff --git a/demos/hello_wolf/processor_range_bearing.cpp b/demos/hello_wolf/processor_range_bearing.cpp
index 7df38e73367b7b45aff81cdbf5f00099d64aa990..06469b0b133453e5d3aaeb12160ed5669c8cb2b2 100644
--- a/demos/hello_wolf/processor_range_bearing.cpp
+++ b/demos/hello_wolf/processor_range_bearing.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--------
/*
* processor_range_bearing.cpp
*
@@ -29,30 +50,27 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
// 1. get KF
- FrameBasePtr kf(nullptr);
- if ( !buffer_pack_kf_.empty() )
+ FrameBasePtr keyframe(nullptr);
+ if ( !buffer_frame_.empty() )
{
// KeyFrame Callback received
- PackKeyFramePtr pack = buffer_pack_kf_.selectPack( _capture->getTimeStamp(), params_->time_tolerance );
+ keyframe = buffer_frame_.select( _capture->getTimeStamp(), params_->time_tolerance );
- if (pack!=nullptr)
- kf = pack->key_frame;
+ buffer_frame_.removeUpTo( _capture->getTimeStamp() );
- buffer_pack_kf_.removeUpTo( _capture->getTimeStamp() );
-
- assert( kf && "Callback KF is not close enough to _capture!");
+ assert( keyframe && "Callback KF is not close enough to _capture!");
}
- if (!kf)
+ if (!keyframe)
{
// No KeyFrame callback received -- we assume a KF is available to hold this _capture (checked in assert below)
- kf = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
- assert( (fabs(kf->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
+ keyframe = getProblem()->closestFrameToTimeStamp(_capture->getTimeStamp());
+ assert( (fabs(keyframe->getTimeStamp() - _capture->getTimeStamp()) < params_->time_tolerance) && "Could not find a KF close enough to _capture!");
}
// 2. cast incoming capture to the range-and-bearing type, add it to the keyframe
CaptureRangeBearingPtr capture_rb = std::static_pointer_cast<CaptureRangeBearing>(_capture);
- capture_rb->link(kf);
+ capture_rb->link(keyframe);
// 3. explore all observations in the capture
for (SizeEigen i = 0; i < capture_rb->getIds().size(); i++)
@@ -82,14 +100,14 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
// 5. create feature
- Vector2d rb(range,bearing);
+ Vector2d measurement_rb(range,bearing);
auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
- rb,
+ measurement_rb,
getSensor()->getNoiseCov());
// 6. create factor
auto prc = shared_from_this();
- auto ctr = FactorBase::emplace<FactorRangeBearing>(ftr,
+ auto fac = FactorBase::emplace<FactorRangeBearing>(ftr,
capture_rb,
ftr,
lmk,
diff --git a/demos/hello_wolf/processor_range_bearing.h b/demos/hello_wolf/processor_range_bearing.h
index f086da9071e4956e3c7ef1ab2d59bff81326f594..ffd8b9b9c754aa0938b8d158eabedec373706577 100644
--- a/demos/hello_wolf/processor_range_bearing.h
+++ b/demos/hello_wolf/processor_range_bearing.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--------
/*
* ProcessorRangeBearing.h
*
@@ -55,9 +76,9 @@ class ProcessorRangeBearing : public ProcessorBase
protected:
// Implementation of pure virtuals from ProcessorBase
void processCapture (CaptureBasePtr _capture) override;
- void processKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
+ void processKeyFrame (FrameBasePtr _keyframe_ptr) override {};
bool triggerInCapture (CaptureBasePtr) const override { return true;};
- bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override {return false;}
+ bool triggerInKeyFrame (FrameBasePtr _keyframe_ptr) const override {return false;}
bool voteForKeyFrame () const override {return false;}
/** \brief store key frame
diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp
index 54507f9ac53771523b9db8716089113686bc1ca0..7073082e34be71b45e60b7671211e1617432a124 100644
--- a/demos/hello_wolf/sensor_range_bearing.cpp
+++ b/demos/hello_wolf/sensor_range_bearing.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--------
/*
* SensorRangeBearing.cpp
*
diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h
index 70684f77951f2681d8c2741d3c5c5b09fc503a12..80f55753b30364216fc22cdf34b1e7bfc2425168 100644
--- a/demos/hello_wolf/sensor_range_bearing.h
+++ b/demos/hello_wolf/sensor_range_bearing.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--------
/*
* SensorRangeBearing.h
*
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index b0eea06960149ef9a26213f186a122b76767b6f0..c13af9ed22c38ba4fb62b3f1c54880dad810deef 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -13,7 +13,7 @@ config:
$sigma:
P: [0.31, 0.31]
O: [0.31]
- time_tolerance: 0.1
+ time_tolerance: 0.5
tree_manager:
type: "none"
@@ -27,8 +27,13 @@ config:
max_num_iterations: 100
verbose: 0
period: 0.2
- update_immediately: false
- n_threads: 1
+ interrupt_on_problem_change: false
+ n_threads: 2
+ compute_cov: false
+ minimizer: LEVENBERG_MARQUARDT
+ use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG
+ function_tolerance: 0.000001
+ gradient_tolerance: 0.0000000001
sensors:
diff --git a/demos/solver/test_SPQR.cpp b/demos/solver/test_SPQR.cpp
index 04592dbd047fed1d4b1d577c2df53cd5bbe57766..b5f159c731741dabe4ed00bbe8eaa0e7968be7f3 100644
--- a/demos/solver/test_SPQR.cpp
+++ b/demos/solver/test_SPQR.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--------
/*
* test_SPQR.cpp
*
diff --git a/demos/solver/test_ccolamd.cpp b/demos/solver/test_ccolamd.cpp
index 48831f6511f2d29689ced193e599dc4ae6292a5f..98f496f26b5de691a38384d42b3349d62fc8b20e 100644
--- a/demos/solver/test_ccolamd.cpp
+++ b/demos/solver/test_ccolamd.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--------
/*
* test_ccolamd.cpp
*
diff --git a/demos/solver/test_ccolamd_blocks.cpp b/demos/solver/test_ccolamd_blocks.cpp
index 83cc7af8407282484ba5aa26dc66eec59a30bb11..14bb174286235f063a375cca4e28f1e4dfeca537 100644
--- a/demos/solver/test_ccolamd_blocks.cpp
+++ b/demos/solver/test_ccolamd_blocks.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--------
/*
* test_ccolamd_blocks.cpp
*
diff --git a/demos/solver/test_iQR.cpp b/demos/solver/test_iQR.cpp
index b027c874d6a2e6c4910d5dc45114055d87aa2d54..53a37cef1a62ec1ffb281a6fdbd741fc73cb59f8 100644
--- a/demos/solver/test_iQR.cpp
+++ b/demos/solver/test_iQR.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--------
/*
* test_iQR.cpp
*
diff --git a/demos/solver/test_iQR_wolf.cpp b/demos/solver/test_iQR_wolf.cpp
index 2fdc1f9f22ca75801b6dc7155dea9bb515d9ccd0..a8df237237669b54aa837d32f35bf8c156fa33eb 100644
--- a/demos/solver/test_iQR_wolf.cpp
+++ b/demos/solver/test_iQR_wolf.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--------
/*
* test_iQR_wolf.cpp
*
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index 82aab6219874c3575dc14fdb24af6d2cc0e2d07a..821ffefeb052acb75f81ec5d3327ebdaf04c4cd7 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.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--------
/*
* test_iQR_wolf.cpp
*
diff --git a/demos/solver/test_incremental_ccolamd_blocks.cpp b/demos/solver/test_incremental_ccolamd_blocks.cpp
index 9283f8411954e0aea8783d067a419e85a09db932..740407a9a560f0efdbdb8267ee7925c22fbf12bf 100644
--- a/demos/solver/test_incremental_ccolamd_blocks.cpp
+++ b/demos/solver/test_incremental_ccolamd_blocks.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--------
/*
* test_ccolamd_blocks.cpp
*
diff --git a/demos/solver/test_permutations.cpp b/demos/solver/test_permutations.cpp
index c33c744c673024b5a66aece490d98b2de5e5543b..0765d8ae77996a72b027a62380804243aa0aedfa 100644
--- a/demos/solver/test_permutations.cpp
+++ b/demos/solver/test_permutations.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--------
/*
* test_permutations.cpp
*
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 68d34c20bfd4e2abe3ca76d331f696d86423effe..d73f17d8ec26cd8765ad1dcbeddaee8b66c91944 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.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_BASE_H_
#define CAPTURE_BASE_H_
@@ -93,6 +114,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
void move(FrameBasePtr);
void link(FrameBasePtr);
+ void unlink();
+
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(FrameBasePtr _frm_ptr, T&&... all);
diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index e9797427f3550289906e159c40fefe681d2e55ad..a41e7dec78fa134b7a09fe99740daaccff42ed7a 100644
--- a/include/core/capture/capture_diff_drive.h
+++ b/include/core/capture/capture_diff_drive.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--------
/**
* \file diff_drive_tools.h
*
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index c0d86ee1089740ecf03b3896f9264cdc2de5db88..a70a54b2aba7e19d8078a209c5b43cad63693671 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.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--------
/**
* \file capture_motion.h
*
diff --git a/include/core/capture/capture_odom_2d.h b/include/core/capture/capture_odom_2d.h
index e0a81bf2d47cad9b8d564f11eb011574daae76d4..8bc18706d5fb6122d9bc7e6262f8879f158bef60 100644
--- a/include/core/capture/capture_odom_2d.h
+++ b/include/core/capture/capture_odom_2d.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--------
/*
* capture_odom_2d.h
*
diff --git a/include/core/capture/capture_odom_3d.h b/include/core/capture/capture_odom_3d.h
index 054c4f935ca63f73fd6fbbb87bab70a10655e5c3..0b6deb6a8f05cb0769a9065b57c190f693c78b62 100644
--- a/include/core/capture/capture_odom_3d.h
+++ b/include/core/capture/capture_odom_3d.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--------
/*
* capture_odom_3d.h
*
diff --git a/include/core/capture/capture_pose.h b/include/core/capture/capture_pose.h
index 07db9e0351ddb1e73ea8877c1a31ddc456e7dacd..15b32f85ea340cc4a83a9745e54ce647f15fc250 100644
--- a/include/core/capture/capture_pose.h
+++ b/include/core/capture/capture_pose.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_POSE_H_
#define CAPTURE_POSE_H_
diff --git a/include/core/capture/capture_void.h b/include/core/capture/capture_void.h
index 0ac7b3b2fc9ed44c4ac430fd9f70387435f05227..1adb08e43512af44f3d04951a8c3b27f685a9a43 100644
--- a/include/core/capture/capture_void.h
+++ b/include/core/capture/capture_void.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_VOID_H_
#define CAPTURE_VOID_H_
diff --git a/include/core/ceres_wrapper/cost_function_wrapper.h b/include/core/ceres_wrapper/cost_function_wrapper.h
index c9972fe780e76c243ed1a298f658d6e702fe8d1e..9bca9394f7f3bd99e1bd2846f07d15e3bdc776e9 100644
--- a/include/core/ceres_wrapper/cost_function_wrapper.h
+++ b/include/core/ceres_wrapper/cost_function_wrapper.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 TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
#define TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
diff --git a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h b/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
index 2504d0b10bf669ececd7ce33e6ab9b15928eb26b..42bf8a7a9abd1339a26b58dd0dc0a195faf8957d 100644
--- a/include/core/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/core/ceres_wrapper/create_numeric_diff_cost_function.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--------
/*
* create_numeric_diff_cost_function.h
*
diff --git a/include/core/ceres_wrapper/iteration_update_callback.h b/include/core/ceres_wrapper/iteration_update_callback.h
index 1717be546ab820d24e13985227dd19753530440e..6cf6179ff8c9d5e7543a5794210b65bd30b75049 100644
--- a/include/core/ceres_wrapper/iteration_update_callback.h
+++ b/include/core/ceres_wrapper/iteration_update_callback.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--------
/*
* iteration_callback.h
*
diff --git a/include/core/ceres_wrapper/local_parametrization_wrapper.h b/include/core/ceres_wrapper/local_parametrization_wrapper.h
index fcffbbf80ed9b09717dd830fe250facd70bf9985..51d10c49764e8386727b1acd17aab4423bbe071c 100644
--- a/include/core/ceres_wrapper/local_parametrization_wrapper.h
+++ b/include/core/ceres_wrapper/local_parametrization_wrapper.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 LOCAL_PARAMETRIZATION_WRAPPER_H_
#define LOCAL_PARAMETRIZATION_WRAPPER_H_
diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h
index 691c44166da222b893c1d8acd428c7e63da71539..d859b6a655825809c35de4808c38af0a7dbcd32e 100644
--- a/include/core/ceres_wrapper/qr_manager.h
+++ b/include/core/ceres_wrapper/qr_manager.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--------
/*
* qr_manager.h
*
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index 4bf6f7de661059e7c9f96e7f12bb444f18bed013..da0ac085504dfe02d64ee7c0b2c4d30b0b029adb 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.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 CERES_MANAGER_H_
#define CERES_MANAGER_H_
@@ -22,7 +43,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
struct ParamsCeres : public ParamsSolver
{
- bool update_immediately = false;
+ bool interrupt_on_problem_change = false;
int min_num_iterations = 1;
ceres::Solver::Options solver_options;
ceres::Problem::Options problem_options;
@@ -39,36 +60,63 @@ struct ParamsCeres : public ParamsSolver
{
loadHardcodedValues();
- // stop solver whenever the problem is updated (via ceres::iterationCallback)
- update_immediately = _server.getParam<bool>(prefix + "update_immediately");
- if (update_immediately)
+ // interrupt solver whenever the problem is updated (via ceres::iterationCallback)
+ interrupt_on_problem_change = _server.getParam<bool>(prefix + "interrupt_on_problem_change");
+ if (interrupt_on_problem_change)
min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations");
- // ceres solver options
+ // CERES SOLVER OPTIONS
solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations");
+ solver_options.function_tolerance = _server.getParam<double>(prefix + "function_tolerance");
+ solver_options.gradient_tolerance = _server.getParam<double>(prefix + "gradient_tolerance");
solver_options.num_threads = _server.getParam<int>(prefix + "n_threads");
- covariance_options.num_threads = _server.getParam<int>(prefix + "n_threads");
+ covariance_options.num_threads = solver_options.num_threads;
+
+ // minimizer type
+ std::string minimizer = _server.getParam<std::string>(prefix + "minimizer");
+ if (minimizer == "LEVENBERG_MARQUARDT" or minimizer == "levenberg_marquardt")
+ {
+ solver_options.minimizer_type = ceres::TRUST_REGION;
+ solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
+ }
+ else if (minimizer == "DOGLEG" or minimizer == "dogleg")
+ {
+ solver_options.minimizer_type = ceres::TRUST_REGION;
+ solver_options.trust_region_strategy_type = ceres::DOGLEG;
+ }
+ else if (minimizer == "LBFGS" or minimizer == "lbfgs")
+ {
+ solver_options.minimizer_type = ceres::LINE_SEARCH;
+ solver_options.line_search_direction_type = ceres::LBFGS;
+ }
+ else if (minimizer == "BFGS" or minimizer == "bfgs")
+ {
+ solver_options.minimizer_type = ceres::LINE_SEARCH;
+ solver_options.line_search_direction_type = ceres::BFGS;
+ }
+ else
+ {
+ throw std::runtime_error("ParamsCeres: Wrong parameter 'minimizer'. Should be 'LEVENBERG_MARQUARDT', 'DOGLEG', 'LBFGS' or 'BFGS' (upper or lowercase)");
+ }
+
+ // specific options for TRUST REGION
+ if (solver_options.minimizer_type == ceres::TRUST_REGION)
+ {
+ solver_options.use_nonmonotonic_steps = _server.getParam<bool>(prefix + "use_nonmonotonic_steps");
+ if (solver_options.use_nonmonotonic_steps)
+ solver_options.max_consecutive_nonmonotonic_steps = _server.getParam<int>(prefix + "max_consecutive_nonmonotonic_steps");
+ }
}
void loadHardcodedValues()
{
solver_options = ceres::Solver::Options();
- solver_options.minimizer_type = ceres::TRUST_REGION; //ceres::LINE_SEARCH;
- solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; //ceres::DOGLEG;
problem_options = ceres::Problem::Options();
covariance_options = ceres::Covariance::Options();
problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
- #if CERES_VERSION_MINOR >= 13
- covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
- covariance_options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
- #elif CERES_VERSION_MINOR >= 10
- covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD;
- #else
- covariance_options.algorithm_type = ceres::SPARSE_QR;//ceres::DENSE_SVD;
- #endif
covariance_options.apply_loss_function = false;
}
@@ -91,6 +139,10 @@ class SolverCeres : public SolverManager
ParamsCeresPtr params_ceres_;
+ // profiling
+ unsigned int n_iter_acc_, n_iter_max_;
+ unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
+
public:
SolverCeres(const ProblemPtr& _wolf_problem);
@@ -112,13 +164,10 @@ class SolverCeres : public SolverManager
bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
bool hasConverged() override;
-
- SizeStd iterations() override;
-
+ bool wasStopped() override;
+ unsigned int iterations() override;
double initialCost() override;
-
double finalCost() override;
-
double totalTime() override;
ceres::Solver::Options& getSolverOptions();
@@ -159,6 +208,8 @@ class SolverCeres : public SolverManager
const LocalParametrizationBasePtr& local_param) override;
bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
+
+ void printProfilingDerived(std::ostream& _stream) const override;
};
inline ceres::Solver::Summary SolverCeres::getSummary()
diff --git a/include/core/ceres_wrapper/sparse_utils.h b/include/core/ceres_wrapper/sparse_utils.h
index 4b7171f77eaf09cc902ae24aea7dd49c5a4adfca..e0d70f26f95c2d5dfd10131e48103586c52fd20d 100644
--- a/include/core/ceres_wrapper/sparse_utils.h
+++ b/include/core/ceres_wrapper/sparse_utils.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--------
/*
* sparse_utils.h
*
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index edd07448729ba69ddda1fdc447b2610c22cd298c..d533ff0da8b627dd0d12ca9a32637956da871af5 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.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--------
/**
* \file factory.h
*
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index dcd7d37176c2eb4c36aea7c8b342aa064b43a896..f450b60deb50b1ffb65e53c87cf22bac28052f84 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.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 NODE_BASE_H_
#define NODE_BASE_H_
diff --git a/include/core/common/params_base.h b/include/core/common/params_base.h
index b9a4886c290fb9c1dcb4889d55b6d1523c42df78..4760d1cdd5185f6842297902ed335c039e673b4e 100644
--- a/include/core/common/params_base.h
+++ b/include/core/common/params_base.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 PARAMS_BASE_H_
#define PARAMS_BASE_H_
diff --git a/include/core/common/time_stamp.h b/include/core/common/time_stamp.h
index 2643f13c8f9629dce921726d6dfacf3171c3417f..c327ab2d05f1d335e51d27ec6dfe07ebf5c5ab5c 100644
--- a/include/core/common/time_stamp.h
+++ b/include/core/common/time_stamp.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 TIME_STAMP_H_
#define TIME_STAMP_H_
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index e055a1bdcac1b6d8477026bfdee5caeb1e73b96d..197a6bfb1b30d79a64376f9468b9481c0a4bc3e3 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.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--------
/**
* \file wolf.h
* \brief General typedefs for the Wolf project
diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index 93daadcee28914f0688d4d2e2169614929da41a0..b2c0d0e4f6f2d589278a243e4cc34de6f6640c3e 100644
--- a/include/core/factor/factor_analytic.h
+++ b/include/core/factor/factor_analytic.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 FACTOR_ANALYTIC_H_
#define FACTOR_ANALYTIC_H_
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 68c820c6f09246f6f7c1eba0f68dd61689d53301..0b8f9e111d007024ff457b2303cd704604a4fb10 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.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 FACTOR_AUTODIFF_H_
#define FACTOR_AUTODIFF_H_
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index e3dd39803de616a707c2d25a21da04bee6bb9e6e..05508895ca766ee28ae61bd4bd615cbb27288edf 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.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 FACTOR_BASE_H_
#define FACTOR_BASE_H_
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 58d0570dd25bf480cc9ca02ef6a4de091a6155b3..3b9e464a01f6f5d9d28b2cfb3e74614e3ef4e212 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.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--------
/**
* \file factor_block_absolute.h
*
diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h
index bdf76c063df0af5b25d5afe4fbd3f24a8df44605..503ace5dd84ece4a7adb05205deba25883b72609 100644
--- a/include/core/factor/factor_block_difference.h
+++ b/include/core/factor/factor_block_difference.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--------
/**
* \file factor_block_difference.h
*
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 72677196437b31bd22d8b72408afbd5f813d7fe8..9606e23b17f21b8772081f090a31e16c617338de 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.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--------
/**
* \file factor_diff_drive.h
*
diff --git a/include/core/factor/factor_distance_3d.h b/include/core/factor/factor_distance_3d.h
index 61f6767a2a9813950fe49e0eda6c200677842881..d4207b254443ad077a0b9ad319ab333a8b0040cc 100644
--- a/include/core/factor/factor_distance_3d.h
+++ b/include/core/factor/factor_distance_3d.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--------
/**
* \file factor_autodiff_distance_3d.h
*
@@ -57,10 +78,6 @@ class FactorDistance3d : public FactorAutodiff<FactorDistance3d, 1, 3, 3>
norm_squared += (T)1e-8;
}
Matrix<T,1,1> dist_exp ( sqrt(norm_squared) );
- // Matrix<T,1,1> dist_meas (getMeasurement().cast<T>());
- // Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper();
-
- // res = sqrt_info_upper * (dist_meas - dist_exp);
res = getMeasurementSquareRootInformationUpper() * (getMeasurement() - dist_exp);
diff --git a/include/core/factor/factor_pose_2d.h b/include/core/factor/factor_pose_2d.h
index 4357aac836f852aaa88caa242730262db984c525..2d3f40ed24c53cb7e28d7cb2054ccaa33140c02d 100644
--- a/include/core/factor/factor_pose_2d.h
+++ b/include/core/factor/factor_pose_2d.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 FACTOR_POSE_2d_H_
#define FACTOR_POSE_2d_H_
diff --git a/include/core/factor/factor_pose_3d.h b/include/core/factor/factor_pose_3d.h
index 139b87b65384063540d2546f465180937f646156..04f7f7b1798eb3669a2f397f1c7f2915871b1b9e 100644
--- a/include/core/factor/factor_pose_3d.h
+++ b/include/core/factor/factor_pose_3d.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 FACTOR_POSE_3d_H_
#define FACTOR_POSE_3d_H_
diff --git a/include/core/factor/factor_pose_3d_with_extrinsics.h b/include/core/factor/factor_pose_3d_with_extrinsics.h
index ad35fb6c2d4a317c0f48051dc7920bdea76ab77b..2daacba6d7d05fb2dde5356a62ccf43a18213777 100644
--- a/include/core/factor/factor_pose_3d_with_extrinsics.h
+++ b/include/core/factor/factor_pose_3d_with_extrinsics.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 FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_H_
#define FACTOR_POSE_3D_WITH_EXTRINSICS_THETA_H_
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 3122e80977c9b0816ca6eb42afa4db494d7c2ea5..5e93a15389990111ddb1751e635e3e11ff73c503 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.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--------
/**
* \file factor_quaternion_absolute.h
*
diff --git a/include/core/factor/factor_relative_pose_2d.h b/include/core/factor/factor_relative_pose_2d.h
index a51c9a7fc9e9013eb4a79746c6bbc024c4d9e386..4a608feb01c29b413b19608987767853eecf5407 100644
--- a/include/core/factor/factor_relative_pose_2d.h
+++ b/include/core/factor/factor_relative_pose_2d.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 FACTOR_RELATIVE_POSE_2d_H_
#define FACTOR_RELATIVE_POSE_2d_H_
diff --git a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h b/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
index 1d7e85804d2e1e94eeca7623f382e0ef19f05dbd..2247a898bb4d5030ed1b07ef6422d72a9ca968e8 100644
--- a/include/core/factor/factor_relative_pose_2d_with_extrinsics.h
+++ b/include/core/factor/factor_relative_pose_2d_with_extrinsics.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 FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
#define FACTOR_RELATIVE_POSE_2D_WITH_EXTRINSICS_THETA_H_
diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index afcf8daf6884c9c08eb800433fa1f2392a8ecd0c..a132c3c0ac105fc0476f18ab2c625ee5a5801bad 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.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--------
/*
* factor_relative_pose_3d.h
*
diff --git a/include/core/factor/factor_velocity_local_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h
index 115ea57f1803aa7a907c6e895e04d7731eab1560..89d8cb1268180c4cdd1076251731f52f4329214a 100644
--- a/include/core/factor/factor_velocity_local_direction_3d.h
+++ b/include/core/factor/factor_velocity_local_direction_3d.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 FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
#define FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 43735ff44e297f6e6b71a43122d8e6a71387b340..5259e5c51f9d5dece25eb3b3f35d7cb5570541d7 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.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_BASE_H_
#define FEATURE_BASE_H_
diff --git a/include/core/feature/feature_diff_drive.h b/include/core/feature/feature_diff_drive.h
index 203e8f05facaf95ef18543f0ffe6508a69a70060..8b756e29a415db4f6b3278497cdab946db736857 100644
--- a/include/core/feature/feature_diff_drive.h
+++ b/include/core/feature/feature_diff_drive.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--------
/**
* \file feature_diff_drive.h
*
@@ -17,16 +38,14 @@ WOLF_PTR_TYPEDEFS(FeatureDiffDrive)
class FeatureDiffDrive : public FeatureMotion
{
-public:
+ public:
- FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
- const Eigen::MatrixXd& _delta_preintegrated_covariance,
- const Eigen::VectorXd& _diff_drive_params,
- const Eigen::MatrixXd& _jacobian_diff_drive_params);
+ FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::VectorXd& _diff_drive_params,
+ const Eigen::MatrixXd& _jacobian_diff_drive_params);
- virtual ~FeatureDiffDrive() = default;
-
-protected:
+ virtual ~FeatureDiffDrive() = default;
};
diff --git a/include/core/feature/feature_match.h b/include/core/feature/feature_match.h
index 0ace09b3e99b53f3eaa63d31db91a846632bf00b..36b5b2ca1cdb9f75547a092379156eac2a26746a 100644
--- a/include/core/feature/feature_match.h
+++ b/include/core/feature/feature_match.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_MATCH_H_
#define FEATURE_MATCH_H_
diff --git a/include/core/feature/feature_motion.h b/include/core/feature/feature_motion.h
index 0f5ecc2a66930370b3a314a1284ff366fc76434f..535acdfe98317b9a1e4c801dd6e47862c1b7b4d3 100644
--- a/include/core/feature/feature_motion.h
+++ b/include/core/feature/feature_motion.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--------
/*
* feature_motion.h
*
diff --git a/include/core/feature/feature_odom_2d.h b/include/core/feature/feature_odom_2d.h
index 91016cbfc53c4437e7bb974b068dcfcc2dc3add6..e83d964e061c74600a94db83623335e33f0e2e26 100644
--- a/include/core/feature/feature_odom_2d.h
+++ b/include/core/feature/feature_odom_2d.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_ODOM_2d_H_
#define FEATURE_ODOM_2d_H_
diff --git a/include/core/feature/feature_pose.h b/include/core/feature/feature_pose.h
index 701967949c395fa02371f55d9313c508174eabeb..bd30ee7394a85240fe8fc69582c5c5a3ca49aabb 100644
--- a/include/core/feature/feature_pose.h
+++ b/include/core/feature/feature_pose.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_POSE_H_
#define FEATURE_POSE_H_
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 7234284a5d00ebcb66f6f9a2efee2c38c9745bbd..c7ab25754729c29711e65d532a2e23b36d7d309a 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.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 FRAME_BASE_H_
@@ -42,7 +63,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
/** \brief Constructor with type, time stamp and state pointer
*
* Constructor with type, time stamp and state pointer
- * \param _tp indicates frame type. Generally either NON_KEY_FRAME or KEY_FRAME. (types defined at wolf.h)
* \param _ts is the time stamp associated to this frame, provided in seconds
* \param _p_ptr StateBlock pointer to the position (default: nullptr)
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 0ef770b1cb1249491c01329b9ecc4952776a9dd3..59f423f4295472897cc2d8065811aba72538fbfb 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.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 HARDWARE_BASE_H_
#define HARDWARE_BASE_H_
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 21098d33232129521a5d4f56b52db1930feb5eee..7984e6773a80615aa33a84f67156cf83f1c42ab6 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.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 LANDMARK_BASE_H_
#define LANDMARK_BASE_H_
diff --git a/include/core/landmark/landmark_match.h b/include/core/landmark/landmark_match.h
index 891173fc5289c4fc0f49359720a2d98dfb16ae54..a7fbe5cc33f3c9c1761fd7d6a575df613324a316 100644
--- a/include/core/landmark/landmark_match.h
+++ b/include/core/landmark/landmark_match.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 LANDMARK_MATCH_H_
#define LANDMARK_MATCH_H_
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 88fd0019053abbc835ee7836c915c14cf6a9c23f..797d2bd3892e339c96a8c40e591eb56d39818ee5 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.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 MAP_BASE_H_
#define MAP_BASE_H_
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index ffd8ac8725e49ea921b32140042eb0936cf5d467..4ce5ab465d6e87ec8e7cacf5af05c06f2a6db32c 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.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--------
/**
* \file SE2.h
*
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 685eefabaccf1d0585ec7de7d73c9d4ba2e1ff8d..9eb2e5fd9b1c644742a7d32d055591231f10c7aa 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.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--------
/*
* SE3.h
*
diff --git a/include/core/math/covariance.h b/include/core/math/covariance.h
index 45cbdf3e1366fab06820ca54fad65cfa7e3af5fd..acb4c086282ea48d37c0394aab2c18b75daaab0d 100644
--- a/include/core/math/covariance.h
+++ b/include/core/math/covariance.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--------
/**
* \file covariance.h
*
diff --git a/include/core/math/rotations.h b/include/core/math/rotations.h
index a0a86c6ee4c3ef095725a2df2230c2fdb8de751f..f33c18fcb075a9c2503556beb4907fd55ad4513e 100644
--- a/include/core/math/rotations.h
+++ b/include/core/math/rotations.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--------
/**
* \file rotations.h
*
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 7004a205cabee71ae225dbae91b9a047431e7a65..7405ae8320deb339ed6efdfe7319b4df8ec14cff 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.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 PROBLEM_H_
#define PROBLEM_H_
@@ -21,7 +42,7 @@ struct ParamsProcessorBase;
#include "core/utils/params_server.h"
#include "core/sensor/factory_sensor.h"
#include "core/processor/factory_processor.h"
-#include "core/processor/is_motion.h"
+#include <core/processor/motion_provider.h>
#include "core/state_block/state_composite.h"
// std includes
@@ -40,7 +61,6 @@ struct PriorOptions
std::string mode = "";
VectorComposite state;
MatrixComposite cov;
- double time_tolerance;
};
WOLF_STRUCT_PTR_TYPEDEFS(PriorOptions);
@@ -51,14 +71,14 @@ class Problem : public std::enable_shared_from_this<Problem>
friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
friend ProcessorBase;
friend ProcessorMotion;
- friend IsMotion;
+ friend MotionProvider;
protected:
TreeManagerBasePtr tree_manager_;
HardwareBasePtr hardware_ptr_;
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
- std::map<int, IsMotionPtr> processor_is_motion_map_;
+ std::map<int, MotionProviderPtr> motion_provider_map_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -173,13 +193,12 @@ class Problem : public std::enable_shared_from_this<Problem>
*
* Add a new processor of type is motion to the processor is motion list.
*/
- void addProcessorIsMotion(IsMotionPtr _processor_motion_ptr);
- void removeProcessorIsMotion(IsMotionPtr proc);
+ void addMotionProvider(MotionProviderPtr _processor_motion_ptr);
+ void removeMotionProvider(MotionProviderPtr proc);
public:
- IsMotionPtr getProcessorIsMotion();
- std::map<int,IsMotionPtr>& getProcessorIsMotionMap();
- const std::map<int,IsMotionPtr>& getProcessorIsMotionMap() const;
+ std::map<int,MotionProviderPtr>& getMotionProviderMap();
+ const std::map<int,MotionProviderPtr>& getMotionProviderMap() const;
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
@@ -187,20 +206,16 @@ class Problem : public std::enable_shared_from_this<Problem>
// Prior
bool isPriorSet() const;
void setPriorOptions(const std::string& _mode,
- const double _time_tolerance = 0,
const VectorComposite& _state = VectorComposite(),
const VectorComposite& _cov = VectorComposite());
FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
FrameBasePtr setPriorFactor(const VectorComposite &_state,
const VectorComposite &_sigma,
- const TimeStamp &_ts,
- const double &_time_tol);
+ const TimeStamp &_ts);
FrameBasePtr setPriorFix(const VectorComposite &_state,
- const TimeStamp &_ts,
- const double &_time_tol);
+ const TimeStamp &_ts);
FrameBasePtr setPriorInitialGuess(const VectorComposite &_state,
- const TimeStamp &_ts,
- const double &_time_tol);
+ const TimeStamp &_ts);
/** \brief Emplace frame from string frame_structure, dimension and vector
* \param _time_stamp Time stamp of the frame
@@ -247,7 +262,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(const TimeStamp& _time_stamp, //
- const VectorComposite& _frame_state);
+ const VectorComposite& _frame_state);
/** \brief Emplace frame from string frame_structure and dimension
* \param _time_stamp Time stamp of the frame
@@ -312,8 +327,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* New key frame callback: It should be called by any processor that creates a new key frame. It calls the keyFrameCallback of the rest of processors.
*/
void keyFrameCallback(FrameBasePtr _keyframe_ptr, //
- ProcessorBasePtr _processor_ptr, //
- const double& _time_tolerance);
+ ProcessorBasePtr _processor_ptr);
// State getters
TimeStamp getTimeStamp ( ) const;
@@ -397,7 +411,7 @@ class Problem : public std::enable_shared_from_this<Problem>
// Print and check ---------------------------------------
/**
* \brief print wolf tree
- * \param depth : levels to show ( 0: H, T, M : 1: H:S:p, T:F, M:L ; 2: T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
+ * \param depth : levels to show ( 0: H, T, M : 1: H:S, T:F, M:L ; 2: H:S:p, T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
* \param constr_by: show factors pointing to F, f and L.
* \param metric : show metric info (status, time stamps, state vectors, measurements)
* \param state_blocks : show state blocks
@@ -429,21 +443,14 @@ inline bool Problem::isPriorSet() const
return prior_options_ == nullptr;
}
-inline IsMotionPtr Problem::getProcessorIsMotion()
-{
- if (not processor_is_motion_map_.empty())
- return processor_is_motion_map_.begin()->second;
- return nullptr;
-}
-
-inline std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap()
+inline std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap()
{
- return processor_is_motion_map_;
+ return motion_provider_map_;
}
-inline const std::map<int,IsMotionPtr>& Problem::getProcessorIsMotionMap() const
+inline const std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap() const
{
- return processor_is_motion_map_;
+ return motion_provider_map_;
}
inline SizeStd Problem::getStateBlockNotificationMapSize() const
diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h
index e36e85a6d109572070bf2d317d81349df73f824a..74f01419ea00bfd01842364b5f79e46df02b525f 100644
--- a/include/core/processor/factory_processor.h
+++ b/include/core/processor/factory_processor.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--------
/**
* \file factory_processor.h
*
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
deleted file mode 100644
index a17ac88e7f2d78bc83e5866f10e85173af380638..0000000000000000000000000000000000000000
--- a/include/core/processor/is_motion.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/**
- * \file is_motion.h
- *
- * Created on: Mar 10, 2020
- * \author: jsola
- */
-
-#ifndef PROCESSOR_IS_MOTION_H_
-#define PROCESSOR_IS_MOTION_H_
-
-#include "core/common/wolf.h"
-#include "core/state_block/state_composite.h"
-#include "core/utils/params_server.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsIsMotion);
-
-struct ParamsIsMotion
-{
- bool state_getter = true;
- int state_priority = 1;
-
- ParamsIsMotion() = default;
- ParamsIsMotion(std::string _unique_name, const ParamsServer& _server)
- {
- state_getter = _server.getParam<bool>("processor/" + _unique_name + "/state_getter");
- state_priority = _server.getParam<double>("processor/" + _unique_name + "/state_priority");
- }
- std::string print() const
- {
- return "state_getter: " + std::to_string(state_getter) + "\n"
- + "state_priority: " + std::to_string(state_priority) + "\n";
- }
-
-};
-class TimeStamp;
-
-WOLF_PTR_TYPEDEFS(IsMotion);
-
-class IsMotion
-{
- public:
-
- IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params);
- virtual ~IsMotion();
-
- // Queries to the processor:
- virtual TimeStamp getTimeStamp() const = 0;
- virtual VectorComposite getState(const StateStructure& _structure = "") const = 0;
- virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
-
- VectorComposite getOdometry ( ) const;
- void setOdometry(const VectorComposite&);
-
- bool isStateGetter() const;
- int getStatePriority() const;
- void setStatePriority(int);
-
- public:
- const StateStructure& getStateStructure ( ) { return state_structure_; };
- void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
- void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr);
-
- protected:
- StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
- VectorComposite odometry_;
- ParamsIsMotionPtr params_is_motion_;
-};
-
-inline IsMotion::IsMotion(const StateStructure& _structure, ParamsIsMotionPtr _params) :
- state_structure_(_structure),
- params_is_motion_(_params)
-{
- //
-}
-
-inline wolf::VectorComposite IsMotion::getOdometry ( ) const
-{
- return odometry_;
-}
-
-}
-
-///// IMPLEMENTATION ///////
-namespace wolf{
-
-inline IsMotion::~IsMotion()
-{
-}
-
-inline bool IsMotion::isStateGetter() const
-{
- return params_is_motion_->state_getter;
-}
-
-inline int IsMotion::getStatePriority() const
-{
- return params_is_motion_->state_priority;
-}
-
-inline void IsMotion::setStatePriority(int _priority)
-{
- params_is_motion_->state_priority = _priority;
-}
-
-} /* namespace wolf */
-
-#endif /* PROCESSOR_IS_MOTION_H_ */
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index 548fcf6e1e4f99b9d020784b226f4f1df75e4b98..4763d0bc45d513611d34775754898da9290b7aec 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.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--------
/**
* \file motion_buffer.h
*
diff --git a/include/core/processor/motion_provider.h b/include/core/processor/motion_provider.h
new file mode 100644
index 0000000000000000000000000000000000000000..9a6e84d9c8949b4c9d5c5e69cdf3ec071d66fa57
--- /dev/null
+++ b/include/core/processor/motion_provider.h
@@ -0,0 +1,131 @@
+//--------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 motion_provider.h
+ *
+ * Created on: Mar 10, 2020
+ * \author: jsola
+ */
+
+#ifndef PROCESSOR_MOTION_PROVIDER_H_
+#define PROCESSOR_MOTION_PROVIDER_H_
+
+#include "core/common/wolf.h"
+#include "core/state_block/state_composite.h"
+#include "core/utils/params_server.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProvider);
+
+struct ParamsMotionProvider
+{
+ bool state_getter = true;
+ int state_priority = 1;
+
+ ParamsMotionProvider() = default;
+ ParamsMotionProvider(std::string _unique_name, const ParamsServer& _server)
+ {
+ state_getter = _server.getParam<bool>("processor/" + _unique_name + "/state_getter");
+ state_priority = _server.getParam<double>("processor/" + _unique_name + "/state_priority");
+ }
+ std::string print() const
+ {
+ return "state_getter: " + std::to_string(state_getter) + "\n"
+ + "state_priority: " + std::to_string(state_priority) + "\n";
+ }
+
+};
+class TimeStamp;
+
+WOLF_PTR_TYPEDEFS(MotionProvider);
+
+class MotionProvider
+{
+ public:
+
+ MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params);
+ virtual ~MotionProvider();
+
+ // Queries to the processor:
+ virtual TimeStamp getTimeStamp() const = 0;
+ virtual VectorComposite getState(const StateStructure& _structure = "") const = 0;
+ virtual VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const = 0;
+
+ VectorComposite getOdometry ( ) const;
+ void setOdometry(const VectorComposite&);
+
+ bool isStateGetter() const;
+ int getStatePriority() const;
+ void setStatePriority(int);
+
+ public:
+ const StateStructure& getStateStructure ( ) { return state_structure_; };
+ void setStateStructure(std::string _state_structure) { state_structure_ = _state_structure; };
+ void addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr);
+
+ protected:
+ StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
+ VectorComposite odometry_;
+ ParamsMotionProviderPtr params_motion_provider_;
+};
+
+inline MotionProvider::MotionProvider(const StateStructure& _structure, ParamsMotionProviderPtr _params) :
+ state_structure_(_structure),
+ params_motion_provider_(_params)
+{
+ //
+}
+
+inline wolf::VectorComposite MotionProvider::getOdometry ( ) const
+{
+ return odometry_;
+}
+
+}
+
+///// IMPLEMENTATION ///////
+namespace wolf{
+
+inline MotionProvider::~MotionProvider()
+{
+}
+
+inline bool MotionProvider::isStateGetter() const
+{
+ return params_motion_provider_->state_getter;
+}
+
+inline int MotionProvider::getStatePriority() const
+{
+ return params_motion_provider_->state_priority;
+}
+
+inline void MotionProvider::setStatePriority(int _priority)
+{
+ params_motion_provider_->state_priority = _priority;
+}
+
+} /* namespace wolf */
+
+#endif /* PROCESSOR_MOTION_PROVIDER_H_ */
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index bbb9d109f4ea76919f4c9ddac9c5960eb94de141..b2e0f6fdb42e6715b0ed41f3c720b18f8b12792e 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.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 PROCESSOR_BASE_H_
#define PROCESSOR_BASE_H_
@@ -9,7 +30,7 @@ class SensorBase;
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
-#include "core/processor/is_motion.h"
+#include <core/processor/motion_provider.h>
#include "core/sensor/sensor_base.h"
#include "core/frame/frame_base.h"
#include "core/common/time_stamp.h"
@@ -59,22 +80,6 @@ static ProcessorBasePtr create(const std::string& _unique_name, const ParamsProc
-/** \brief Key frame class pack
- *
- * To store a key_frame with an associated time tolerance.
- *
- * Used in keyframe callbacks as the minimal pack of information needed by the processor receiving the callback.
- */
-class PackKeyFrame
-{
- public:
- PackKeyFrame(const FrameBasePtr _key_frame, const double _time_tolerance) : key_frame(_key_frame), time_tolerance(_time_tolerance) {};
- ~PackKeyFrame(){};
- FrameBasePtr key_frame;
- double time_tolerance;
-};
-
-WOLF_PTR_TYPEDEFS(PackKeyFrame);
/** \brief Buffer for arbitrary type objects
@@ -91,16 +96,16 @@ public:
Buffer(){};
~Buffer(void){};
- /**\brief Select a Pack from the buffer
+ /**\brief Select an element from the buffer
*
- * Select from the buffer the closest pack (w.r.t. time stamp),
+ * Select from the buffer the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
T select(const TimeStamp& _time_stamp, const double& _time_tolerance);
- /**\brief Select a Pack iterator from the buffer
+ /**\brief Select an element iterator from the buffer
*
- * Select from the buffer the iterator pointing to the closest pack (w.r.t. time stamp),
+ * Select from the buffer the iterator pointing to the closest element (w.r.t. time stamp),
* respecting a defined time tolerances
*/
Iterator selectIterator(const TimeStamp& _time_stamp, const double& _time_tolerance);
@@ -118,10 +123,10 @@ public:
*/
SizeStd size(void);
- /**\brief Add a pack to the buffer
+ /**\brief Add a element to the buffer
*
*/
- void add(const TimeStamp& _time_stamp, const T& _element); //const double& _time_tolerance);
+ void emplace(const TimeStamp& _time_stamp, const T& _element);
/** \brief returns the container with elements of the buffer
*
@@ -129,12 +134,12 @@ public:
*/
std::map<TimeStamp,T>& getContainer();
- /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ /**\brief Remove all elements in the buffer with a time stamp older than the specified
*
*/
void removeUpTo(const TimeStamp& _time_stamp);
- /**\brief Remove all packs in the buffer with a time stamp older than the specified
+ /**\brief Remove all elements in the buffer with a time stamp older than the specified
*
*/
void removeUpToLower(const TimeStamp& _time_stamp);
@@ -149,12 +154,13 @@ public:
*/
bool empty();
+protected:
/**\brief Check time tolerance
*
* Check if the time distance between two time stamps is smaller than
* the time tolerance.
*/
- static bool simpleCheckTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance);
+ static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2, const double& _time_tolerance);
/**\brief Check time tolerance
*
@@ -169,47 +175,16 @@ protected:
};
-/** \brief Buffer of Key frame class objects
+/** \brief Buffer of Frames
*
- * Object and functions to manage a buffer of KFPack objects.
+ * Object and functions to manage a buffer of FrameBasePtr objects.
*/
-class BufferPackKeyFrame : public Buffer<PackKeyFramePtr>
-{
- public:
+class BufferFrame : public Buffer<FrameBasePtr> { };
- /**\brief Select a Pack from the buffer
- *
- * Select from the buffer the closest pack (w.r.t. time stamp),
- * respecting a defined time tolerances
- */
- PackKeyFramePtr selectPack(const TimeStamp& _time_stamp, const double& _time_tolerance);
- PackKeyFramePtr selectPack(const CaptureBasePtr _capture, const double& _time_tolerance);
-
- PackKeyFramePtr selectFirstPackBefore(const TimeStamp& _time_stamp, const double& _time_tolerance);
- PackKeyFramePtr selectFirstPackBefore(const CaptureBasePtr _capture, const double& _time_tolerance);
-
- /**\brief Add a pack to the buffer
- *
- */
- void add(const FrameBasePtr& _key_frame, const double& _time_tolerance);
-
- /**\brief Print buffer information
- *
- */
- void print() const;
- /**\brief Alias funct
- *
- */
- static bool checkTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1, const TimeStamp& _time_stamp2, const double& _time_tolerance2)
- { return doubleCheckTimeTolerance(_time_stamp1, _time_tolerance1, _time_stamp2, _time_tolerance2); };
-
-};
-
-
-/** \brief Buffer of Capture class objects
+/** \brief Buffer of Captures
*
- * Object and functions to manage a buffer of Capture objects.
+ * Object and functions to manage a buffer of CaptureBasePtr objects.
*/
class BufferCapture : public Buffer<CaptureBasePtr> {};
@@ -255,7 +230,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
protected:
unsigned int processor_id_;
ParamsProcessorBasePtr params_;
- BufferPackKeyFrame buffer_pack_kf_;
+ BufferFrame buffer_frame_;
BufferCapture buffer_capture_;
int dim_;
@@ -299,7 +274,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
* Each derived processor should implement this function. It will be called if:
* - A new KF arrived and triggerInKF() returned true.
*/
- virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) = 0;
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) = 0;
/** \brief trigger in capture
*
@@ -311,7 +286,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
*
* Returns true if processKeyFrame() should be called after the provided KF arrived.
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const = 0;
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const = 0;
/** \brief store key frame
*
@@ -341,25 +316,24 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
public:
/**\brief notify a new keyframe made by another processor
*
- * It stores the new KF in buffer_pack_kf_ and calls triggerInKF()
+ * It stores the new KF in buffer_frame_ and calls triggerInKF()
*
*/
- void keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other);
+ void keyFrameCallback(FrameBasePtr _keyframe);
/**\brief notify a new capture
*
* It stores the new capture in buffer_capture_ and calls triggerInCapture()
*/
- void captureCallback(CaptureBasePtr _capture_ptr);
+ void captureCallback(CaptureBasePtr _capture);
SensorBasePtr getSensor() const;
private:
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
public:
- bool isMotion() const;
+ bool isMotionProvider() const;
- void setTimeTolerance(double _time_tolerance);
bool isVotingActive() const;
@@ -367,7 +341,13 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
int getDim() const;
+ void setTimeTolerance(double _time_tolerance);
double getTimeTolerance() const;
+ bool checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2);
+ bool checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts);
+ bool checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts);
+ bool checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap);
+
void link(SensorBasePtr);
template<typename classType, typename... T>
@@ -427,10 +407,10 @@ inline void ProcessorBase::setVotingActive(bool _voting_active)
params_->voting_active = _voting_active;
}
-inline bool ProcessorBase::isMotion() const
+inline bool ProcessorBase::isMotionProvider() const
{
- // check if this inherits from IsMotion
- return (std::dynamic_pointer_cast<const IsMotion>(shared_from_this()) != nullptr);
+ // check if this inherits from MotionProvider
+ return (std::dynamic_pointer_cast<const MotionProvider>(shared_from_this()) != nullptr);
}
inline unsigned int ProcessorBase::id() const
@@ -443,14 +423,15 @@ inline SensorBasePtr ProcessorBase::getSensor() const
return sensor_ptr_.lock();
}
-inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
-{
- params_->time_tolerance = _time_tolerance;
-}
inline int ProcessorBase::getDim() const
{
return dim_;
}
+
+inline void ProcessorBase::setTimeTolerance(double _time_tolerance)
+{
+ params_->time_tolerance = _time_tolerance;
+}
inline double ProcessorBase::getTimeTolerance() const
{
return params_->time_tolerance;
@@ -475,13 +456,13 @@ typename Buffer<T>::Iterator Buffer<T>::selectIterator(const TimeStamp& _time_st
bool prev_exists = (post != container_.begin());
bool post_exists = (post != container_.end());
- bool post_ok = post_exists && simpleCheckTimeTolerance(post->first, _time_stamp, _time_tolerance);
+ bool post_ok = post_exists && checkTimeTolerance(post->first, _time_stamp, _time_tolerance);
if (prev_exists)
{
Buffer<T>::Iterator prev = std::prev(post);
- bool prev_ok = simpleCheckTimeTolerance(prev->first, _time_stamp, _time_tolerance);
+ bool prev_ok = checkTimeTolerance(prev->first, _time_stamp, _time_tolerance);
if (prev_ok && !post_ok)
return prev;
@@ -527,16 +508,16 @@ T Buffer<T>::selectFirstBefore(const TimeStamp& _time_stamp, const double& _time
if (container_.empty())
return nullptr;
- // Checking on begin() since packs are ordered in time
- // Return first pack if is older than time stamp
+ // Checking on begin() since elements are ordered in time
+ // Return first element if is older than time stamp
if (container_.begin()->first < _time_stamp)
return container_.begin()->second;
- // Return first pack if despite being newer, it is within the time tolerance
- if (simpleCheckTimeTolerance(container_.begin()->first, _time_stamp, _time_tolerance))
+ // Return first element if despite being newer, it is within the time tolerance
+ if (checkTimeTolerance(container_.begin()->first, _time_stamp, _time_tolerance))
return container_.begin()->second;
- // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
+ // otherwise return nullptr (no element before the provided ts or within the tolerance was found)
return nullptr;
}
@@ -548,16 +529,16 @@ T Buffer<T>::selectLastAfter(const TimeStamp& _time_stamp, const double& _time_t
if (container_.empty())
return nullptr;
- // Checking on rbegin() since packs are ordered in time
- // Return last pack if is newer than time stamp
+ // Checking on rbegin() since elements are ordered in time
+ // Return last element if is newer than time stamp
if (container_.rbegin()->first > _time_stamp)
return container_.rbegin()->second;
- // Return last pack if despite being older, it is within the time tolerance
- if (simpleCheckTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
+ // Return last element if despite being older, it is within the time tolerance
+ if (checkTimeTolerance(container_.rbegin()->first, _time_stamp, _time_tolerance))
return container_.rbegin()->second;
- // otherwise return nullptr (no pack after the provided ts or within the tolerance was found)
+ // otherwise return nullptr (no element after the provided ts or within the tolerance was found)
return nullptr;
}
@@ -584,7 +565,7 @@ T Buffer<T>::selectLast()
}
template <typename T>
-void Buffer<T>::add(const TimeStamp& _time_stamp, const T& _element)
+void Buffer<T>::emplace(const TimeStamp& _time_stamp, const T& _element)
{
container_.emplace(_time_stamp, _element);
}
@@ -628,8 +609,10 @@ inline void Buffer<T>::removeUpToLower(const TimeStamp& _time_stamp)
}
template <typename T>
-inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, const double& _time_tolerance1,
- const TimeStamp& _time_stamp2, const double& _time_tolerance2)
+inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1,
+ const double& _time_tolerance1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance2)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
double time_tol = std::min(_time_tolerance1, _time_tolerance2);
@@ -638,8 +621,9 @@ inline bool Buffer<T>::doubleCheckTimeTolerance(const TimeStamp& _time_stamp1, c
}
template <typename T>
-inline bool Buffer<T>::simpleCheckTimeTolerance(const TimeStamp& _time_stamp1, const TimeStamp& _time_stamp2,
- const double& _time_tolerance)
+inline bool Buffer<T>::checkTimeTolerance(const TimeStamp& _time_stamp1,
+ const TimeStamp& _time_stamp2,
+ const double& _time_tolerance)
{
double time_diff = std::fabs(_time_stamp1 - _time_stamp2);
bool pass = time_diff <= _time_tolerance;
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 7203ff3fd48833c5be92de7445dc174193e6a3e1..e9cf00b45a14bbf5ddbab51c8cbfd093d02cd985 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.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--------
/**
* \file processor_diff_drive.h
*
diff --git a/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fixed_wing_model.h
similarity index 52%
rename from include/core/processor/processor_fix_wing_model.h
rename to include/core/processor/processor_fixed_wing_model.h
index 8c23917354ca1d0e0c3723045ded8d118e3c3abc..bccbc2317135376f7cf7dd1f70113fb933fc72b0 100644
--- a/include/core/processor/processor_fix_wing_model.h
+++ b/include/core/processor/processor_fixed_wing_model.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--------
/*
* processor_fix_wing_model.h
*
@@ -5,31 +26,31 @@
* Author: joanvallve
*/
-#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
-#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
+#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_
+#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_
#include "core/processor/processor_base.h"
namespace wolf
{
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixWingModel);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixedWingModel);
-struct ParamsProcessorFixWingModel : public ParamsProcessorBase
+struct ParamsProcessorFixedWingModel : public ParamsProcessorBase
{
Eigen::Vector3d velocity_local;
double angle_stdev;
double min_vel_norm;
- ParamsProcessorFixWingModel() = default;
- ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
+ ParamsProcessorFixedWingModel() = default;
+ ParamsProcessorFixedWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorBase(_unique_name, _server)
{
velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev");
min_vel_norm = _server.getParam<double> (prefix + _unique_name + "/min_vel_norm");
- assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixWingModel: 'velocity_local' must be normalized");
+ assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixedWingModel: 'velocity_local' must be normalized");
}
std::string print() const override
{
@@ -40,17 +61,17 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
}
};
-WOLF_PTR_TYPEDEFS(ProcessorFixWingModel);
+WOLF_PTR_TYPEDEFS(ProcessorFixedWingModel);
-class ProcessorFixWingModel : public ProcessorBase
+class ProcessorFixedWingModel : public ProcessorBase
{
public:
- ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params);
+ ProcessorFixedWingModel(ParamsProcessorFixedWingModelPtr _params);
// Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
+ WOLF_PROCESSOR_CREATE(ProcessorFixedWingModel, ParamsProcessorFixedWingModel);
- virtual ~ProcessorFixWingModel() override;
+ virtual ~ProcessorFixedWingModel() override;
void configure(SensorBasePtr _sensor) override;
protected:
@@ -61,7 +82,7 @@ class ProcessorFixWingModel : public ProcessorBase
/** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
*/
- virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr) override;
/** \brief trigger in capture
*/
@@ -69,7 +90,7 @@ class ProcessorFixWingModel : public ProcessorBase
/** \brief trigger in key-frame
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return true;};
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return true;};
/** \brief store key frame
*/
@@ -84,9 +105,9 @@ class ProcessorFixWingModel : public ProcessorBase
virtual bool voteForKeyFrame() const override {return false;};
// ATTRIBUTES
- ParamsProcessorFixWingModelPtr params_processor_;
+ ParamsProcessorFixedWingModelPtr params_processor_;
};
} /* namespace wolf */
-#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_ */
+#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIXED_WING_MODEL_H_ */
diff --git a/include/core/processor/processor_logging.h b/include/core/processor/processor_logging.h
deleted file mode 100644
index bc1f8399aadc6ac344973baa377b8ea8bdb85db7..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_logging.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/**
- * \file processor_logging.h
- *
- * Created on: Oct 5, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_PROCESSOR_LOGGING_H_
-#define _WOLF_PROCESSOR_LOGGING_H_
-
-/// @brief un-comment for IDE highlights.
-
-
-#define __INTERNAL_WOLF_ASSERT_PROCESSOR \
- static_assert(std::is_base_of<ProcessorBase, \
- typename std::remove_pointer<decltype(this)>::type>::value, \
- "This macro can be used only within the body of a " \
- "non-static " \
- "ProcessorBase (and derived) function !");
-
-#define WOLF_PROCESSOR_INFO(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_INFO_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_WARN(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_WARN_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_ERROR(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_ERROR_NAMED(getType(), __VA_ARGS__);
-#define WOLF_PROCESSOR_DEBUG(...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_DEBUG_NAMED(getType(), __VA_ARGS__);
-
-#define WOLF_PROCESSOR_INFO_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_INFO_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_WARN_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_WARN_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_ERROR_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_ERROR_NAMED_COND(getType(), cond, __VA_ARGS__);
-#define WOLF_PROCESSOR_DEBUG_COND(cond, ...) __INTERNAL_WOLF_ASSERT_PROCESSOR WOLF_DEBUG_NAMED_COND(getType(), cond, __VA_ARGS__);
-
-#endif /* _WOLF_PROCESSOR_LOGGING_H_ */
diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h
index 94af3fa1692397e533db0379a342a39edebb51e0..b6e4d965ba4927b481f85687fe0cee2580253bcd 100644
--- a/include/core/processor/processor_loop_closure.h
+++ b/include/core/processor/processor_loop_closure.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 _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
#define _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
@@ -94,10 +115,10 @@ class ProcessorLoopClosure : public ProcessorBase
virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
void processCapture(CaptureBasePtr) override;
- void processKeyFrame(FrameBasePtr, const double&) override;
+ void processKeyFrame(FrameBasePtr _frm) override;
bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
- bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+ bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;};
bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
bool storeCapture(CaptureBasePtr _cap) override { return false;};
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 207bacb8eb7c9770787e00e9a811b30543708c51..66f40e6e62e32e6312270fe994e7e38ceb426822 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.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--------
/**
* \file processor_motion.h
*
@@ -9,9 +30,9 @@
#define PROCESSOR_MOTION_H_
// Wolf
+#include <core/processor/motion_provider.h>
#include "core/capture/capture_motion.h"
#include "core/processor/processor_base.h"
-#include "core/processor/is_motion.h"
#include "core/common/time_stamp.h"
#include "core/utils/params_server.h"
@@ -23,7 +44,7 @@ namespace wolf
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorMotion);
-struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion
+struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsMotionProvider
{
double max_time_span = 0.5;
unsigned int max_buff_length = 10;
@@ -34,7 +55,7 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion
ParamsProcessorMotion() = default;
ParamsProcessorMotion(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorBase(_unique_name, _server),
- ParamsIsMotion(_unique_name, _server)
+ ParamsMotionProvider(_unique_name, _server)
{
max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span");
max_buff_length = _server.getParam<unsigned int>(prefix + _unique_name + "/keyframe_vote/max_buff_length");
@@ -45,7 +66,7 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion
std::string print() const override
{
return ParamsProcessorBase::print() + "\n" +
- ParamsIsMotion::print() + "\n"
+ ParamsMotionProvider::print() + "\n"
+ "max_time_span: " + std::to_string(max_time_span) + "\n"
+ "max_buff_length: " + std::to_string(max_buff_length) + "\n"
+ "dist_traveled: " + std::to_string(dist_traveled) + "\n"
@@ -132,7 +153,7 @@ struct ParamsProcessorMotion : public ParamsProcessorBase, public ParamsIsMotion
* // TODO: JS: review instructions up to here
*
*/
-class ProcessorMotion : public ProcessorBase, public IsMotion
+class ProcessorMotion : public ProcessorBase, public MotionProvider
{
public:
typedef enum {
@@ -219,7 +240,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*
* The ProcessorMotion only processes incoming captures (it is not called).
*/
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) override {};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
/** \brief trigger in capture
*
@@ -231,19 +252,19 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*
* The ProcessorMotion only processes incoming captures, then it returns false.
*/
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) 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
*/
- bool storeKeyFrame(FrameBasePtr) override;
+ bool storeKeyFrame(FrameBasePtr) override { return true;}
/** \brief store capture
*
* Returns true if the capture should be stored
*/
- bool storeCapture(CaptureBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override { return false;}
bool voteForKeyFrame() const override;
@@ -251,8 +272,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
void integrateOneStep();
void reintegrateBuffer(CaptureMotionPtr _capture_ptr) const;
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
- TimeStamp ts_split,
- const FrameBasePtr& keyframe_target,
+ const TimeStamp ts_split,
const wolf::CaptureMotionPtr& capture_target) const;
/** Pre-process incoming Capture
@@ -280,7 +300,9 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
*/
virtual void postProcess(){ };
- PackKeyFramePtr computeProcessingStep();
+ FrameBasePtr computeProcessingStep();
+
+
// These are the pure virtual functions doing the mathematics
public:
@@ -520,6 +542,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
+
}
#include "core/frame/frame_base.h"
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index 254eded5ccc644faf5b6c20e128f55da77c92b95..f3a50080d135b9c7d19444c2b7bed4f0237f6e2a 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.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--------
/**
* \file processor_odom_2d.h
*
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 95035e56abd313b123c61aebf6068b862b6b794e..83baf370b1b536bc8e472cab19200abdc34d0314 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.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--------
/**
* \file processor_odom_3d.h
*
diff --git a/include/core/processor/processor_pose.h b/include/core/processor/processor_pose.h
index 0e0a68527a194bf071c38ee2ffadf561de2524ea..ca5cd69cd5c06f9455c0948cd1a476b76a882303 100644
--- a/include/core/processor/processor_pose.h
+++ b/include/core/processor/processor_pose.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 PROCESSOR_POSE_NOMOVE_H
#define PROCESSOR_POSE_NOMOVE_H
@@ -37,10 +58,10 @@ class ProcessorPose : public ProcessorBase{
void configure(SensorBasePtr _sensor) override;
- void processCapture(CaptureBasePtr) override;
- void processKeyFrame(FrameBasePtr, const double&) override;
+ void processCapture(CaptureBasePtr _cap) override;
+ void processKeyFrame(FrameBasePtr _frm) override;
bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
- bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+ bool triggerInKeyFrame(FrameBasePtr _frm) const override { return true;};
bool storeKeyFrame(FrameBasePtr _frm) override { return true;};
bool storeCapture(CaptureBasePtr _cap) override { return true;};
bool voteForKeyFrame() const override { return false;};
diff --git a/include/core/processor/processor_tracker.h b/include/core/processor/processor_tracker.h
index 855d30ee277a718826870f150d6bc13d789b0931..f03ed4ea4d5e2ae46864479e65ad19c7d16aa9a9 100644
--- a/include/core/processor/processor_tracker.h
+++ b/include/core/processor/processor_tracker.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--------
/*
* processor_tracker.h
*
@@ -87,12 +108,12 @@ class ProcessorTracker : public ProcessorBase
{
public:
typedef enum {
- FIRST_TIME_WITH_PACK,
- FIRST_TIME_WITHOUT_PACK,
- SECOND_TIME_WITH_PACK,
- SECOND_TIME_WITHOUT_PACK,
- RUNNING_WITH_PACK,
- RUNNING_WITHOUT_PACK
+ FIRST_TIME_WITH_KEYFRAME,
+ FIRST_TIME_WITHOUT_KEYFRAME,
+ SECOND_TIME_WITH_KEYFRAME,
+ SECOND_TIME_WITHOUT_KEYFRAME,
+ RUNNING_WITH_KEYFRAME,
+ RUNNING_WITHOUT_KEYFRAME
} ProcessingStep ;
protected:
@@ -117,11 +138,6 @@ class ProcessorTracker : public ProcessorBase
StateStructure getStateStructure() const;
- bool checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2);
- bool checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts);
- bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
- bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
-
virtual CaptureBasePtr getOrigin() const;
virtual CaptureBasePtr getLast() const;
virtual CaptureBasePtr getIncoming() const;
@@ -138,31 +154,31 @@ class ProcessorTracker : public ProcessorBase
*
* The ProcessorTracker only processes incoming captures (it is not called).
*/
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _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.
*/
- bool triggerInCapture(CaptureBasePtr) const override;
+ 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_ptr, const double& _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
*/
- bool storeKeyFrame(FrameBasePtr) override;
+ bool storeKeyFrame(FrameBasePtr) override { return true;}
/** \brief store capture
*
* Returns true if the capture should be stored
*/
- bool storeCapture(CaptureBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override { return false;}
/** Pre-process incoming Capture
*
@@ -285,26 +301,6 @@ inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
return new_features_incoming_;
}
-inline bool ProcessorTracker::checkTimeTolerance(const TimeStamp& _ts1, const TimeStamp& _ts2)
-{
- return (std::fabs(_ts2 - _ts2) < params_tracker_->time_tolerance);
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const CaptureBasePtr _cap, const TimeStamp& _ts)
-{
- return checkTimeTolerance(_cap->getTimeStamp(), _ts);
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts)
-{
- return checkTimeTolerance(_frm->getTimeStamp(), _ts);
-}
-
-inline bool ProcessorTracker::checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap)
-{
- return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
-}
-
inline StateStructure ProcessorTracker::getStateStructure ( ) const
{
return state_structure_;
diff --git a/include/core/processor/processor_tracker_feature.h b/include/core/processor/processor_tracker_feature.h
index 86fc8f35a78a78d882820f3512a6bcec12eeb193..f9c954bf2fe4c0877c01bb75833f8e56bb687417 100644
--- a/include/core/processor/processor_tracker_feature.h
+++ b/include/core/processor/processor_tracker_feature.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--------
/*
* \processor_tracker_feature.h
*
diff --git a/include/core/processor/processor_tracker_landmark.h b/include/core/processor/processor_tracker_landmark.h
index 23f0b89fc03432ea411227ca9de85dc40407693e..bbccccf950aa02f499d215a21f5488a724330435 100644
--- a/include/core/processor/processor_tracker_landmark.h
+++ b/include/core/processor/processor_tracker_landmark.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--------
/**
* \file processor_tracker_landmark.h
*
diff --git a/include/core/processor/track_matrix.h b/include/core/processor/track_matrix.h
index 1d8bfb343d851a7c5890a8442d844d3bc7b44e9e..87511440e721baf20d2b3aa05123f0499caa4c84 100644
--- a/include/core/processor/track_matrix.h
+++ b/include/core/processor/track_matrix.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--------
/**
* \file track_matrix.h
*
diff --git a/include/core/sensor/factory_sensor.h b/include/core/sensor/factory_sensor.h
index fb75e84cd1c27c6f3681d1a6a5065ba6d29ac9f4..3a77700e87d23ffdc58418046c6da4f573d55d93 100644
--- a/include/core/sensor/factory_sensor.h
+++ b/include/core/sensor/factory_sensor.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--------
/**
* \file factory_sensor.h
*
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index b80ac75c31d27014cb1affc8d9012e60b6476017..0fdcac98b7c6c317b29f2a6dd718ae155db6e018 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.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_BASE_H_
#define SENSOR_BASE_H_
@@ -33,29 +54,29 @@ namespace wolf {
*
* We recommend writing one of such constructors in your derived sensors.
*/
-#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass, ExtrinsicsSize) \
-static \
-SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \
-{ \
+#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass, ExtrinsicsSize) \
+static \
+SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \
+{ \
Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose"); \
- \
- assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
- \
- auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \
- \
- auto sensor = std::make_shared<SensorClass>(extrinsics, params); \
- \
- sensor ->setName(_unique_name); \
- \
- return sensor; \
-} \
- \
+ \
+ assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
+ \
+ auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \
+ \
+ auto sensor = std::make_shared<SensorClass>(extrinsics, params); \
+ \
+ sensor ->setName(_unique_name); \
+ \
+ return sensor; \
+} \
+ \
static \
-SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics) \
+SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics)\
{ \
assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
\
- auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \
+ auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \
\
auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \
\
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 212a955b19c1a76aecebaa90f7cc39be73f38a7a..5d1f2bb0c3ef4e9068673ea1a855289993bdcebd 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.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--------
/**
* \file sensor_diff_drive.h
*
diff --git a/include/core/sensor/sensor_model.h b/include/core/sensor/sensor_model.h
deleted file mode 100644
index 5e2a37df70243f7d331dadde464fffd1d7f081ef..0000000000000000000000000000000000000000
--- a/include/core/sensor/sensor_model.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#ifndef SRC_SENSOR_MODEL_H_
-#define SRC_SENSOR_MODEL_H_
-
-//wolf includes
-#include "core/sensor/sensor_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(SensorModel);
-
-class SensorModel : public SensorBase
-{
- public:
- SensorModel();
- ~SensorModel() override;
-
- static SensorBasePtr create(const std::string& _unique_name,
- const ParamsServer& _server)
- {
- auto sensor = std::make_shared<SensorModel>();
- sensor ->setName(_unique_name);
- return sensor;
- }
-
- static SensorBasePtr create(const std::string& _unique_name,
- const Eigen::VectorXd& _extrinsics,
- const ParamsSensorBasePtr _intrinsics)
- {
- auto sensor = std::make_shared<SensorModel>();
- sensor ->setName(_unique_name);
- return sensor;
- }
-};
-
-
-} /* namespace wolf */
-
-#endif /* SRC_SENSOR_POSE_H_ */
diff --git a/include/core/sensor/sensor_motion_model.h b/include/core/sensor/sensor_motion_model.h
new file mode 100644
index 0000000000000000000000000000000000000000..68da8c7d481aa502c8352a350e03498de7f758e0
--- /dev/null
+++ b/include/core/sensor/sensor_motion_model.h
@@ -0,0 +1,60 @@
+//--------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 SRC_SENSOR_MOTION_MODEL_H_
+#define SRC_SENSOR_MOTION_MODEL_H_
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+
+namespace wolf {
+
+
+WOLF_PTR_TYPEDEFS(SensorMotionModel);
+
+class SensorMotionModel : public SensorBase
+{
+ public:
+ SensorMotionModel();
+ ~SensorMotionModel() override;
+
+ static SensorBasePtr create(const std::string& _unique_name,
+ const ParamsServer& _server)
+ {
+ auto sensor = std::make_shared<SensorMotionModel>();
+ sensor ->setName(_unique_name);
+ return sensor;
+ }
+
+ static SensorBasePtr create(const std::string& _unique_name,
+ const Eigen::VectorXd& _extrinsics,
+ const ParamsSensorBasePtr _intrinsics)
+ {
+ auto sensor = std::make_shared<SensorMotionModel>();
+ sensor ->setName(_unique_name);
+ return sensor;
+ }
+};
+
+
+} /* namespace wolf */
+
+#endif /* SRC_SENSOR_MOTION_MODEL_H_ */
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index 0859f15bcee22d3be1c1857365d03d50f0315962..a734043ce8a94776831d3ed43819d0c321935e16 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/include/core/sensor/sensor_odom_2d.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_ODOM_2d_H_
#define SENSOR_ODOM_2d_H_
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
index d5f5b417eb36a0628a3425822d03b3fef494fb36..2b94194e2e1cb37fe33a34008d0647e7130e43c9 100644
--- a/include/core/sensor/sensor_odom_3d.h
+++ b/include/core/sensor/sensor_odom_3d.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--------
/**
* \file sensor_odom_3d.h
*
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index 4df6215468e6aabba07caee1b8d28af225d6fccc..e653fe458d39107c143da729a006c727df3959e3 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/include/core/sensor/sensor_pose.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--------
/**
* \file sensor_pose.h
*
diff --git a/include/core/solver/factory_solver.h b/include/core/solver/factory_solver.h
index 3e13e598fbd2726532333f2a71d4eabc5adcc1fb..fc8e5107093de4044f7ef65be490d88cf5af3c9f 100644
--- a/include/core/solver/factory_solver.h
+++ b/include/core/solver/factory_solver.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--------
/**
* \file factory_solver.h
*
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index f3fbeefb3dce65e58ad82d3c604ba3e554169624..c98804624b249ef71faddf8509116ee35bd937e0 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.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 _WOLF_SOLVER_MANAGER_H_
#define _WOLF_SOLVER_MANAGER_H_
@@ -38,7 +59,7 @@ static SolverManagerPtr create(const ProblemPtr& _problem,
return std::make_shared<SolverClass>(_problem, params); \
} \
- struct ParamsSolver;
+struct ParamsSolver;
/**
* \brief Solver manager for WOLF
@@ -115,23 +136,12 @@ class SolverManager
*/
std::string solve(const ReportVerbosity report_level);
+ virtual bool computeCovariances() final;
+
virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
- virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
-
- virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
-
- virtual bool hasConverged() = 0;
-
- virtual SizeStd iterations() = 0;
-
- virtual double initialCost() = 0;
-
- virtual double finalCost() = 0;
-
- virtual double totalTime() = 0;
/**
* \brief Updates solver's problem according to the wolf_problem
@@ -140,17 +150,22 @@ class SolverManager
ProblemPtr getProblem();
+ virtual ParamsSolverPtr getParams() const;
+
double getPeriod() const;
+ double getCovPeriod() const;
+
ReportVerbosity getVerbosity() const;
virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
- virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
+ virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
+
virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) final;
@@ -186,22 +201,37 @@ class SolverManager
virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
+ // Derived virtual functions
protected:
- // Derived virtual functions
virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
+ virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
+ virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
+
virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
+
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
- virtual bool checkDerived(std::string prefix="") const = 0;
virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
+ virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) = 0;
- virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
+
+ virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0;
+ virtual bool checkDerived(std::string prefix="") const = 0;
+
+ public:
+ virtual bool hasConverged() = 0;
+ virtual bool wasStopped() = 0;
+ virtual unsigned int iterations() = 0;
+ virtual double initialCost() = 0;
+ virtual double finalCost() = 0;
+ virtual double totalTime() = 0;
+
};
// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
@@ -210,6 +240,9 @@ struct ParamsSolver: public ParamsBase
std::string prefix = "solver/";
double period = 0.0;
SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET;
+ bool compute_cov = false;
+ SolverManager::CovarianceBlocksToBeComputed cov_enum = SolverManager::CovarianceBlocksToBeComputed::ROBOT_LANDMARKS;
+ double cov_period = 1.0;
ParamsSolver() = default;
ParamsSolver(std::string _unique_name, const ParamsServer& _server):
@@ -217,10 +250,20 @@ struct ParamsSolver: public ParamsBase
{
period = _server.getParam<double>(prefix + "period");
verbose = (SolverManager::ReportVerbosity)_server.getParam<int>(prefix + "verbose");
+ compute_cov = _server.getParam<bool>(prefix + "compute_cov");
+ if (compute_cov)
+ {
+ cov_enum = (SolverManager::CovarianceBlocksToBeComputed)_server.getParam<int>(prefix + "cov_enum");
+ cov_period = _server.getParam<double>(prefix + "cov_period");
+ }
}
std::string print() const override
{
- return "period: " + std::to_string(period) + "\n";
+ return "period: " + std::to_string(period) + "\n" +
+ "verbose: " + std::to_string((int)verbose) + "\n" +
+ "compute_cov: " + std::to_string(compute_cov) + "\n" +
+ "cov_enum: " + std::to_string((int)cov_enum) + "\n" +
+ "cov_period: " + std::to_string(cov_period) + "\n";
}
~ParamsSolver() override = default;
diff --git a/include/core/solver_suitesparse/ccolamd_ordering.h b/include/core/solver_suitesparse/ccolamd_ordering.h
index b20b857bdf543f65ec61d23a06a3a6ca9de567cb..4683373559f7df440b834c01c7552628e4f4b7b1 100644
--- a/include/core/solver_suitesparse/ccolamd_ordering.h
+++ b/include/core/solver_suitesparse/ccolamd_ordering.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--------
/*
* ccolamd_ordering.h
*
diff --git a/include/core/solver_suitesparse/cost_function_base.h b/include/core/solver_suitesparse/cost_function_base.h
index 523e2147ace0242d5b4739480a59912686fd07cd..fb094beedc0868846edfdbce7a0c64ddce3fb6a6 100644
--- a/include/core/solver_suitesparse/cost_function_base.h
+++ b/include/core/solver_suitesparse/cost_function_base.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--------
/*
* cost_function_base.h
*
diff --git a/include/core/solver_suitesparse/cost_function_sparse.h b/include/core/solver_suitesparse/cost_function_sparse.h
index 806e4d0405f10192e2170dcf264e78d289cc9bde..43e367f4f0975b32f0cb9333ffbb5094bf9f56ab 100644
--- a/include/core/solver_suitesparse/cost_function_sparse.h
+++ b/include/core/solver_suitesparse/cost_function_sparse.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 TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
diff --git a/include/core/solver_suitesparse/cost_function_sparse_base.h b/include/core/solver_suitesparse/cost_function_sparse_base.h
index 78bfd843d8d89ed05b21b7ef9cd568db2ef705fa..8c752208bf2eabd2981fa73c1de2b1ada8dfcefc 100644
--- a/include/core/solver_suitesparse/cost_function_sparse_base.h
+++ b/include/core/solver_suitesparse/cost_function_sparse_base.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--------
/*
* cost_function_sparse.h
*
diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h
index b89d49c8cac06092aaaada411598c20514fab574..83c392894a0a63b06c9b86a8d8b31907a2803bf9 100644
--- a/include/core/solver_suitesparse/qr_solver.h
+++ b/include/core/solver_suitesparse/qr_solver.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--------
/*
* qr_solver.h
*
diff --git a/include/core/solver_suitesparse/solver_QR.h b/include/core/solver_suitesparse/solver_QR.h
index e625515d2fcba147bb6cda32e1697bf893010551..3f0f689fec8a1d3aefd293ac50b2a30515da2f1f 100644
--- a/include/core/solver_suitesparse/solver_QR.h
+++ b/include/core/solver_suitesparse/solver_QR.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--------
/*
* solver_QR.h
*
diff --git a/include/core/solver_suitesparse/solver_manager.h b/include/core/solver_suitesparse/solver_manager.h
index 449c8eee3a7b1d7ce54782300017604c51d1843c..f3baf950fc917017d26ecfe95213205c1a92cffc 100644
--- a/include/core/solver_suitesparse/solver_manager.h
+++ b/include/core/solver_suitesparse/solver_manager.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 CERES_MANAGER_H_
#define CERES_MANAGER_H_
diff --git a/include/core/solver_suitesparse/sparse_utils.h b/include/core/solver_suitesparse/sparse_utils.h
index 7f5e0a0d0d2a56c9c798509c8b092eba0a35d855..393a2fb275507aa1e9b5b6c67172a6b589be4f9f 100644
--- a/include/core/solver_suitesparse/sparse_utils.h
+++ b/include/core/solver_suitesparse/sparse_utils.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--------
/*
* sparse_utils.h
*
diff --git a/include/core/state_block/factory_state_block.h b/include/core/state_block/factory_state_block.h
index 4e1028bf954a7c2ac71e4d133a097700a6150283..c440584c0539625d93b4fa4bc5bbafaa1cad1461 100644
--- a/include/core/state_block/factory_state_block.h
+++ b/include/core/state_block/factory_state_block.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--------
/*
* \file factory_state_block.h
*
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 418e0909fdfd7e32f42347dee4644cb6eba73c97..f1bcd4519ae9fdb88fe182cf45270740cacc452f 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.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--------
/**
* \file has_state_blocks.h
*
diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h
index 460835cb9bf03c92cfb73f8516248816212c6571..62833a8b4565bf1d155d950edb121ab86c2b275b 100644
--- a/include/core/state_block/local_parametrization_angle.h
+++ b/include/core/state_block/local_parametrization_angle.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--------
/**
* \file local_parametrization_angle.h
*
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 8494ec3a89215af8287584c8255b241af3486b45..435489cd3feaa83347fe0c20e5ade7d8b1a8d0ea 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.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--------
/*
* \file local_parametrization_base.h
*
diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h
index b8bbb981c6cec93c9e4fbfa29049a12f16d137a9..476aceb79a8ab48b2cb43bbf148b732411a204ba 100644
--- a/include/core/state_block/local_parametrization_homogeneous.h
+++ b/include/core/state_block/local_parametrization_homogeneous.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--------
/*
* \file local_parametrization_homogeneous.h
*
diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h
index c637ab6bb07a23b0e5dd5528d6a1c240327e1473..70f4c3018b15c00fbfef5b95f14ad1a7a2907d1d 100644
--- a/include/core/state_block/local_parametrization_quaternion.h
+++ b/include/core/state_block/local_parametrization_quaternion.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--------
/*
* \file local_parametrization_quaternion.h
*
diff --git a/include/core/state_block/state_angle.h b/include/core/state_block/state_angle.h
index 1a5fda068c7befa1c563bcbfe05f938e0d4c576b..3cc093665df69b32ed10d2d49921fdacedea7871 100644
--- a/include/core/state_block/state_angle.h
+++ b/include/core/state_block/state_angle.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--------
/**
* \file state_angle.h
*
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 5406fe0c105c67d87e4793ba86a2e52f81bbfa87..8f4b0468b8d9fdc519e11137a8dc491330ec3ac7 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.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 STATE_BLOCK_H_
#define STATE_BLOCK_H_
diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index d0ddf4e7fc453212f345640f746cca212170f3b7..e3569b5b4ed30159e0593de789b3b7a6c3644387 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.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--------
/*
* state_composite.h
*
@@ -99,7 +120,7 @@ class MatrixComposite : public std::unordered_map < char, std::unordered_map < c
const std::list<int>& _row_sizes,
const StateStructure& _col_structure,
const std::list<int>& _col_sizes);
- MatrixComposite (const MatrixComposite& m);
+
/**
* \brief Construct from Eigen::VectorXd and structure
*
@@ -358,11 +379,6 @@ inline StateBlockPtr wolf::StateBlockComposite::emplace(const char &_sb_type,
return sb;
}
-inline MatrixComposite::MatrixComposite (const MatrixComposite& m)
- : unordered_map<char, unordered_map<char, MatrixXd> >(m), size_rows_(m.size_rows_), size_cols_(m.size_cols_)
-{
-}
-
}
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index ccc45424e0304ab4705e6eab9625a5d5b67ab290..6ee8a85c28725805ff5a28fb2149950f7cb8e132 100644
--- a/include/core/state_block/state_homogeneous_3d.h
+++ b/include/core/state_block/state_homogeneous_3d.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--------
/*
* \file state_homogeneous_3d.h
*
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index d52b18c83165ac2cfe98bcebd944db4c612a4887..4200f81f7e3faac814a45bea8995a13436b6b7c3 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.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--------
/*
* \file StateQuaternion.h
*
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 431524af94e218b17f2ce223ca47483c0d3cd058..46cce552ccbfe28ec30981b905b191ec2b9456cc 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.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 TRAJECTORY_BASE_H_
#define TRAJECTORY_BASE_H_
diff --git a/include/core/tree_manager/factory_tree_manager.h b/include/core/tree_manager/factory_tree_manager.h
index c9e91935fe52e08b2ea1bf0c264b5e67ca5ef4e1..ae478208823b947161ad687dc8cb32fb6a7cafa3 100644
--- a/include/core/tree_manager/factory_tree_manager.h
+++ b/include/core/tree_manager/factory_tree_manager.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 FACTORY_TREE_MANAGER_H_
#define FACTORY_TREE_MANAGER_H_
diff --git a/include/core/tree_manager/tree_manager_base.h b/include/core/tree_manager/tree_manager_base.h
index 9fd06f2f0d78e82ee1eda5800c10707b4c84b759..8aa2ac43d0268e5531977c7e9afb220fa0c37c74 100644
--- a/include/core/tree_manager/tree_manager_base.h
+++ b/include/core/tree_manager/tree_manager_base.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 INCLUDE_TREE_MANAGER_BASE_H_
#define INCLUDE_TREE_MANAGER_BASE_H_
@@ -69,7 +90,7 @@ class TreeManagerBase : public NodeBase
params_(_params)
{}
- ~TreeManagerBase() override{}
+ virtual ~TreeManagerBase() {}
virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
diff --git a/include/core/tree_manager/tree_manager_sliding_window.h b/include/core/tree_manager/tree_manager_sliding_window.h
index 16e58428dc7fdce89b919e22109a6595dce5afd3..46eedc0f211ce66a6aded99b9e2385c549a9d0fd 100644
--- a/include/core/tree_manager/tree_manager_sliding_window.h
+++ b/include/core/tree_manager/tree_manager_sliding_window.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 INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
diff --git a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
index f0e20f4b5eec08e6f322424f7353ca124b448e90..6df971b5b1a7ab48b0792e8f3bd5329450d703c9 100644
--- a/include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
+++ b/include/core/tree_manager/tree_manager_sliding_window_dual_rate.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 INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
diff --git a/include/core/utils/check_log.h b/include/core/utils/check_log.h
index 035bd0aa0e2e9fa4989c95509800d1fee517bd46..947a53175be061589f460577666d765fdfc7a0e4 100644
--- a/include/core/utils/check_log.h
+++ b/include/core/utils/check_log.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 CHECK_LOG_H
#define CHECK_LOG_H
#include <iostream>
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 7a266d50b2e26eb387ff03ac9ba1f94ab1c39437..1c53906c6c624ef4e88ac95785fa1871b9b5c122 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.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 CONVERTER_H
#define CONVERTER_H
diff --git a/include/core/utils/converter_utils.h b/include/core/utils/converter_utils.h
index 1765c3f9091edebb4db92e66790d333e0116548a..36d67b012337334b2ab26d42fb87cc134fb70b3a 100644
--- a/include/core/utils/converter_utils.h
+++ b/include/core/utils/converter_utils.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 CONVERTER_UTILS_H
#define CONVERTER_UTILS_H
@@ -43,4 +64,4 @@ namespace utils{
std::string splitMapStringRepresentation(std::string str_map);
std::vector<std::string> parseList(std::string val);
}
-#endif
\ No newline at end of file
+#endif
diff --git a/include/core/utils/eigen_assert.h b/include/core/utils/eigen_assert.h
index 7873b792cf89c4af8e015776798e381f0315776f..fcd44023d307b8fd02403d7be2c2df752b5772b6 100644
--- a/include/core/utils/eigen_assert.h
+++ b/include/core/utils/eigen_assert.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 _WOLF_EIGEN_ASSERT_H_
#define _WOLF_EIGEN_ASSERT_H_
diff --git a/include/core/utils/eigen_predicates.h b/include/core/utils/eigen_predicates.h
deleted file mode 100644
index 1f319f3f81df939415a688fd6e7206918f5d1825..0000000000000000000000000000000000000000
--- a/include/core/utils/eigen_predicates.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/**
- * \file eigen_predicates.h
- * \brief Some utils for comparing Eigen types
- * \author Jeremie Deray
- * Created on: 24/10/2017
- */
-
-#ifndef _WOLF_EIGEN_PREDICATES_H_
-#define _WOLF_EIGEN_PREDICATES_H_
-
-#include "core/math/rotations.h"
-
-namespace wolf {
-
-/// @brief check that each Matrix/Vector elem is approx equal to value +- precision
-///
-/// @note we overload this rather than using Eigen::Matrix::isConstant() since it is
-/// defined as :
-/// abs(x - y) <= (min)(abs(x), abs(y)) * prec
-/// which does not play nice with y = 0
-auto is_constant = [](const Eigen::MatrixXd& lhs, const double val, const double precision)
- {
- for (Eigen::MatrixXd::Index j = 0; j < lhs.cols(); ++j)
- for (Eigen::MatrixXd::Index i = 0; i < lhs.rows(); ++i)
- if (std::abs(lhs.coeff(i, j) - val) > precision)
- return false;
- return true;
- };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_zero = [](const Eigen::MatrixXd& lhs, const double precision)
- { return is_constant(lhs, 0, precision); };
-
-/// @brief check that each element of the Matrix/Vector difference
-/// is approx 0 +- precision
-auto pred_diff_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return pred_zero(lhs - rhs, precision); };
-
-/// @brief check that the Matrix/Vector difference norm is approx 0 +- precision
-auto pred_diff_norm_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return std::abs((lhs - rhs).norm()) <= std::abs(precision); };
-
-/// @brief check that the lhs Matrix/Vector is elem-wise approx the rhs +-precision
-auto pred_is_approx = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
- { return lhs.isApprox(rhs, precision); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to rhs
-/// is <= precision
-auto pred_quat_is_approx = [](const Eigen::Quaterniond& lhs, const Eigen::Quaterniond& rhs, const double precision)
- { return pred_zero( log_q(rhs * lhs.inverse()), precision ); };
-
-/// @brief check that angle θ of rotation required to get from lhs quaternion to identity
-/// is <= precision
-auto pred_quat_identity = [](const Eigen::Quaterniond& lhs, const double precision)
- { return pred_quat_is_approx(lhs, Eigen::Quaterniond::Identity(), precision); };
-
-/// @brief check that rotation angle to get from lhs angle to rhs is <= precision
-auto pred_angle_is_approx = [](const double lhs, const double rhs, const double precision)
- { return std::abs(pi2pi(lhs - rhs)) <= std::abs(precision); };
-
-/// @brief check that rotation angle to get from lhs angle to 0 is <= precision
-auto pred_angle_zero = [](const double lhs, const double precision)
- { return pred_angle_is_approx(lhs, 0, precision); };
-
-/// @brief check that the lhs pose is approx rhs +- precision
-///
-/// @note
-/// Comparison is :
-/// d = inv(lhs) * rhs
-/// d.translation().elem_wise() ~ 0 (+- precision)
-///
-/// if pose 3d :
-/// d.rotation_as_quaternion() ~ quaternion.getIdentity (+- precision)
-///
-/// else if pose 2d:
-/// d.rotation_angle() ~ 0 (+- precision)
-///
-/// else throw std::runtime
-///
-/// @see pred_zero for translation comparison
-/// @see pred_quat_identity for 3d rotation comparison
-/// @see pred_angle_zero for 2d rotation comparison
-//auto pred_pose_is_approx = [](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs, const double precision)
-// {
-// const Eigen::MatrixXd d = lhs.inverse() * rhs;
-// const bool tok = pred_zero(d.rightCols(1), precision);
-
-// const bool qok = (lhs.rows() == 3)?
-// pred_quat_identity(Eigen::Quaterniond(d.block(0,0,3,1)),
-// precision)
-// : (lhs.rows() == 2)? pred_angle_zero(getYaw(d), precision)
-// : throw std::runtime_error("Canno't compare pose in Dim > 3 !");
-
-// return tok && qok;
-// };
-
-} // namespace wolf
-
-#endif /* _WOLF_EIGEN_PREDICATES_H_ */
diff --git a/include/core/utils/graph_search.h b/include/core/utils/graph_search.h
index cf14ff844d981922848645774799f2b18655e06f..549c1f1dec8f5b45579904ac5ec1f0adebbca593 100644
--- a/include/core/utils/graph_search.h
+++ b/include/core/utils/graph_search.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 GRAPH_SEARCH_H
#define GRAPH_SEARCH_H
diff --git a/include/core/utils/loader.h b/include/core/utils/loader.h
index 7da10d043ebaf5f64f28520d11fe2b114a3c1284..13fe987d736a47d50a52de2d86f519c3cea1a63b 100644
--- a/include/core/utils/loader.h
+++ b/include/core/utils/loader.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 LOADER_H
#define LOADER_H
@@ -16,7 +37,7 @@ class LoaderRaw: public Loader{
void* resource_;
public:
LoaderRaw(std::string _file);
- ~LoaderRaw();
+ virtual ~LoaderRaw();
void load() override;
void close() override;
};
diff --git a/include/core/utils/logging.h b/include/core/utils/logging.h
index b59a96c31cba4ca7efb245a733402cf05c14c44c..a0e1556898fd453f5543ba90ec3645ccf282525c 100644
--- a/include/core/utils/logging.h
+++ b/include/core/utils/logging.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--------
/**
* \file logging.h
*
@@ -201,16 +222,6 @@ inline void Logger::set_pattern(const std::string& p)
using LoggerPtr = std::unique_ptr<Logger>;
-/// dummy namespace to avoid colision with c++14
-/// @todo use std version once we move to cxx14
-namespace not_std {
-template <typename T, typename... Args>
-std::unique_ptr<T> make_unique(Args&&... args)
-{
- return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
-}
-} /* namespace not_std */
-
class LoggerManager
{
public:
@@ -244,7 +255,7 @@ protected:
/// @note would be easier with cpp17 map.try_emplace...
const bool created = existsImpl(name) ?
false :
- logger_map_.emplace(name, not_std::make_unique<Logger>(name)).second;
+ logger_map_.emplace(name, std::make_unique<Logger>(name)).second;
return created;
}
diff --git a/include/core/utils/make_unique.h b/include/core/utils/make_unique.h
deleted file mode 100644
index f336eb4f7ff52eae0e7014505180bb060f48ee70..0000000000000000000000000000000000000000
--- a/include/core/utils/make_unique.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/**
- * \file make_unique.h
- *
- * Created on: Oct 11, 2017
- * \author: Jeremie Deray
- */
-
-#ifndef _WOLF_MAKE_UNIQUE_H_
-#define _WOLF_MAKE_UNIQUE_H_
-
-#include <memory>
-
-namespace wolf {
-
-/// @note this appears only in cpp14
-template <typename T, typename... Args>
-std::unique_ptr<T> make_unique(Args&&... args)
-{
- return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
-}
-
-}
-
-#endif /* _WOLF_MAKE_UNIQUE_H_ */
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
index e8b473ca26703f17ea932cca4d1e913717108026..e5283ccd5b5f669e000fda1904245c99e58955ee 100644
--- a/include/core/utils/params_server.h
+++ b/include/core/utils/params_server.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 PARAMS_SERVER_H
#define PARAMS_SERVER_H
diff --git a/include/core/utils/singleton.h b/include/core/utils/singleton.h
index 3595ea584249a0261e6cd2f65b3c017846d0a2e3..edbaa1b5424280f921595859ec9199e4e2a2f863 100644
--- a/include/core/utils/singleton.h
+++ b/include/core/utils/singleton.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--------
/**
* \file singleton.h
*
diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index 31564f92f125ec81cbc2273e56dc032d057b6250..b89b9102ab64073b729744ec5d6c7d4d8098b143 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.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--------
/**
* \file utils_gtest.h
* \brief Some utils for gtest
diff --git a/include/core/yaml/parser_yaml.h b/include/core/yaml/parser_yaml.h
index 74473f695b2c7078aea77111cefe34ec6977efd2..d5f94896161992d3bb42dc729fa5c45165bd88da 100644
--- a/include/core/yaml/parser_yaml.h
+++ b/include/core/yaml/parser_yaml.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 PARSER_YAML_H
#define PARSER_YAML_H
diff --git a/include/core/yaml/yaml_conversion.h b/include/core/yaml/yaml_conversion.h
index 0542cba4d870978b913f36c02f429491ee61d389..76991419a0ec370174055479cbb9dfdb910e0356 100644
--- a/include/core/yaml/yaml_conversion.h
+++ b/include/core/yaml/yaml_conversion.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--------
/**
* \file yaml_conversion.h
*
diff --git a/prova.txt b/prova.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 546fafeec33538915a23ed5e96a98400cebd58c4..8e91f7e75caedf348de594a725f0d0ee2c707d3d 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.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/capture/capture_base.h"
#include "core/sensor/sensor_base.h"
@@ -189,34 +210,41 @@ void CaptureBase::move(FrameBasePtr _frm_ptr)
assert((this->getFrame() == nullptr || not this->getFrame()->getProblem()) && "Forbidden: trying to move a capture already linked to a KF!");
// Unlink
- if (this->getFrame())
- {
- // unlink from previous non-key frame
- this->getFrame()->removeCapture(shared_from_this());
- this->setFrame(nullptr);
- }
+ unlink();
// link
link(_frm_ptr);
}
-
void CaptureBase::link(FrameBasePtr _frm_ptr)
{
assert(!is_removing_ && "linking a removed capture");
assert(this->getFrame() == nullptr && "linking a capture already linked");
+ WOLF_WARN_COND(_frm_ptr == nullptr, "Linking Capture ", id(), " to a nullptr");
+
if(_frm_ptr)
{
-
_frm_ptr->addCapture(shared_from_this());
this->setFrame(_frm_ptr);
this->setProblem(_frm_ptr->getProblem());
}
- else
- {
- WOLF_WARN("Linking Capture ", id(), " to a nullptr");
- }
+}
+
+void CaptureBase::unlink()
+{
+ WOLF_WARN_COND(this->getFrame() == nullptr, "Unlinking a not linked Capture ", id(), ". Nothing to do, skipping...");
+
+ if (not this->getFrame())
+ return;
+
+ for (auto ftr : getFeatureList())
+ assert(ftr->getFactorList().empty() && " unlinking a capture with factors!");
+ assert(getConstrainedByList().empty() && " unlinking a capture constrained by factors!");
+
+ // unlink from frame
+ this->getFrame()->removeCapture(shared_from_this());
+ this->setFrame(nullptr);
}
void CaptureBase::setProblem(ProblemPtr _problem)
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index 7a09336470bfefeaf304cfbb817a8793c4c9a82d..f4f15934597e6354a026e9cae7b92176d9ac74a2 100644
--- a/src/capture/capture_diff_drive.cpp
+++ b/src/capture/capture_diff_drive.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/capture/capture_diff_drive.h"
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index b87e4fb10c37d79ede036e72983faea5d7d12b7c..3f925fd62a6acb93d7bcff2b3a45fbb28edc1bd7 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.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--------
/**
* \file capture_motion.cpp
*
@@ -105,7 +126,10 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
if (getStateBlockMap().size() > 0)
printState(_metric, _state_blocks, _stream, _tabs);
- _stream << _tabs << " " << "buffer size : " << getBuffer().size() << std::endl;
+ _stream << _tabs << " " << "buffer size : " << getBuffer().size();
+ if (getBuffer().size() > 0) _stream << " ; nbr of data samples : " << getBuffer().size() - 1;
+ _stream << std::endl;
+
if ( _metric && ! getBuffer().empty())
{
_stream << _tabs << " " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
diff --git a/src/capture/capture_odom_2d.cpp b/src/capture/capture_odom_2d.cpp
index 4755b6fe067fcbcd81c3ae66ecb4be6b562078e7..c7f394fdcbe40982e3ea1c077bbccad766149d69 100644
--- a/src/capture/capture_odom_2d.cpp
+++ b/src/capture/capture_odom_2d.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--------
/*
* capture_odom_2d.cpp
*
diff --git a/src/capture/capture_odom_3d.cpp b/src/capture/capture_odom_3d.cpp
index a692011ffc2b61c0d46e9f024d39b354106feeb7..ba1ab7408c73928d4c7f94f5d12da78fb2bb731d 100644
--- a/src/capture/capture_odom_3d.cpp
+++ b/src/capture/capture_odom_3d.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--------
/*
* capture_odom_3d.cpp
*
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 190fa7090c698b4a613d0ebba2594e391043f925..113ac5710753ffd6c81de747977b914046fefc5c 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.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/capture/capture_pose.h"
namespace wolf{
diff --git a/src/capture/capture_void.cpp b/src/capture/capture_void.cpp
index dcce03c9dc66e1dc38c62fa87dcdfdd4a25b528e..304e5627343db4661acfa46d5d9c8dd658647a9b 100644
--- a/src/capture/capture_void.cpp
+++ b/src/capture/capture_void.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/capture/capture_void.h"
namespace wolf {
diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp
index 1cb1425c08c761a56ad615a63f9e26786c37600a..c6623d4f5ce0c735b97b9e77cb902c5c0452fd3e 100644
--- a/src/ceres_wrapper/local_parametrization_wrapper.cpp
+++ b/src/ceres_wrapper/local_parametrization_wrapper.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/ceres_wrapper/local_parametrization_wrapper.h"
namespace wolf {
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 0d7746f929f427d3e2bc9f6bcce53e4147b7924d..297b041befe463e33fd1dcc1869a74174f4d0257 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.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--------
/*
* qr_manager.cpp
*
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 5914b40a8bc67e44bbc392b146766331e344ced7..ab8753351aee84c5b4f290af171b370663060499 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.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/ceres_wrapper/solver_ceres.h"
#include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
#include "core/ceres_wrapper/cost_function_wrapper.h"
@@ -6,7 +27,6 @@
#include "core/trajectory/trajectory_base.h"
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
-#include "core/utils/make_unique.h"
namespace wolf
{
@@ -20,11 +40,16 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
const ParamsCeresPtr& _params)
: SolverManager(_wolf_problem, _params)
, params_ceres_(_params)
+ , n_iter_acc_(0)
+ , n_iter_max_(0)
+ , n_convergence_(0)
+ , n_interrupted_(0)
+ , n_no_convergence_(0)
{
- covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
- ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options);
+ covariance_ = std::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
+ ceres_problem_ = std::make_unique<ceres::Problem>(params_ceres_->problem_options);
- if (params_ceres_->update_immediately)
+ if (params_ceres_->interrupt_on_problem_change)
getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_,
params_ceres_->min_num_iterations,
params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
@@ -53,24 +78,6 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
// run Ceres Solver
ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
- /*/ if termination by user (Iteration callback, update and solve again)
- // solve until max iterations reached
- if (params_ceres_->update_immediately)
- {
- auto max_num_iterations = getSolverOptions().max_num_iterations;
- while (summary_.termination_type == ceres::USER_SUCCESS)
- {
- // decrease wasted iterations
- getSolverOptions().max_num_iterations -= summary_.iterations.size();
- if (getSolverOptions().max_num_iterations <= 0)
- break;
-
- // update and solve again
- update();
- ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
- }
- getSolverOptions().max_num_iterations = max_num_iterations;
- }*/
std::string report;
//return report
@@ -79,6 +86,19 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
else if (report_level == ReportVerbosity::FULL)
report = summary_.FullReport();
+ // PROFILING
+ // iterations (profiling)
+ n_iter_acc_ += iterations();
+ n_iter_max_ = std::max(n_iter_max_, iterations());
+
+ // convergence (profiling)
+ if (hasConverged())
+ n_convergence_++;
+ else if (wasStopped())
+ n_interrupted_++;
+ else
+ n_no_convergence_++;
+
return report;
}
@@ -161,17 +181,6 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
break;
}
}
- // // loop all marginals (PO marginals)
- // for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
- // {
- // state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i]);
- // state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
- // state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
- //
- // double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i]->get());
- // double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i+1]->get());
- // double_pairs.emplace_back(all_state_blocks[2*i+1]->get(),all_state_blocks[2*i+1]->get());
- // }
break;
}
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
@@ -597,9 +606,16 @@ bool SolverCeres::hasConverged()
return summary_.termination_type == ceres::CONVERGENCE;
}
-SizeStd SolverCeres::iterations()
+bool SolverCeres::wasStopped()
{
- return summary_.iterations.size();
+ return summary_.termination_type == ceres::USER_FAILURE or summary_.termination_type == ceres::USER_SUCCESS;
+}
+
+unsigned int SolverCeres::iterations()
+{
+ if (summary_.num_successful_steps + summary_.num_unsuccessful_steps < 1)
+ return 0;
+ return summary_.num_successful_steps + summary_.num_unsuccessful_steps;
}
double SolverCeres::initialCost()
@@ -796,6 +812,20 @@ bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
== state_blocks_local_param_.at(st).get();
}
+void SolverCeres::printProfilingDerived(std::ostream& _stream) const
+{
+ _stream << "Iterations:"
+ << "\n\tUser-defined limit: " << params_ceres_->solver_options.max_num_iterations
+ << "\n\tAverage iterations: " << n_iter_acc_ / n_solve_
+ << "\n\tMax. iterations: " << n_iter_max_
+ << "\nTermination:"
+ << "\n\tConvergence: " << 1e2 * n_convergence_ / n_solve_ << " %"
+ << "\n\tInterrupted by problem change: " << 1e2 * n_interrupted_ / n_solve_ << " %"
+ << "\n\tMax. iterations reached: " << 1e2 * n_no_convergence_ / n_solve_ << " %"
+ << std::endl;
+}
+
+
} // namespace wolf
#include "core/solver/factory_solver.h"
namespace wolf {
diff --git a/src/common/node_base.cpp b/src/common/node_base.cpp
index 3db2a0798a593d555e96d0d5dbd45263fad28642..af4db4552d268b503a05266f4b0f97ca9bb7c28a 100644
--- a/src/common/node_base.cpp
+++ b/src/common/node_base.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/common/node_base.h"
namespace wolf {
diff --git a/src/common/time_stamp.cpp b/src/common/time_stamp.cpp
index 0421efa9b06aaf3f2f75796910485aa687217bba..5ea454b0ad715c2aff3cedc8711539586e9c155d 100644
--- a/src/common/time_stamp.cpp
+++ b/src/common/time_stamp.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/common/time_stamp.h"
diff --git a/src/factor/factor_analytic.cpp b/src/factor/factor_analytic.cpp
index f3c300baa90528cfcca7fc604c77facf214d7a3e..bf7fb3d0256dde347713302ab1daee5353655bbc 100644
--- a/src/factor/factor_analytic.cpp
+++ b/src/factor/factor_analytic.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/factor/factor_analytic.h"
#include "core/state_block/state_block.h"
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index df80bf00bc0f38d08d2b9c6806a7d0ef9a01a8c7..46151eba686f2fa4f12a1c734565575da362eeec 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.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/factor/factor_base.h"
#include "core/frame/frame_base.h"
#include "core/landmark/landmark_base.h"
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index c2af9127c6a2e028d98f4f5413490117bfbd7825..11db6b079bd8371a48a660547997f7c68d3198bf 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.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/feature/feature_base.h"
#include "core/factor/factor_base.h"
#include "core/capture/capture_base.h"
diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp
index 9ba01d1f11c90e54ae118466b1cc2bf745d132ed..62dcff2cc20aa81cf14a19535d8e804f62564984 100644
--- a/src/feature/feature_diff_drive.cpp
+++ b/src/feature/feature_diff_drive.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/feature/feature_diff_drive.h"
namespace wolf
diff --git a/src/feature/feature_motion.cpp b/src/feature/feature_motion.cpp
index d0b988b79efd5b8e376c6c244a7154933479538f..6d0260b07b65294f033e28c6fc41a1541eff9aa7 100644
--- a/src/feature/feature_motion.cpp
+++ b/src/feature/feature_motion.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--------
/*
* feature_motion.cpp
*
diff --git a/src/feature/feature_odom_2d.cpp b/src/feature/feature_odom_2d.cpp
index a4d2571b91da0ce812b125f7a7d603c3edd64f97..c0d2083caaf7bbce409647b55298e72699defdfc 100644
--- a/src/feature/feature_odom_2d.cpp
+++ b/src/feature/feature_odom_2d.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/feature/feature_odom_2d.h"
namespace wolf {
diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp
index cb5ea4c105b0cf99d0f5322544983cd29345f01b..bcf4e2b487e77c30a51f987e4db87a8e2304bf00 100644
--- a/src/feature/feature_pose.cpp
+++ b/src/feature/feature_pose.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/feature/feature_pose.h"
namespace wolf {
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 71e344552517ef659bb3ae60f66abbb95c25056f..951d544ea9afb33dbc412128f8cda51ac7d1cdb9 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.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/frame/frame_base.h"
#include "core/factor/factor_base.h"
diff --git a/src/hardware/hardware_base.cpp b/src/hardware/hardware_base.cpp
index e6e5a15c3c216d2abfa9242a48bb97081e22d016..9db6b0c373e8f49838ea0d354a0694bffba0da04 100644
--- a/src/hardware/hardware_base.cpp
+++ b/src/hardware/hardware_base.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/hardware/hardware_base.h"
#include "core/sensor/sensor_base.h"
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 11e26334cfedde1e6ffcae6f4588a57651c51eaa..09d317544ff4c0ae6ce7b6a24e2d3b2acf2cfe93 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.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/landmark/landmark_base.h"
#include "core/factor/factor_base.h"
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index e263f9ad02d4f66417cbddf31ce8da280a1d3ec4..8194c87d95dc05bb20b5f736afdb704a11a75560 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -1,3 +1,25 @@
+//--------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--------
+
// wolf
#include "core/map/map_base.h"
#include "core/landmark/landmark_base.h"
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index daf1b49909392719da9188aa74746f4b1da46ab4..05e45510ac36c608d3e1378befe3c4179951551b 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.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--------
// wolf includes
#include "core/problem/problem.h"
#include "core/hardware/hardware_base.h"
@@ -45,8 +66,8 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim, MapBasePtr
tree_manager_(nullptr),
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>()),
- map_ptr_(_map),
- processor_is_motion_map_(),
+ map_ptr_(std::make_shared<MapBase>()),
+ motion_provider_map_(),
frame_structure_(_frame_structure),
prior_options_(std::make_shared<PriorOptions>())
{
@@ -94,35 +115,37 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
#else
std::string lib_extension = ".so";
#endif
+
// Problem structure and dimension
std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
- int dim = _server.getParam<int> ("problem/dimension");
- auto problem = Problem::create(frame_structure, dim, nullptr);
+ int dim = _server.getParam<int> ("problem/dimension");
+ auto problem = Problem::create(frame_structure, dim);
- // cout << "PRINTING SERVER MAP" << endl;
- // _server.print();
- // cout << "-----------------------------------" << endl;
WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
// Load plugins
- auto loaders = std::vector<Loader *>();
+ auto loaders = std::vector<std::shared_ptr<Loader>>();
std::string plugins_path;
try{
plugins_path = _server.getParam<std::string>("plugins_path");
}
catch(MissingValueException& e){
WOLF_WARN(e.what());
- WOLF_WARN("Setting '/usr/local/lib/iri-algorithms/' as plugins path...");
- plugins_path="/usr/local/lib/iri-algorithms/";
+ WOLF_WARN("Setting '/usr/local/lib/' as plugins path...");
+ plugins_path="/usr/local/lib/";
}
- for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins")) {
+ for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins"))
+ {
if (plugin_name == "core" or plugin_name == "wolf" or plugin_name == "") continue; // ignore plugin "core"
+
std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
- WOLF_TRACE("Loading plugin " + plugin);
- auto l = new LoaderRaw(plugin);
+ WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin);
+ auto l = std::make_shared<LoaderRaw>(plugin);
l->load();
loaders.push_back(l);
}
+
+ // load Packages for subscribers and publishers
std::string packages_path;
try {
packages_path = _server.getParam<std::string>("packages_path");
@@ -130,20 +153,22 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
WOLF_WARN(e.what());
WOLF_WARN("Support for subscribers disabled...");
}
- for (auto it : _server.getParam<std::vector<std::string>>("packages_subscriber")) {
- std::string subscriber = packages_path + "/libsubscriber_" + it + lib_extension;
- WOLF_TRACE("Loading subscriber " + subscriber);
- auto l = new LoaderRaw(subscriber);
+ for (auto subscriber_name : _server.getParam<std::vector<std::string>>("packages_subscriber")) {
+ std::string subscriber = packages_path + "/libsubscriber_" + subscriber_name + lib_extension;
+ WOLF_TRACE("Loading subscriber " + subscriber_name + " via " + subscriber);
+ auto l = std::make_shared<LoaderRaw>(subscriber);
l->load();
loaders.push_back(l);
}
- for (auto it : _server.getParam<std::vector<std::string>>("packages_publisher")) {
- std::string subscriber = packages_path + "/libpublisher_" + it + lib_extension;
- WOLF_TRACE("Loading publisher " + subscriber);
- auto l = new LoaderRaw(subscriber);
+ for (auto publisher_name : _server.getParam<std::vector<std::string>>("packages_publisher")) {
+ std::string publisher = packages_path + "/libpublisher_" + publisher_name + lib_extension;
+ WOLF_TRACE("Loading publisher " + publisher_name + " via " + publisher);
+ auto l = std::make_shared<LoaderRaw>(publisher);
l->load();
loaders.push_back(l);
}
+
+ // load raw libs
std::vector<std::string> raw_libs;
try {
raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
@@ -153,22 +178,25 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
}
for (auto lib : raw_libs) {
WOLF_TRACE("Loading raw lib " + lib);
- auto l = new LoaderRaw(lib);
+ auto l = std::make_shared<LoaderRaw>(lib);
l->load();
loaders.push_back(l);
}
// Install sensors and processors
- auto sensorMap = std::map<std::string, SensorBasePtr>();
- auto procesorMap = std::map<std::string, ProcessorBasePtr>();
- auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
- for(auto sen : sensors){
- sensorMap.insert(std::pair<std::string, SensorBasePtr>(sen["name"], problem->installSensor(sen["type"], sen["name"], _server)));
- }
- auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
- for(auto prc : processors){
- procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc["name"], problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server)));
- }
+ auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
+ for(auto sen : sensors)
+ problem->installSensor(sen["type"],
+ sen["name"],
+ _server);
+
+ auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
+ for(auto prc : processors)
+ problem->installProcessor(prc["type"],
+ prc["name"],
+ prc["sensor_name"],
+ _server);
+
// Map
std::string map_type = _server.getParam<std::string>("map/type");
@@ -178,7 +206,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
std::string plugin = plugins_path + "libwolf" + map_plugin + lib_extension;
WOLF_TRACE("Loading plugin " + plugin);
- auto l = new LoaderRaw(plugin);
+ auto l = std::make_shared<LoaderRaw>(plugin);
l->load();
loaders.push_back(l);
}
@@ -196,14 +224,14 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
std::string plugin = plugins_path + "libwolf" + tm_plugin + lib_extension;
WOLF_TRACE("Loading plugin " + plugin);
- auto l = new LoaderRaw(plugin);
+ auto l = std::make_shared<LoaderRaw>(plugin);
l->load();
loaders.push_back(l);
}
problem->setTreeManager(AutoConfFactoryTreeManager::create(tree_manager_type, "tree manager", _server));
}
- // Prior
+ // Set problem prior -- first keyframe
std::string prior_mode = _server.getParam<std::string>("problem/prior/mode");
assert((prior_mode == "nothing" || prior_mode == "initial_guess" || prior_mode == "fix" || prior_mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
WOLF_TRACE("Prior mode: ", prior_mode);
@@ -214,15 +242,12 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
else if (prior_mode == "factor")
{
problem->setPriorOptions(prior_mode,
- _server.getParam<double>("problem/prior/time_tolerance"),
_server.getParam<VectorComposite>("problem/prior/state"),
_server.getParam<VectorComposite>("problem/prior/sigma"));
}
else
{
- WOLF_TRACE("Prior mode: ", prior_mode);
problem->setPriorOptions(prior_mode,
- _server.getParam<double>("problem/prior/time_tolerance"),
_server.getParam<VectorComposite>("problem/prior/state"));
}
@@ -247,8 +272,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
-
- const Eigen::VectorXd& _extrinsics, //
+ const Eigen::VectorXd& _extrinsics, //
const std::string& _intrinsics_filename)
{
@@ -447,7 +471,7 @@ TimeStamp Problem::getTimeStamp ( ) const
{
TimeStamp ts = TimeStamp::Invalid();
- for (const auto& prc_pair : processor_is_motion_map_)
+ for (const auto& prc_pair : motion_provider_map_)
if (prc_pair.second->getTimeStamp().ok())
if ( (not ts.ok() ) or prc_pair.second->getTimeStamp() > ts)
ts = prc_pair.second->getTimeStamp();
@@ -474,8 +498,8 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
VectorComposite state;
- // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state
- for (const auto& prc_pair : processor_is_motion_map_)
+ // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
+ for (const auto& prc_pair : motion_provider_map_)
{
const auto& prc_state = prc_pair.second->getState(structure);
@@ -483,7 +507,15 @@ VectorComposite Problem::getState(const StateStructure& _structure) const
for (const auto& pair_key_vec : prc_state)
{
if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
- state.insert(pair_key_vec);
+ {
+ state.insert(pair_key_vec);
+ }
+ }
+
+ //If all keys are filled return
+ if (state.size() == structure.size())
+ {
+ return state;
}
}
@@ -522,8 +554,8 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
VectorComposite state;
- // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state
- for (const auto& prc_pair : processor_is_motion_map_)
+ // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
+ for (const auto& prc_pair : motion_provider_map_)
{
const auto& prc_state = prc_pair.second->getState(_ts, structure);
@@ -533,6 +565,12 @@ VectorComposite Problem::getState (const TimeStamp& _ts, const StateStructure& _
if (state.count(pair_key_vec.first) == 0) // Only write once. This gives priority to processors with more priority
state.insert(pair_key_vec);
}
+
+ //If all keys are filled return
+ if (state.size() == structure.size())
+ {
+ return state;
+ }
}
// check for empty blocks and fill them with the closest KF to ts, with the prior, or with zeros in the worst case
@@ -570,8 +608,8 @@ VectorComposite Problem::getOdometry(const StateStructure& _structure) const
VectorComposite odom_state;
- // compose the states of all IsMotion processors (ordered by user-defined priority) into one only state
- for (const auto& prc_pair : processor_is_motion_map_)
+ // compose the states of all MotionProvider processors (ordered by user-defined priority) into one only state
+ for (const auto& prc_pair : motion_provider_map_)
{
const auto& prc_state = prc_pair.second->getOdometry();
@@ -633,39 +671,39 @@ void Problem::setTreeManager(TreeManagerBasePtr _gm)
}
-void Problem::addProcessorIsMotion(IsMotionPtr _is_motion_ptr)
+void Problem::addMotionProvider(MotionProviderPtr _motion_provider_ptr)
{
// Check if is state getter
- if (not _is_motion_ptr->isStateGetter())
+ if (not _motion_provider_ptr->isStateGetter())
{
- WOLF_WARN("Problem::addProcessorIsMotion: adding a IsMotion processor with state_getter=false. Not adding this processor");
+ WOLF_WARN("Problem::addMotionProvider: adding a MotionProvider processor with state_getter=false. Not adding this processor");
return;
}
// check duplicated priority
- while (processor_is_motion_map_.count(_is_motion_ptr->getStatePriority()) == 1)
+ while (motion_provider_map_.count(_motion_provider_ptr->getStatePriority()) == 1)
{
- WOLF_ERROR("Problem::addProcessorIsMotion: adding a IsMotion processor with priority = ",
- _is_motion_ptr->getStatePriority(),
+ WOLF_ERROR("Problem::addMotionProvider: adding a MotionProvider processor with priority = ",
+ _motion_provider_ptr->getStatePriority(),
" which is already taken. Trying to add it with priority = ",
- _is_motion_ptr->getStatePriority()+1);
- _is_motion_ptr->setStatePriority(_is_motion_ptr->getStatePriority()+1);
+ _motion_provider_ptr->getStatePriority()+1);
+ _motion_provider_ptr->setStatePriority(_motion_provider_ptr->getStatePriority()+1);
}
// add to map ordered by priority
- processor_is_motion_map_.emplace(_is_motion_ptr->getStatePriority(), _is_motion_ptr);
- appendToStructure(_is_motion_ptr->getStateStructure());
+ motion_provider_map_.emplace(_motion_provider_ptr->getStatePriority(), _motion_provider_ptr);
+ appendToStructure(_motion_provider_ptr->getStateStructure());
}
-void Problem::removeProcessorIsMotion(IsMotionPtr proc)
+void Problem::removeMotionProvider(MotionProviderPtr proc)
{
- WOLF_WARN_COND(processor_is_motion_map_.count(proc->getStatePriority()) == 0, "Problem::clearProcessorIsMotion: missing processor");
+ WOLF_WARN_COND(motion_provider_map_.count(proc->getStatePriority()) == 0, "Problem::clearMotionProvider: missing processor");
- processor_is_motion_map_.erase(proc->getStatePriority());
+ motion_provider_map_.erase(proc->getStatePriority());
// rebuild frame structure with remaining motion processors
frame_structure_.clear();
- for (const auto& pm : processor_is_motion_map_)
+ for (const auto& pm : motion_provider_map_)
appendToStructure(pm.second->getStateStructure());
}
@@ -701,9 +739,9 @@ bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const
return true;
}
-void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
+void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr)
{
- WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
+ WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotionProvider() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
// pause processor profiling
@@ -717,7 +755,7 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
#ifdef PROFILING
auto start = std::chrono::high_resolution_clock::now();
#endif
- processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
+ processor->keyFrameCallback(_keyframe_ptr);
#ifdef PROFILING
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
@@ -971,42 +1009,6 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
}
return success;
-
-
-
-// bool success(true);
-// int i = 0, j = 0;
-//
-// const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
-//
-// // computing size
-// SizeEigen sz = 0;
-// for (const auto& sb : state_bloc_vec)
-// if (sb)
-// sz += sb->getLocalSize();
-//
-// // resizing
-// _covariance = Eigen::MatrixXs(sz, sz);
-//
-// // filling covariance
-//
-// for (const auto& sb_i : state_bloc_vec)
-// {
-// if (sb_i)
-// {
-// j = 0;
-// for (const auto& sb_j : state_bloc_vec)
-// {
-// if (sb_j)
-// {
-// success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
-// j += sb_j->getLocalSize();
-// }
-// }
-// i += sb_i->getLocalSize();
-// }
-// }
-// return success;
}
MapBasePtr Problem::getMap() const
@@ -1042,7 +1044,6 @@ FrameBasePtr Problem::closestFrameToTimeStamp(const TimeStamp& _ts) const
}
void Problem::setPriorOptions(const std::string& _mode,
- const double _time_tolerance ,
const VectorComposite& _state ,
const VectorComposite& _sigma )
{
@@ -1056,13 +1057,10 @@ void Problem::setPriorOptions(const std::string& _mode,
if (prior_options_->mode != "nothing")
{
- assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
assert(_state.includesStructure(frame_structure_) && "any missing key in prior state");
WOLF_TRACE("prior state: ", _state);
- WOLF_TRACE("prior time tolerance: ", _time_tolerance);
prior_options_->state = _state;
- prior_options_->time_tolerance = _time_tolerance;
if (prior_options_->mode == "factor")
{
@@ -1100,7 +1098,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state);
// Update origin for odometry processors
- for (auto proc_pair : processor_is_motion_map_)
+ for (auto proc_pair : motion_provider_map_)
proc_pair.second->setOdometry(prior_options_->state);
if (prior_options_->mode == "fix")
@@ -1149,7 +1147,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");
// notify all processors
- keyFrameCallback(prior_keyframe, nullptr, prior_options_->time_tolerance);
+ keyFrameCallback(prior_keyframe, nullptr);
}
// remove prior options
prior_options_ = nullptr;
@@ -1159,27 +1157,24 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
FrameBasePtr Problem::setPriorFactor(const VectorComposite &_state,
const VectorComposite &_sigma,
- const TimeStamp &_ts,
- const double &_time_tol)
+ const TimeStamp &_ts)
{
- setPriorOptions("factor", _time_tol, _state, _sigma);
+ setPriorOptions("factor", _state, _sigma);
return applyPriorOptions(_ts);
}
FrameBasePtr Problem::setPriorFix(const VectorComposite &_state,
- const TimeStamp &_ts,
- const double &_time_tol)
+ const TimeStamp &_ts)
{
- setPriorOptions("fix", _time_tol, _state);
+ setPriorOptions("fix", _state);
return applyPriorOptions(_ts);
}
FrameBasePtr Problem::setPriorInitialGuess(const VectorComposite &_state,
- const TimeStamp &_ts,
- const double &_time_tol)
+ const TimeStamp &_ts)
{
- setPriorOptions("initial_guess", _time_tol, _state);
+ setPriorOptions("initial_guess", _state);
return applyPriorOptions(_ts);
}
@@ -1197,7 +1192,7 @@ void Problem::print(int _depth, bool _constr_by, bool _metric, bool _state_block
{
_stream << std::endl;
- _stream << "P: wolf tree status ---------------------" << std::endl;
+ _stream << "Wolf tree status ------------------------" << std::endl;
getHardware()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
deleted file mode 100644
index 57333a05529ea41f3866cf7c52fb782c695ae3ca..0000000000000000000000000000000000000000
--- a/src/processor/is_motion.cpp
+++ /dev/null
@@ -1,23 +0,0 @@
-#include "core/processor/is_motion.h"
-#include "core/problem/problem.h"
-
-using namespace wolf;
-
-void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
-{
- setOdometry(_prb_ptr->stateZero(state_structure_));
- if (not isStateGetter())
- {
- WOLF_WARN("IsMotion::addToProblem: IsMotion processor with state_getter=false. Not adding this processor");
- return;
- }
- _prb_ptr->addProcessorIsMotion(_motion_ptr);
-}
-
-void IsMotion::setOdometry(const VectorComposite& _odom)
-{
- assert(_odom.includesStructure(state_structure_) && "IsMotion::setOdometry(): any key missing in _odom.");
-
- for (auto key : state_structure_)
- odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_
-}
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 52383b14cbf0cb7c355f416ef96a20e726bd711b..f61c1b1f9ef7ba4d4f3609fed41fedef53e3cbb8 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.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/processor/motion_buffer.h"
namespace wolf
{
diff --git a/src/processor/motion_provider.cpp b/src/processor/motion_provider.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e2723db61a38f9ee6b98a362eeafac8289875e03
--- /dev/null
+++ b/src/processor/motion_provider.cpp
@@ -0,0 +1,44 @@
+//--------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/processor/motion_provider.h>
+#include "core/problem/problem.h"
+
+using namespace wolf;
+
+void MotionProvider::addToProblem(ProblemPtr _prb_ptr, MotionProviderPtr _motion_ptr)
+{
+ setOdometry(_prb_ptr->stateZero(state_structure_));
+ if (not isStateGetter())
+ {
+ WOLF_WARN("MotionProvider::addToProblem: MotionProvider processor with state_getter=false. Not adding this processor");
+ return;
+ }
+ _prb_ptr->addMotionProvider(_motion_ptr);
+}
+
+void MotionProvider::setOdometry(const VectorComposite& _odom)
+{
+ assert(_odom.includesStructure(state_structure_) && "MotionProvider::setOdometry(): any key missing in _odom.");
+
+ for (auto key : state_structure_)
+ odometry_[key] = _odom.at(key); //overwrite/insert only keys of the state_structure_
+}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 28963ed644aacd240d2d01a0bb23e6280dc6619a..adeac3db58b63afc2907d12892325fcd8feefc8d 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.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/processor/processor_base.h"
#include "core/processor/processor_motion.h"
#include "core/capture/capture_base.h"
@@ -33,31 +54,31 @@ bool ProcessorBase::permittedKeyFrame()
return isVotingActive() && getProblem()->permitKeyFrame(shared_from_this());
}
-void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe_ptr, const double& _time_tol_other)
+void ProcessorBase::keyFrameCallback(FrameBasePtr _keyframe)
{
- assert(_keyframe_ptr != nullptr && "keyFrameCallback with a nullptr frame");
- WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": KF", _keyframe_ptr->id(), " callback received with ts = ", _keyframe_ptr->getTimeStamp());
+ assert(_keyframe != nullptr && "keyFrameCallback with a nullptr frame");
+ WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": KF", _keyframe->id(), " callback received with ts = ", _keyframe->getTimeStamp());
// profiling
n_kf_callback_++;
startKFProfiling();
- // asking if key frame should be stored
- if (storeKeyFrame(_keyframe_ptr))
- buffer_pack_kf_.add(_keyframe_ptr, _time_tol_other);
+ // asking if frame should be stored
+ if (storeKeyFrame(_keyframe))
+ buffer_frame_.emplace(_keyframe->getTimeStamp(), _keyframe);
- // if trigger true -> processKeyFrame
- if (triggerInKeyFrame(_keyframe_ptr, _time_tol_other))
- processKeyFrame(_keyframe_ptr, _time_tol_other);
+ // asking if frame should be processed
+ if (triggerInKeyFrame(_keyframe))
+ processKeyFrame(_keyframe);
// profiling
stopKFProfiling();
}
-void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
+void ProcessorBase::captureCallback(CaptureBasePtr _capture)
{
- assert(_capture_ptr != nullptr && "captureCallback with a nullptr capture");
- WOLF_DEBUG("P", isMotion() ? "M " : "T ", getName(), ": Capture ", _capture_ptr->id(), " callback received with ts = ", _capture_ptr->getTimeStamp());
+ assert(_capture != nullptr && "captureCallback with a nullptr capture");
+ WOLF_DEBUG("P", isMotionProvider() ? "M " : "T ", getName(), ": Capture ", _capture->id(), " callback received with ts = ", _capture->getTimeStamp());
// profiling
n_capture_callback_++;
@@ -65,15 +86,15 @@ void ProcessorBase::captureCallback(CaptureBasePtr _capture_ptr)
// apply prior in problem if not done (very first capture)
if (getProblem() && !getProblem()->isPriorSet())
- getProblem()->applyPriorOptions(_capture_ptr->getTimeStamp());
+ getProblem()->applyPriorOptions(_capture->getTimeStamp());
// asking if capture should be stored
- if (storeCapture(_capture_ptr))
- buffer_capture_.add(_capture_ptr->getTimeStamp(), _capture_ptr);
+ if (storeCapture(_capture))
+ buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
- // if trigger, process directly without buffering
- if (triggerInCapture(_capture_ptr))
- processCapture(_capture_ptr);
+ // asking if capture should be processed
+ if (triggerInCapture(_capture))
+ processCapture(_capture);
// profiling
stopCaptureProfiling();
@@ -86,12 +107,12 @@ void ProcessorBase::remove()
is_removing_ = true;
ProcessorBasePtr this_p = shared_from_this();
- if (isMotion())
+ if (isMotionProvider())
{
ProblemPtr P = getProblem();
- auto this_proc_cast_attempt = std::dynamic_pointer_cast<IsMotion>( shared_from_this() );
+ auto this_proc_cast_attempt = std::dynamic_pointer_cast<MotionProvider>( shared_from_this() );
if(P && this_proc_cast_attempt )
- P->removeProcessorIsMotion(this_proc_cast_attempt);
+ P->removeMotionProvider(this_proc_cast_attempt);
}
// remove from upstream
@@ -128,107 +149,12 @@ void ProcessorBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
- // adding processor is motion to the processor is motion vector
- auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this());
- if (is_motion_ptr)
- is_motion_ptr->addToProblem(_problem, is_motion_ptr);
+ // adding motion provider to the motion providers vector
+ auto motion_provider_ptr = std::dynamic_pointer_cast<MotionProvider>(shared_from_this());
+ if (motion_provider_ptr)
+ motion_provider_ptr->addToProblem(_problem, motion_provider_ptr);
}
-/////////////////////////////////////////////////////////////////////////////////////////
-
-void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const double& _time_tolerance)
-{
- TimeStamp time_stamp = _key_frame->getTimeStamp();
- PackKeyFramePtr kfpack = std::make_shared<PackKeyFrame>(_key_frame, _time_tolerance);
- Buffer::add(time_stamp, kfpack);
-}
-
-PackKeyFramePtr BufferPackKeyFrame::selectPack(const TimeStamp& _time_stamp, const double& _time_tolerance)
-{
- if (container_.empty())
- return nullptr;
-
- BufferPackKeyFrame::Iterator post = container_.upper_bound(_time_stamp);
-
- // remove packs corresponding to removed KFs (keeping the next iterator in post)
- while (post != container_.end() && post->second->key_frame->isRemoving())
- post = container_.erase(post);
- while (post != container_.begin() && std::prev(post)->second->key_frame->isRemoving())
- container_.erase(std::prev(post));
-
- bool prev_exists = (post != container_.begin());
- bool post_exists = (post != container_.end());
-
- bool post_ok = post_exists && doubleCheckTimeTolerance(post->first, post->second->time_tolerance, _time_stamp, _time_tolerance);
-
- if (prev_exists)
- {
- BufferPackKeyFrame::Iterator prev = std::prev(post);
-
- bool prev_ok = doubleCheckTimeTolerance(prev->first, prev->second->time_tolerance, _time_stamp, _time_tolerance);
-
- if (prev_ok && !post_ok)
- return prev->second;
-
- else if (!prev_ok && post_ok)
- return post->second;
-
- else if (prev_ok && post_ok)
- {
- if (std::fabs(post->first - _time_stamp) < std::fabs(prev->first - _time_stamp))
- return post->second;
- else
- return prev->second;
- }
- }
- else if (post_ok)
- return post->second;
-
- return nullptr;
-}
-PackKeyFramePtr BufferPackKeyFrame::selectPack(const CaptureBasePtr _capture, const double& _time_tolerance)
-{
- return selectPack(_capture->getTimeStamp(), _time_tolerance);
-}
-
-PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const TimeStamp& _time_stamp, const double& _time_tolerance)
-{
- // remove packs corresponding to removed KFs
- while (!container_.empty() && container_.begin()->second->key_frame->isRemoving())
- container_.erase(container_.begin());
-
- // There is no pack
- if (container_.empty())
- return nullptr;
-
- // Checking on begin() since packs are ordered in time
- // Return first pack if is older than time stamp
- if (container_.begin()->first < _time_stamp)
- return container_.begin()->second;
-
- // Return first pack if despite being newer, it is within the time tolerance
- if (doubleCheckTimeTolerance(container_.begin()->first, container_.begin()->second->time_tolerance, _time_stamp, _time_tolerance))
- return container_.begin()->second;
-
- // otherwise return nullptr (no pack before the provided ts or within the tolerance was found)
- return nullptr;
-
-}
-
-PackKeyFramePtr BufferPackKeyFrame::selectFirstPackBefore(const CaptureBasePtr _capture, const double& _time_tolerance)
-{
- return selectFirstPackBefore(_capture->getTimeStamp(), _time_tolerance);
-}
-
-void BufferPackKeyFrame::print(void) const
-{
- std::cout << "[ ";
- for (auto iter : container_)
- {
- std::cout << "( tstamp: " << iter.first << ", id: " << iter.second->key_frame->id() << ") ";
- }
- std::cout << "]" << std::endl;
-}
void ProcessorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
@@ -291,4 +217,25 @@ void ProcessorBase::printProfiling(std::ostream& _stream) const
}
+bool ProcessorBase::checkTimeTolerance (const TimeStamp& _ts1, const TimeStamp& _ts2)
+{
+ auto dt = std::fabs(_ts1 - _ts2);
+ return dt <= params_->time_tolerance;
+}
+
+bool ProcessorBase::checkTimeTolerance (const CaptureBasePtr _cap, const TimeStamp& _ts)
+{
+ return checkTimeTolerance(_cap->getTimeStamp(), _ts);
+}
+
+bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const TimeStamp& _ts)
+{
+ return checkTimeTolerance(_frm->getTimeStamp(), _ts);
+}
+
+bool ProcessorBase::checkTimeTolerance (const FrameBasePtr _frm, const CaptureBasePtr _cap)
+{
+ return checkTimeTolerance(_frm->getTimeStamp(), _cap->getTimeStamp());
+}
+
} // namespace wolf
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 43f15c52d8e0eee217a4c824c67bca1e9ae5b9e3..db06d0078261369000aed77e8d8f4d290cd2a355 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.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--------
/**
* \file processor_diff_drive.cpp
*
@@ -8,7 +29,8 @@
#include "core/processor/processor_diff_drive.h"
#include "core/sensor/sensor_diff_drive.h"
-#include "core/feature/feature_motion.h"
+//#include "core/feature/feature_motion.h"
+#include "core/feature/feature_diff_drive.h"
#include "core/factor/factor_diff_drive.h"
#include "core/math/SE2.h"
@@ -146,8 +168,7 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
{
- auto key_feature_ptr = FeatureBase::emplace<FeatureMotion>(_capture_motion,
- "ProcessorDiffDrive",
+ auto key_feature_ptr = FeatureBase::emplace<FeatureDiffDrive>(_capture_motion,
_capture_motion->getBuffer().back().delta_integr_,
_capture_motion->getBuffer().back().delta_integr_cov_,
_capture_motion->getCalibrationPreint(),
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fixed_wing_model.cpp
similarity index 53%
rename from src/processor/processor_fix_wing_model.cpp
rename to src/processor/processor_fixed_wing_model.cpp
index 7c12e90f8f08005d2f0086d57ad1633534dbe8ea..a994d8a3cef2b93dcf0e5897392a1e9cf8d1533d 100644
--- a/src/processor/processor_fix_wing_model.cpp
+++ b/src/processor/processor_fixed_wing_model.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--------
/*
* processor_fix_wing_model.cpp
*
@@ -5,7 +26,7 @@
* Author: joanvallve
*/
-#include "core/processor/processor_fix_wing_model.h"
+#include "../../include/core/processor/processor_fixed_wing_model.h"
#include "core/capture/capture_base.h"
#include "core/feature/feature_base.h"
@@ -14,22 +35,22 @@
namespace wolf
{
-ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params) :
- ProcessorBase("ProcessorFixWingModel", 3, _params),
+ProcessorFixedWingModel::ProcessorFixedWingModel(ParamsProcessorFixedWingModelPtr _params) :
+ ProcessorBase("ProcessorFixedWingModel", 3, _params),
params_processor_(_params)
{
}
-ProcessorFixWingModel::~ProcessorFixWingModel()
+ProcessorFixedWingModel::~ProcessorFixedWingModel()
{
}
-void ProcessorFixWingModel::configure(SensorBasePtr _sensor)
+void ProcessorFixedWingModel::configure(SensorBasePtr _sensor)
{
assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
}
-void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+void ProcessorFixedWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (_keyframe_ptr->getV()->isFixed())
return;
@@ -60,7 +81,7 @@ void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const do
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorFixWingModel);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixWingModel);
+WOLF_REGISTER_PROCESSOR(ProcessorFixedWingModel);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixedWingModel);
} // namespace wolf
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index 486be875ac6eb306e6e0a7973742b7f3e52241c7..2c8e16ce62b9cb38c47849ae7520dba095a51776 100644
--- a/src/processor/processor_loop_closure.cpp
+++ b/src/processor/processor_loop_closure.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/processor/processor_loop_closure.h"
namespace wolf
@@ -30,67 +51,67 @@ void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
process(_capture);
// remove the frame and older frames
- buffer_pack_kf_.removeUpTo(_capture->getFrame()->getTimeStamp());
+ buffer_frame_.removeUpTo(_capture->getFrame()->getTimeStamp());
return;
}
// Search for any stored frame within time tolerance of capture
- auto frame_pack = buffer_pack_kf_.select(_capture->getTimeStamp(), params_->time_tolerance);
+ auto keyframe_from_callback = buffer_frame_.select(_capture->getTimeStamp(), params_->time_tolerance);
// CASE 2:
- if (_capture->getFrame() == nullptr and frame_pack)
+ if (_capture->getFrame() == nullptr and keyframe_from_callback)
{
WOLF_DEBUG("CASE 2");
- _capture->link(frame_pack->key_frame);
+ _capture->link(keyframe_from_callback);
process(_capture);
// remove the frame and older frames
- buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp());
+ buffer_frame_.removeUpTo(keyframe_from_callback->getTimeStamp());
return;
}
// CASE 3:
WOLF_DEBUG("CASE 3");
- buffer_capture_.add(_capture->getTimeStamp(), _capture);
+ buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
}
-void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance)
+void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe)
{
/* This function has 4 scenarios:
* 1. Frame already have a capture of the sensor -> process
- * 2. Frame has a timestamp within time tolerances of any stored capture -> link + process
+ * 2. Frame has a timestamp within time tolerances of some stored capture -> link + process
* 3. Frame is more recent than any stored capture -> store frame to be processed later in processCapture
* 4. Otherwise: The frame is not compatible with any stored capture -> discard frame
*/
- WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _frame->id());
+ WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _keyframe->id());
// CASE 1:
- auto cap = _frame->getCaptureOf(getSensor());
- if (cap)
+ auto capture = _keyframe->getCaptureOf(getSensor());
+ if (capture)
{
WOLF_DEBUG("CASE 1");
- process(cap);
+ process(capture);
// remove the capture (if stored)
- buffer_capture_.getContainer().erase(cap->getTimeStamp());
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
return;
}
// Search for any stored capture within time tolerance of frame
- auto capture = buffer_capture_.select(_frame->getTimeStamp(), params_->time_tolerance);
+ capture = buffer_capture_.select(_keyframe->getTimeStamp(), params_->time_tolerance);
// CASE 2:
if (capture and not capture->getFrame())
{
WOLF_DEBUG("CASE 2");
- capture->link(_frame);
+ capture->link(_keyframe);
process(capture);
@@ -98,17 +119,17 @@ void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _t
buffer_capture_.getContainer().erase(capture->getTimeStamp());
// remove old captures (10s of old captures are kept in case frames arrives unordered)
- buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10);
+ buffer_capture_.removeUpTo(_keyframe->getTimeStamp() - 10);
return;
}
// CASE 3:
- if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
+ if (buffer_capture_.selectLastAfter(_keyframe->getTimeStamp(), params_->time_tolerance) == nullptr)
{
WOLF_DEBUG("CASE 3");
// store frame
- buffer_pack_kf_.add(_frame, _time_tolerance);
+ buffer_frame_.emplace(_keyframe->getTimeStamp(), _keyframe);
return;
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 9b7f8da44e1e58c86277a2cb689401426932c723..805f9bb4cac6847906a3aa9ed2e5f577dbdc63c4 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.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--------
/**
* \file processor_motion.cpp
*
@@ -23,7 +44,7 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
SizeEigen _calib_size,
ParamsProcessorMotionPtr _params_motion) :
ProcessorBase(_type, _dim, _params_motion),
- IsMotion(_state_structure, _params_motion),
+ MotionProvider(_state_structure, _params_motion),
params_motion_(_params_motion),
processing_step_(RUNNING_WITHOUT_KF),
x_size_(_state_size),
@@ -46,7 +67,13 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
jacobian_delta_(delta_cov_size_, delta_cov_size_),
jacobian_calib_(delta_cov_size_, calib_size_)
{
- //
+ jacobian_delta_preint_ .setIdentity(delta_cov_size_,delta_cov_size_); // dDp'/dDp, dDv'/dDv, all zeros
+ jacobian_delta_ .setIdentity(delta_cov_size_,delta_cov_size_); //
+ jacobian_calib_ .setZero(delta_cov_size_,calib_size_);
+ unmeasured_perturbation_cov_ =
+ params_motion_->unmeasured_perturbation_std
+ * params_motion_->unmeasured_perturbation_std
+ * MatrixXd::Identity(delta_cov_size_, delta_cov_size_);
}
ProcessorMotion::~ProcessorMotion()
@@ -95,8 +122,7 @@ void ProcessorMotion::mergeCaptures(CaptureMotionConstPtr cap_prev,
}
void ProcessorMotion::splitBuffer(const CaptureMotionPtr& _capture_source,
- TimeStamp _ts_split,
- const FrameBasePtr& _keyframe_target,
+ const TimeStamp _ts_split,
const CaptureMotionPtr& _capture_target) const
{
/** we are doing this:
@@ -162,9 +188,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
preProcess(); // Derived class operations
- PackKeyFramePtr pack = computeProcessingStep();
- if (pack)
- buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = computeProcessingStep();
+ if (keyframe_from_callback)
+ buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
switch(processing_step_)
{
@@ -183,7 +209,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
case FIRST_TIME_WITH_KF_ON_INCOMING :
{
// can joint to the KF
- setOrigin(pack->key_frame);
+ setOrigin(keyframe_from_callback);
break;
}
case FIRST_TIME_WITH_KF_AFTER_INCOMING :
@@ -238,26 +264,25 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
* \_f_/ \_f_/
*/
- // extract pack elements
- FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ // extract KF elements
+ TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
// find the capture whose buffer is affected by the new keyframe
- auto capture_existing = findCaptureContainingTimeStamp(ts_from_callback); // k
+ auto capture_existing = findCaptureContainingTimeStamp(timestamp_from_callback); // k
if (!capture_existing)
{
- WOLF_WARN("A KF before first motion capture (TS = ", ts_from_callback, "). ProcessorMotion cannot do anything.");
+ WOLF_WARN("A KF before first motion capture (TS = ", timestamp_from_callback, "). ProcessorMotion cannot do anything.");
break;
}
// update KF state (adding missing StateBlocks)
- auto proc_state = getState(ts_from_callback);
- for (auto pair_ckey_vec : proc_state)
- if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
- keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
- FactoryStateBlock::create(string(1, pair_ckey_vec.first),
- pair_ckey_vec.second,
+ auto proc_state = getState(timestamp_from_callback);
+ for (auto pair_key_vec : proc_state)
+ if (!keyframe_from_callback->isInStructure(pair_key_vec.first))
+ keyframe_from_callback->addStateBlock(pair_key_vec.first,
+ FactoryStateBlock::create(string(1, pair_key_vec.first),
+ pair_key_vec.second,
false),
getProblem());
keyframe_from_callback->setState(proc_state);
@@ -271,7 +296,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// emplace a new motion capture to the new keyframe
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
getSensor(),
- ts_from_callback,
+ timestamp_from_callback,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
calib_origin,
@@ -280,7 +305,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
+ splitBuffer(capture_existing, timestamp_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto feature_new = emplaceFeature(capture_for_keyframe_callback);
@@ -295,20 +320,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
capture_existing->getFeatureList().back()->remove(); // factor is removed automatically
assert(capture_existing->getFeatureList().empty());// there was only one feature!
+
auto new_feature_existing = emplaceFeature(capture_existing);
- emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
-// auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
-//
-// // Modify existing feature --------
-// feature_existing->setMeasurement (capture_existing->getBuffer().back().delta_integr_);
-// feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
-//
-// // Modify existing factor --------
-// // Instead of modifying, we remove one ctr, and create a new one.
-// auto fac_to_remove = feature_existing->getFactorList().back(); // there is only one factor!
-// auto new_ctr = emplaceFactor(feature_existing, capture_for_keyframe_callback);
-// fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
}
break;
@@ -349,17 +364,16 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
* \____f____/ \_f_/
*/
- // extract pack elements
- FrameBasePtr keyframe_from_callback = pack->key_frame;
- TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
+ // extract KF elements
+ TimeStamp timestamp_from_callback = keyframe_from_callback->getTimeStamp();
// update KF state (adding missing StateBlocks)
- auto proc_state = getState(ts_from_callback);
- for (auto pair_ckey_vec : proc_state)
- if (!keyframe_from_callback->isInStructure(pair_ckey_vec.first))
- keyframe_from_callback->addStateBlock(pair_ckey_vec.first,
- FactoryStateBlock::create(string(1, pair_ckey_vec.first),
- pair_ckey_vec.second,
+ auto proc_state = this->getState(timestamp_from_callback);
+ for (auto pair_key_vector : proc_state)
+ if (!keyframe_from_callback->isInStructure(pair_key_vector.first))
+ keyframe_from_callback->addStateBlock(pair_key_vector.first,
+ FactoryStateBlock::create(string(1, pair_key_vector.first),
+ pair_key_vector.second,
false),
getProblem());
keyframe_from_callback->setState(proc_state);
@@ -375,7 +389,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// emplace a new motion capture to the new keyframe
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
getSensor(),
- ts_from_callback,
+ timestamp_from_callback,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
calib_origin,
@@ -384,7 +398,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// split the buffer
// and give the part of the buffer before the new keyframe to the capture for the KF callback
- splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
+ splitBuffer(capture_existing, timestamp_from_callback, capture_for_keyframe_callback);
// create motion feature and add it to the capture
auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
@@ -408,11 +422,11 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_ptr_->setTimeStamp( ts );
last_ptr_->getFrame()->setTimeStamp( ts );
VectorComposite state_propa = getState( ts );
- for (auto pair_ckey_vec : state_propa)
- if (!last_ptr_->getFrame()->isInStructure(pair_ckey_vec.first))
- last_ptr_->getFrame()->addStateBlock(pair_ckey_vec.first,
- FactoryStateBlock::create(string(1, pair_ckey_vec.first),
- pair_ckey_vec.second,
+ for (auto & pair_key_vec : state_propa)
+ if (!last_ptr_->getFrame()->isInStructure(pair_key_vec.first))
+ last_ptr_->getFrame()->addStateBlock(pair_key_vec.first,
+ FactoryStateBlock::create(string(1, pair_key_vec.first),
+ pair_key_vec.second,
false),
getProblem());
last_ptr_->getFrame()->setState( state_propa );
@@ -454,8 +468,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
setCalibration(last_ptr_, getCalibration(origin_ptr_));
// Set the frame of last_ptr as key
- auto key_frame = last_ptr_->getFrame();
- key_frame ->link(getProblem());
+ auto keyframe = last_ptr_->getFrame();
+ keyframe ->link(getProblem());
// create motion feature and add it to the key_capture
auto key_feature = emplaceFeature(last_ptr_);
@@ -470,14 +484,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
// create a new capture
auto capture_new = emplaceCapture(frame_new,
getSensor(),
- key_frame->getTimeStamp(),
+ keyframe->getTimeStamp(),
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getCalibration(origin_ptr_),
getCalibration(origin_ptr_),
last_ptr_);
// reset the new buffer
- capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
+ capture_new->getBuffer().push_back( motionZero(keyframe->getTimeStamp()) ) ;
// reset derived things
resetDerived();
@@ -488,7 +502,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
last_frame_ptr_ = frame_new;
// callback to other processors
- getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance);
+ getProblem()->keyFrameCallback(keyframe, shared_from_this());
}
@@ -912,21 +926,22 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
return capture_motion;
}
-PackKeyFramePtr ProcessorMotion::computeProcessingStep()
+FrameBasePtr ProcessorMotion::computeProcessingStep()
{
// Origin not set
if (!origin_ptr_)
{
- PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(incoming_ptr_, params_motion_->time_tolerance);
+ FrameBasePtr keyframe_from_callback = buffer_frame_.selectFirstBefore(incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance);
- if (pack)
+ if (keyframe_from_callback)
{
- if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, incoming_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ if (checkTimeTolerance(keyframe_from_callback,
+ incoming_ptr_))
{
WOLF_DEBUG("First time with a KF compatible.")
processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
}
- else if (pack->key_frame->getTimeStamp() < incoming_ptr_->getTimeStamp())
+ else if (keyframe_from_callback->getTimeStamp() < incoming_ptr_->getTimeStamp())
{
WOLF_DEBUG("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
@@ -943,23 +958,24 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
processing_step_ = FIRST_TIME_WITHOUT_KF;
}
- return pack;
+ return keyframe_from_callback;
}
else
{
- PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
+ FrameBasePtr keyframe_from_callback = buffer_frame_.selectFirstBefore(last_ptr_->getTimeStamp(), params_motion_->time_tolerance);
// ignore "future" KF to avoid MotionBuffer::split() error
- if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().back().ts_)
- pack = nullptr;
+ if (keyframe_from_callback && keyframe_from_callback->getTimeStamp() > last_ptr_->getBuffer().back().ts_)
+ keyframe_from_callback = nullptr;
- if (pack)
+ if (keyframe_from_callback)
{
- if (buffer_pack_kf_.checkTimeTolerance(pack->key_frame->getTimeStamp(), pack->time_tolerance, origin_ptr_->getTimeStamp(), params_motion_->time_tolerance))
+ if (checkTimeTolerance(keyframe_from_callback,
+ origin_ptr_))
processing_step_ = RUNNING_WITH_KF_ON_ORIGIN;
- else if (pack->key_frame->getTimeStamp() < origin_ptr_->getTimeStamp())
+ else if (keyframe_from_callback->getTimeStamp() < origin_ptr_->getTimeStamp())
processing_step_ = RUNNING_WITH_KF_BEFORE_ORIGIN;
@@ -971,22 +987,13 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
else
processing_step_ = RUNNING_WITHOUT_KF;
- return pack;
+ return keyframe_from_callback;
}
// not reached
return nullptr;
}
-bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)
-{
- return true;
-}
-bool ProcessorMotion::storeCapture(CaptureBasePtr _cap_ptr)
-{
- return false;
-}
-
TimeStamp ProcessorMotion::getTimeStamp ( ) const
{
if (not origin_ptr_ or
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 573406efe82a815daecddc2abfdde1bd39ff2fb6..98f55c0f0d6c5ff9af08d627c1ef4d67b351783e 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.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/processor/processor_odom_2d.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/math/covariance.h"
@@ -11,7 +32,7 @@ ProcessorOdom2d::ProcessorOdom2d(ParamsProcessorOdom2dPtr _params) :
ProcessorMotion("ProcessorOdom2d", "PO", 2, 3, 3, 3, 2, 0, _params),
params_odom_2d_(_params)
{
- unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity();
+ //
}
ProcessorOdom2d::~ProcessorOdom2d()
@@ -106,6 +127,7 @@ bool ProcessorOdom2d::voteForKeyFrame() const
WOLF_DEBUG("PM", id(), " ", getType(), " votes per distance");
return true;
}
+ // Buffer length
if (getBuffer().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
{
WOLF_DEBUG("PM", id(), " ", getType(), " votes per angle");
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index e57ba32bd0d6c312d10e956e6806830b55164100..bf24a1d4e2d04c9111605792120df484e369f888 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.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/processor/processor_odom_3d.h"
#include "core/math/SE3.h"
@@ -8,11 +29,7 @@ ProcessorOdom3d::ProcessorOdom3d(ParamsProcessorOdom3dPtr _params) :
ProcessorMotion("ProcessorOdom3d", "PO", 3, 7, 7, 6, 6, 0, _params),
params_odom_3d_ (_params)
{
- // Set constant parts of Jacobians
- jacobian_delta_preint_.setIdentity(6,6);
- jacobian_delta_.setIdentity(6,6);
- jacobian_calib_.setZero(6,0);
- unmeasured_perturbation_cov_ = pow(params_odom_3d_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix6d::Identity();
+ //
}
ProcessorOdom3d::~ProcessorOdom3d()
@@ -152,12 +169,6 @@ void ProcessorOdom3d::statePlusDelta(const VectorComposite& _x,
bool ProcessorOdom3d::voteForKeyFrame() const
{
- //WOLF_DEBUG( "Time span : " , getBuffer().back().ts_ - getBuffer().front().ts_ );
- //WOLF_DEBUG( " last ts : ", getBuffer().back().ts_);
- //WOLF_DEBUG( " first ts : ", getBuffer().front().ts_);
- //WOLF_DEBUG( "BufferLength: " , getBuffer().size() );
- //WOLF_DEBUG( "DistTraveled: " , delta_integrated_.head(3).norm() );
- //WOLF_DEBUG( "AngleTurned : " , 2.0 * acos(delta_integrated_(6)) );
// time span
if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_odom_3d_->max_time_span)
{
@@ -184,7 +195,6 @@ bool ProcessorOdom3d::voteForKeyFrame() const
WOLF_DEBUG( "PM: vote: angle turned" );
return true;
}
- //WOLF_DEBUG( "PM: do not vote" );
return false;
}
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
index 70d9c6c976190d8a105e5984be914c1d4933a2e9..727a49d747057313cc1a6fd709d54e983d306e01 100644
--- a/src/processor/processor_pose.cpp
+++ b/src/processor/processor_pose.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--------
/**
* \file processor_pose.cpp
*
@@ -24,24 +45,19 @@ void ProcessorPose::configure(SensorBasePtr _sensor)
void ProcessorPose::createFactorIfNecessary(){
auto sensor_pose = std::static_pointer_cast<SensorPose>(getSensor());
- auto kf_it_last = buffer_pack_kf_.getContainer().end();
- auto kf_it = buffer_pack_kf_.getContainer().begin();
- while (kf_it != buffer_pack_kf_.getContainer().end())
+ auto kf_it_last = buffer_frame_.getContainer().end();
+ auto kf_it = buffer_frame_.getContainer().begin();
+ while (kf_it != buffer_frame_.getContainer().end())
{
TimeStamp t = kf_it->first;
- double time_tolerance = std::min(getTimeTolerance(), kf_it->second->time_tolerance);
- if (getTimeTolerance() == 0.0){
- WOLF_WARN("Time tolerance set to zero -> value not used");
- time_tolerance = kf_it->second->time_tolerance;
- }
- auto cap_it = buffer_capture_.selectIterator(t, time_tolerance);
+ auto cap_it = buffer_capture_.selectIterator(t, getTimeTolerance());
// if capture with corresponding timestamp is not found, assume you will get it later
if (cap_it != buffer_capture_.getContainer().end())
{
// if a corresponding capture exists, link it to the KF and create a factor
auto cap = std::static_pointer_cast<CapturePose>(cap_it->second);
- cap->link(kf_it->second->key_frame);
+ cap->link(kf_it->second);
FeatureBasePtr feat = FeatureBase::emplace<FeatureBase>(cap, "Pose", cap->getData(), cap->getDataCovariance());
FactorPose3dWithExtrinsicsPtr fac = FactorBase::emplace<FactorPose3dWithExtrinsics>(feat, feat, shared_from_this(), false, TOP_MOTION);
@@ -57,11 +73,11 @@ void ProcessorPose::createFactorIfNecessary(){
}
// whatever happened, remove very old captures
- buffer_capture_.removeUpTo(buffer_pack_kf_.getContainer().begin()->first - 5);
+ buffer_capture_.removeUpTo(buffer_frame_.getContainer().begin()->first - 5);
// now we erase the kf buffer if there was a match
- if (kf_it_last != buffer_pack_kf_.getContainer().end()){
- buffer_pack_kf_.getContainer().erase(buffer_pack_kf_.getContainer().begin(), std::next(kf_it_last));
+ if (kf_it_last != buffer_frame_.getContainer().end()){
+ buffer_frame_.getContainer().erase(buffer_frame_.getContainer().begin(), std::next(kf_it_last));
}
}
@@ -75,11 +91,11 @@ inline void ProcessorPose::processCapture(CaptureBasePtr _capture)
return;
}
// nothing to do if any of the two buffer is empty
- if(buffer_pack_kf_.empty()){
- WOLF_DEBUG("PInertialKinematic: KF pack buffer empty, time ", _capture->getTimeStamp());
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("PInertialKinematic: Frame buffer empty, time ", _capture->getTimeStamp());
return;
}
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("PInertialKinematics: Capture buffer empty, time ", _capture->getTimeStamp());
return;
}
@@ -88,7 +104,7 @@ inline void ProcessorPose::processCapture(CaptureBasePtr _capture)
}
-inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (!_keyframe_ptr)
{
@@ -96,11 +112,11 @@ inline void ProcessorPose::processKeyFrame(FrameBasePtr _keyframe_ptr, const dou
return;
}
// nothing to do if any of the two buffer is empty
- if(buffer_pack_kf_.empty()){
- WOLF_DEBUG("ProcessorPose: KF pack buffer empty, time ", _keyframe_ptr->getTimeStamp());
+ if(buffer_frame_.empty()){
+ WOLF_DEBUG("ProcessorPose: Frame buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
- if(buffer_pack_kf_.empty()){
+ if(buffer_frame_.empty()){
WOLF_DEBUG("ProcessorPose: Capture buffer empty, time ", _keyframe_ptr->getTimeStamp());
return;
}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 52421e86398bc049321e8b3dc5f75dc56d2d3001..c707c37eadae74f631b392317c286fe057713f76 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.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--------
/*
* ProcessorTracker.cpp
*
@@ -20,7 +41,7 @@ ProcessorTracker::ProcessorTracker(const std::string& _type,
ParamsProcessorTrackerPtr _params_tracker) :
ProcessorBase(_type, _dim, _params_tracker),
params_tracker_(_params_tracker),
- processing_step_(FIRST_TIME_WITHOUT_PACK),
+ processing_step_(FIRST_TIME_WITHOUT_KEYFRAME),
origin_ptr_(nullptr),
last_ptr_(nullptr),
incoming_ptr_(nullptr),
@@ -54,15 +75,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
switch(processing_step_)
{
- case FIRST_TIME_WITH_PACK :
+ case FIRST_TIME_WITH_KEYFRAME :
{
- PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance);
+ buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
+ WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
// Append incoming to KF
- incoming_ptr_->link(pack->key_frame);
+ incoming_ptr_->link(keyframe_from_callback);
// Process info
// TrackerFeature: We only process new features in Last, here last = nullptr, so we do not have anything to do.
@@ -71,28 +92,31 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Update pointers
resetDerived();
- origin_ptr_ = incoming_ptr_;
- last_ptr_ = incoming_ptr_;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = incoming_ptr_;
+ last_ptr_ = incoming_ptr_;
+ incoming_ptr_ = nullptr;
break;
}
- case FIRST_TIME_WITHOUT_PACK :
+ case FIRST_TIME_WITHOUT_KEYFRAME :
{
- WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_PACK" );
+ WOLF_DEBUG( "PT ", getName(), " FIRST_TIME_WITHOUT_KEYFRAME" );
- FrameBasePtr kfrm = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
- incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
- incoming_ptr_->link(kfrm);
+ // make a new KF at incoming
+ FrameBasePtr keyframe = FrameBase::emplace<FrameBase>(getProblem()->getTrajectory(),
+ incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
+
+ // link incoming to the new KF
+ incoming_ptr_->link(keyframe);
// Process info
processKnown();
// We only process new features in Last, here last = nullptr, so we do not have anything to do.
// Issue KF callback with new KF
- getProblem()->keyFrameCallback(kfrm, shared_from_this(), params_tracker_->time_tolerance);
+ getProblem()->keyFrameCallback(keyframe, shared_from_this());
resetDerived();
@@ -103,21 +127,23 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
break;
}
- case SECOND_TIME_WITH_PACK :
+ case SECOND_TIME_WITH_KEYFRAME :
{
// No-break case only for debug. Next case will be executed too.
- PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
- WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( incoming_ptr_->getTimeStamp(),
+ params_tracker_->time_tolerance);
+
+ WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
}
// Fall through
- case SECOND_TIME_WITHOUT_PACK :
+ case SECOND_TIME_WITHOUT_KEYFRAME :
{
- WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
+ WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" );
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- getProblem()->getState());
- incoming_ptr_->link(frm);
+ FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ getProblem()->getState());
+ incoming_ptr_->link(keyframe);
// We have a last_ Capture with no features, so we do not process known features, and we do not vote for KF.
// Process info
@@ -131,30 +157,31 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
resetDerived();
origin_ptr_ = last_ptr_;
last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
+ last_frame_ptr_ = keyframe;
incoming_ptr_ = nullptr;
break;
}
- case RUNNING_WITH_PACK :
+ case RUNNING_WITH_KEYFRAME :
{
- PackKeyFramePtr pack = buffer_pack_kf_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
- buffer_pack_kf_.removeUpTo( pack->key_frame->getTimeStamp() );
+ FrameBasePtr keyframe_from_callback = buffer_frame_.select( last_ptr_->getTimeStamp() ,
+ params_tracker_->time_tolerance);
+ buffer_frame_.removeUpTo( keyframe_from_callback->getTimeStamp() );
- WOLF_DEBUG( "PT ", getName(), " RUNNING_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
+ WOLF_DEBUG( "PT ", getName(), " RUNNING_WITH_KEYFRAME: KF" , keyframe_from_callback->id() , " callback unpacked with ts= " , keyframe_from_callback->getTimeStamp() );
processKnown();
// Capture last_ is added to the new keyframe
FrameBasePtr last_old_frame = last_ptr_->getFrame();
- last_ptr_->move(pack->key_frame);
+ last_ptr_->move(keyframe_from_callback);
last_old_frame->remove();
- // Create new frame
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ // Create new frame for incoming
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
getProblem()->getFrameStructure(),
getProblem()->getState());
- incoming_ptr_->link(frm);
+ incoming_ptr_->link(frame);
// Detect new Features, initialize Landmarks, create Factors, ...
processNew(params_tracker_->max_new_features);
@@ -164,16 +191,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
// Update pointers
resetDerived();
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
- incoming_ptr_ = nullptr;
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frame;
+ incoming_ptr_ = nullptr;
break;
}
- case RUNNING_WITHOUT_PACK :
+ case RUNNING_WITHOUT_KEYFRAME :
{
- WOLF_DEBUG( "PT ", getName(), " RUNNING_WITHOUT_PACK");
+ WOLF_DEBUG( "PT ", getName(), " RUNNING_WITHOUT_KEYFRAME");
processKnown();
@@ -197,20 +224,20 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
establishFactors();
// Call the new keyframe callback in order to let the other processors to establish their factors
- getProblem()->keyFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
+ getProblem()->keyFrameCallback(last_ptr_->getFrame(), shared_from_this());
// Update pointers
resetDerived();
// make F; append incoming to new F
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
- incoming_ptr_->link(frm);
- origin_ptr_ = last_ptr_;
- last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
- incoming_ptr_ = nullptr;
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
+ incoming_ptr_ ->link(frame);
+ origin_ptr_ = last_ptr_;
+ last_ptr_ = incoming_ptr_;
+ last_frame_ptr_ = frame;
+ incoming_ptr_ = nullptr;
}
else
@@ -221,15 +248,15 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
advanceDerived();
// Replace last frame for a new one in incoming
- FrameBasePtr frm = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
- getProblem()->getFrameStructure(),
- last_ptr_->getFrame()->getState());
- incoming_ptr_->link(frm);
- last_ptr_->getFrame()->remove(); // implicitly calling last_ptr_->remove();
+ FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+ getProblem()->getFrameStructure(),
+ last_ptr_->getFrame()->getState());
+ incoming_ptr_->link(frame);
+ last_ptr_->unlink(); // unlink last (destroying the frame) instead of frame destruction that would implicitly destroy last
// Update pointers
last_ptr_ = incoming_ptr_;
- last_frame_ptr_ = frm;
+ last_frame_ptr_ = frame;
incoming_ptr_ = nullptr;
}
break;
@@ -257,56 +284,43 @@ void ProcessorTracker::computeProcessingStep()
{
case FIRST_TIME :
- if (buffer_pack_kf_.selectPack(incoming_ptr_, params_tracker_->time_tolerance))
- processing_step_ = FIRST_TIME_WITH_PACK;
+ if (buffer_frame_.select(incoming_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
+ processing_step_ = FIRST_TIME_WITH_KEYFRAME;
else // ! last && ! pack(incoming)
- processing_step_ = FIRST_TIME_WITHOUT_PACK;
+ processing_step_ = FIRST_TIME_WITHOUT_KEYFRAME;
break;
case SECOND_TIME :
- if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
- processing_step_ = SECOND_TIME_WITH_PACK;
+ if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
+ processing_step_ = SECOND_TIME_WITH_KEYFRAME;
else
- processing_step_ = SECOND_TIME_WITHOUT_PACK;
+ processing_step_ = SECOND_TIME_WITHOUT_KEYFRAME;
break;
case RUNNING :
default :
- if (buffer_pack_kf_.selectPack(last_ptr_, params_tracker_->time_tolerance))
+ if (buffer_frame_.select(last_ptr_->getTimeStamp(), params_tracker_->time_tolerance))
{
if (last_ptr_->getFrame()->getProblem())
{
WOLF_WARN("||*||");
WOLF_INFO(" ... It seems you missed something!");
- WOLF_INFO("Pack's KF and last's Frame have matching time stamps (i.e. below time tolerances)");
+ WOLF_INFO("Received KF and last's Frame have matching time stamps (i.e. below time tolerances)");
WOLF_INFO("Check the following for correctness:");
WOLF_INFO(" - You have all processors installed before starting receiving any data");
WOLF_INFO(" - You have configured all your processors with compatible time tolerances");
- WOLF_ERROR("Pack's KF and last's KF have matching time stamps (i.e. below time tolerances).");
+ WOLF_ERROR("Received KF and last's KF have matching time stamps (i.e. below time tolerances).");
}
- processing_step_ = RUNNING_WITH_PACK;
+ processing_step_ = RUNNING_WITH_KEYFRAME;
}
else
- processing_step_ = RUNNING_WITHOUT_PACK;
+ processing_step_ = RUNNING_WITHOUT_KEYFRAME;
break;
}
}
-bool ProcessorTracker::triggerInCapture(CaptureBasePtr _cap_ptr) const
-{
- return true;
-}
-bool ProcessorTracker::storeKeyFrame(FrameBasePtr _frame_ptr)
-{
- return true;
-}
-bool ProcessorTracker::storeCapture(CaptureBasePtr _cap_ptr)
-{
- return false;
-}
-
void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
_stream << _tabs << "PrcT" << id() << " " << getType() << " \"" << getName() << "\"" << std::endl;
@@ -319,5 +333,6 @@ void ProcessorTracker::printHeader(int _depth, bool _constr_by, bool _metric, bo
if (getIncoming())
_stream << _tabs << " " << "i: Cap" << getIncoming()->id() << std::endl;
}
+
} // namespace wolf
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index cda95e5ed6259e7ef11d94d01c3fe939ffe61941..d55a2666cc00b3c8cdc53854ab43f7903f0b9d6e 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.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--------
/*
* \processor_tracker_feature.cpp
*
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index ec16f76dc095de7cf01ebf45400d0eacd7733516..bab09ac8279b7c9ec770e985b953b29ed9cb82af 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.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--------
/**
* \file processor_tracker_landmark.cpp
*
diff --git a/src/processor/track_matrix.cpp b/src/processor/track_matrix.cpp
index da6a88b75e98e0ada648f76155f5d12501c21c4d..1830270b13d7fea4d6462f2c97c7977b0879308f 100644
--- a/src/processor/track_matrix.cpp
+++ b/src/processor/track_matrix.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--------
/**
* \file track_matrix.cpp
*
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0700d403f00f24fc4effcc8d176cdea9277be1a5..0e649ec5cce5c11ca522c21bab70c67855922839 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.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/sensor/sensor_base.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
@@ -437,7 +458,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
{
auto sb = getStateBlockDynamic(key);
if (sb)
- _stream << _tabs << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
+ _stream << _tabs << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
<< std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl;
}
}
@@ -453,7 +474,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
{
const auto &sb = getStateBlockDynamic(key);
if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
+ _stream << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
_stream << std::endl;
}
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index afdc43e02a8085feb99c4ae0ac1a9a4cdefe2162..4620caea4e169aa3ef64d233e6883fdea53bed02 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.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--------
/**
* \file sensor_diff_drive.cpp
*
diff --git a/src/sensor/sensor_model.cpp b/src/sensor/sensor_model.cpp
deleted file mode 100644
index 6eb00f021667436360821f27e5684a512eb425e4..0000000000000000000000000000000000000000
--- a/src/sensor/sensor_model.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-#include "core/sensor/sensor_model.h"
-
-namespace wolf {
-
-
-SensorModel::SensorModel() :
- SensorBase("SensorModel", nullptr, nullptr, nullptr, 6)
-{
- //
-}
-
-SensorModel::~SensorModel()
-{
- //
-}
-
-} // namespace wolf
-
-// Register in the FactorySensor
-#include "core/sensor/factory_sensor.h"
-namespace wolf {
-WOLF_REGISTER_SENSOR(SensorModel);
-WOLF_REGISTER_SENSOR_AUTO(SensorModel);
-}
diff --git a/src/sensor/sensor_motion_model.cpp b/src/sensor/sensor_motion_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9ef4a8098144ab88374b54b16e61526a93db6c68
--- /dev/null
+++ b/src/sensor/sensor_motion_model.cpp
@@ -0,0 +1,45 @@
+//--------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/core/sensor/sensor_motion_model.h"
+
+namespace wolf {
+
+
+SensorMotionModel::SensorMotionModel() :
+ SensorBase("SensorMotionModel", nullptr, nullptr, nullptr, 6)
+{
+ //
+}
+
+SensorMotionModel::~SensorMotionModel()
+{
+ //
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorMotionModel);
+WOLF_REGISTER_SENSOR_AUTO(SensorMotionModel);
+}
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index 574e4277e9181ee768b9bb08df1ba3f4d5082dd0..711545f66b48b99fcfaf891954db3ddba7b56d7a 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/src/sensor/sensor_odom_2d.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/sensor/sensor_odom_2d.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_angle.h"
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index b261029d677a40c7c8b821b1e4708251d4bd38ca..b44561ff402bef4971b211cfa2eae43724457fc3 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/src/sensor/sensor_odom_3d.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--------
/**
* \file sensor_odom_3d.cpp
*
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
index c308bed7130e146599e857170655488d52df3521..87f2880b8f3e58e1898470572e630375bd792d8c 100644
--- a/src/sensor/sensor_pose.cpp
+++ b/src/sensor/sensor_pose.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--------
/**
* \file sensor_pose.cpp
*
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 99dfb8729c9bc5cb05f00758494571de9d52fc23..e0168dd1bc83a6f349f32f31995d08a776fa5699 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.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/solver/solver_manager.h"
#include "core/trajectory/trajectory_base.h"
#include "core/map/map_base.h"
@@ -163,11 +184,6 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
// update StateBlocks with optimized state value.
auto start_state = std::chrono::high_resolution_clock::now();
- /// @todo whatif someone has changed the state notification during opti ??
- /// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
-
- //std::map<StateBlockPtr, Eigen::VectorXd>::iterator it = state_blocks_.begin(),
- // it_end = state_blocks_.end();
for (auto& stateblock_statevector : state_blocks_)
{
// Avoid usuless copies
@@ -185,6 +201,10 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
return report;
}
+bool SolverManager::computeCovariances()
+{
+ return computeCovariances(params_->cov_enum);
+}
bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks)
{
@@ -500,11 +520,21 @@ int SolverManager::numStateBlocksFloating() const
return floating_state_blocks_.size();
}
+ParamsSolverPtr SolverManager::getParams() const
+{
+ return params_;
+}
+
double SolverManager::getPeriod() const
{
return params_->period;
}
+double SolverManager::getCovPeriod() const
+{
+ return params_->cov_period;
+}
+
bool SolverManager::check(std::string prefix) const
{
bool ok = true;
@@ -627,6 +657,7 @@ void SolverManager::printProfiling(std::ostream& _stream) const
<< "\n\t\tUpdate state: " << 1e-3*max_duration_state_.count() << " ms"
<< "\n\tSolving covariance: " << 1e-3*max_duration_cov_.count() << " ms" << std::endl;
+ printProfilingDerived(_stream);
}
} // namespace wolf
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index f94353d93dff9a0425e984372dde89b0116c8b4d..0f4d8283734110d8a7d19bd8b8a3681ad5f1641f 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.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 "../../include/core/ceres_wrapper/solver_ceres.h"
SolverManager::SolverManager()
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index ce743db05620f2e5bea90fd6cb8512c9669d394f..36cada937e0412330e7242d2092948807549fea9 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.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/state_block/has_state_blocks.h"
@@ -63,7 +84,7 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
auto sb = getStateBlock(key);
if (sb)
_stream << _tabs << " " << key
- << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << " [" << (sb->isFixed() ? "Fix" : "Est")
<< "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
<< " @ " << sb << std::endl;
}
@@ -82,7 +103,7 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
const auto &sb = getStateBlock(key);
if (sb)
_stream << " " << key
- << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << " [" << (sb->isFixed() ? "Fix" : "Est")
<< "] @ " << sb;
}
_stream << std::endl;
diff --git a/src/state_block/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp
index aee3f704126bf4ba81e9658b3c337ded088c78a5..93f9060918960bc56c22697d0a538c6ac73a7a0f 100644
--- a/src/state_block/local_parametrization_base.cpp
+++ b/src/state_block/local_parametrization_base.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/state_block/local_parametrization_base.h"
namespace wolf {
diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
index 846b14a49934c70c0d3879efd03f454ffab74605..c6e90d7a5b91de8a6db43b29cdb292078ba655b4 100644
--- a/src/state_block/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.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--------
/*
* \file local_parametrization_homogeneous.cpp
*
diff --git a/src/state_block/local_parametrization_quaternion.cpp b/src/state_block/local_parametrization_quaternion.cpp
index 727fa0de8f737dee986eb69e99581ad49b343355..601a93c540c2db7207a411977a3d9c439e9bb9dc 100644
--- a/src/state_block/local_parametrization_quaternion.cpp
+++ b/src/state_block/local_parametrization_quaternion.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/state_block/local_parametrization_quaternion.h"
#include "core/math/rotations.h"
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index 0280e2657a3e5bb0bd0dcc184b6800844ee68ef8..513d6d4627627a029e3ebe61bab550497d369634 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.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/state_block/state_block.h"
namespace wolf
{
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 04658a0fb6ccc9fd20412e650709d0d9320e169c..9f3e729eafd84f37fe67655142fb77555034253f 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.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--------
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index 4a64613a3445bfead7565f38ae5527f01f2fde5c..4bf0282fd90354856dc43b896a12a97ad88b1d7c 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.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/trajectory/trajectory_base.h"
#include "core/frame/frame_base.h"
diff --git a/src/tree_manager/tree_manager_sliding_window.cpp b/src/tree_manager/tree_manager_sliding_window.cpp
index 7d10c3167b5fe23aa279808e743bc4f8b4abb3da..5d6f1345ee09674750d129e64a89a87ca081902d 100644
--- a/src/tree_manager/tree_manager_sliding_window.cpp
+++ b/src/tree_manager/tree_manager_sliding_window.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/tree_manager/tree_manager_sliding_window.h"
namespace wolf
diff --git a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp b/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
index 35c20da4585856df0e18739dce5553c74efb4807..b2ab0e2fdc0d807e6953c0fab95c59ed391b5311 100644
--- a/src/tree_manager/tree_manager_sliding_window_dual_rate.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_dual_rate.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/tree_manager/tree_manager_sliding_window_dual_rate.h"
#include "core/capture/capture_motion.h"
#include "core/processor/processor_motion.h"
@@ -31,14 +52,14 @@ void TreeManagerSlidingWindowDualRate::keyFrameCallback(FrameBasePtr _frame)
params_swdr_->n_frames_recent - 1)->second;
// compose motion captures for all processors motion
- for (auto is_motion_pair : getProblem()->getProcessorIsMotionMap())
+ for (auto motion_provider_pair : getProblem()->getMotionProviderMap())
{
- auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(is_motion_pair.second);
+ auto proc_motion = std::dynamic_pointer_cast<ProcessorMotion>(motion_provider_pair.second);
if (proc_motion == nullptr)
{
- // FIXME: IsMotion::mergeCaptures pure virtual in IsMotion without need of casting
- WOLF_INFO("TreeManagerSlidingWindowDualRate::keyFrameCallback: IsMotion ",
- std::dynamic_pointer_cast<ProcessorBase>(is_motion_pair.second)->getName(),
+ // FIXME: MotionProvider::mergeCaptures pure virtual in MotionProvider without need of casting
+ WOLF_INFO("TreeManagerSlidingWindowDualRate::keyFrameCallback: MotionProvider ",
+ std::dynamic_pointer_cast<ProcessorBase>(motion_provider_pair.second)->getName(),
" couldn't be casted to ProcessorMotion. Not merging its captures...");
continue;
}
diff --git a/src/utils/check_log.cpp b/src/utils/check_log.cpp
index 06e58733aa3d99b9cffa312a9a88bdb223d15eb5..830073ea71f64c80d8bc2672204b101ddafaba50 100644
--- a/src/utils/check_log.cpp
+++ b/src/utils/check_log.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/check_log.h"
using namespace wolf;
diff --git a/src/utils/converter_utils.cpp b/src/utils/converter_utils.cpp
index 8f445c572dd03b2fa10aae48bb34f4a80798ed41..f77008e1bb528118d955789e15a914d329d804bc 100644
--- a/src/utils/converter_utils.cpp
+++ b/src/utils/converter_utils.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/converter_utils.h"
// Eigen
@@ -129,4 +150,4 @@ std::vector<std::string> parseList(std::string val) {
throw std::runtime_error("Unclosed delimiter [] or {}");
return words;
}
-} // namespace utils
\ No newline at end of file
+} // namespace utils
diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
index d2876701b84d3acabf5e4e9c761a49570bffc2fe..20dc65e0e7040cfe92c7d6a39b14f66092ef3f14 100644
--- a/src/utils/graph_search.cpp
+++ b/src/utils/graph_search.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/graph_search.h"
using namespace wolf;
diff --git a/src/utils/loader.cpp b/src/utils/loader.cpp
index 9fdf6984b6a3de57fa8a8ae062232a685897209f..c11bb26671bdf6368de27b4637b708bc6b54e87b 100644
--- a/src/utils/loader.cpp
+++ b/src/utils/loader.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/loader.h"
#include <dlfcn.h>
@@ -7,7 +28,7 @@ Loader::Loader(std::string _file)
{
path_ = _file;
}
-LoaderRaw::LoaderRaw(std::string _file) : Loader(_file)
+LoaderRaw::LoaderRaw(std::string _file) : Loader(_file), resource_(nullptr)
{
//
}
diff --git a/src/utils/params_server.cpp b/src/utils/params_server.cpp
index 6344466792706c9e499695721f95e781089ec07e..ce4a79743d24302907e85117b812458ed4de6c83 100644
--- a/src/utils/params_server.cpp
+++ b/src/utils/params_server.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/params_server.h"
using namespace wolf;
diff --git a/src/yaml/parser_yaml.cpp b/src/yaml/parser_yaml.cpp
index f07cc4d559c911e9cfc9e4ecb2797417c9b21201..b4c830bd547b7b07e288cb406027deab95751db3 100644
--- a/src/yaml/parser_yaml.cpp
+++ b/src/yaml/parser_yaml.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/yaml/parser_yaml.h"
#include <string>
diff --git a/src/yaml/processor_odom_3d_yaml.cpp b/src/yaml/processor_odom_3d_yaml.cpp
index bd168ee20b162426a85d7b1b78e508f2c00a8fe1..a8dd748ea29aa2a63eb72003b5c37465d62c3d17 100644
--- a/src/yaml/processor_odom_3d_yaml.cpp
+++ b/src/yaml/processor_odom_3d_yaml.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--------
/**
* \file processor_odom_3d_yaml.cpp
*
diff --git a/src/yaml/sensor_odom_2d_yaml.cpp b/src/yaml/sensor_odom_2d_yaml.cpp
index a33b7e3c7d1cf0b73374aa10510083bf9e47f9fd..68a44ff61adaabb99cb16cc1bc1c72017de6d813 100644
--- a/src/yaml/sensor_odom_2d_yaml.cpp
+++ b/src/yaml/sensor_odom_2d_yaml.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--------
/**
* \file sensor_odom_2d_yaml.cpp
*
diff --git a/src/yaml/sensor_odom_3d_yaml.cpp b/src/yaml/sensor_odom_3d_yaml.cpp
index edba0bd17d21b86508e01a407b4dea5bf41f91bd..02e0ff3d8e5296398a5f9cfc52b66fd21cf15d85 100644
--- a/src/yaml/sensor_odom_3d_yaml.cpp
+++ b/src/yaml/sensor_odom_3d_yaml.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--------
/**
* \file sensor_odom_3d_yaml.cpp
*
diff --git a/src/yaml/sensor_pose_yaml.cpp b/src/yaml/sensor_pose_yaml.cpp
index 4b9521595c7d5ac100e7d6dd72b266dc884d7336..102729bd91dcfca4f51a567f47d790ce1de6c6b7 100644
--- a/src/yaml/sensor_pose_yaml.cpp
+++ b/src/yaml/sensor_pose_yaml.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--------
/**
* \file sensor_pose_yaml.cpp
*
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 46964ac57b3430b8113edbb7ee23d4923fc0d50b..743787f40026b6449169470e1d3eb66c14bddc38 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -44,6 +44,10 @@ target_link_libraries(dummy ${PLUGIN_NAME})
# ------- First Core classes ----------
+# BufferFrame
+wolf_add_gtest(gtest_buffer_frame gtest_buffer_frame.cpp)
+target_link_libraries(gtest_buffer_frame ${PLUGIN_NAME} dummy)
+
# CaptureBase class test
wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
target_link_libraries(gtest_capture_base ${PLUGIN_NAME})
@@ -64,10 +68,6 @@ target_link_libraries(gtest_factor_autodiff ${PLUGIN_NAME})
wolf_add_gtest(gtest_factory_state_block gtest_factory_state_block.cpp)
target_link_libraries(gtest_factory_state_block ${PLUGIN_NAME})
-# FeatureBase classes test
-wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
-target_link_libraries(gtest_eigen_predicates ${PLUGIN_NAME})
-
# Node Emplace test
wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
target_link_libraries(gtest_emplace ${PLUGIN_NAME} dummy)
@@ -89,8 +89,8 @@ wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
# IsMotion classes test
-wolf_add_gtest(gtest_is_motion gtest_is_motion.cpp)
-target_link_libraries(gtest_is_motion ${PLUGIN_NAME})
+wolf_add_gtest(gtest_motion_provider gtest_motion_provider.cpp)
+target_link_libraries(gtest_motion_provider ${PLUGIN_NAME})
# LocalParametrizationXxx classes test
wolf_add_gtest(gtest_local_param gtest_local_param.cpp)
@@ -104,10 +104,6 @@ target_link_libraries(gtest_logging ${PLUGIN_NAME})
wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
target_link_libraries(gtest_motion_buffer ${PLUGIN_NAME})
-# PackKFBuffer
-wolf_add_gtest(gtest_pack_KF_buffer gtest_pack_KF_buffer.cpp)
-target_link_libraries(gtest_pack_KF_buffer ${PLUGIN_NAME} dummy)
-
# Parameters server
wolf_add_gtest(gtest_param_server gtest_param_server.cpp ${CMAKE_CURRENT_LIST_DIR})
target_link_libraries(gtest_param_server ${PLUGIN_NAME})
@@ -233,11 +229,11 @@ target_link_libraries(gtest_map_yaml ${PLUGIN_NAME})
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
-# ProcessorDiffDriveSelfcalib class test
-wolf_add_gtest(gtest_processor_fix_wing_model gtest_processor_fix_wing_model.cpp)
-target_link_libraries(gtest_processor_fix_wing_model ${PLUGIN_NAME})
+# ProcessorFixedWingModel class test
+wolf_add_gtest(gtest_processor_fixed_wing_model gtest_processor_fixed_wing_model.cpp)
+target_link_libraries(gtest_processor_fixed_wing_model ${PLUGIN_NAME})
-# ProcessorFixWingModel class test
+# ProcessorDiffDrive class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
diff --git a/test/dummy/factor_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h
index f2d174702710418cb6ff17ff55fd586be872ec68..5a52d9c47f61581048d132eb421d4e47474c02d4 100644
--- a/test/dummy/factor_dummy_zero_1.h
+++ b/test/dummy/factor_dummy_zero_1.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 FACTOR_DUMMY_ZERO_1_H_
#define FACTOR_DUMMY_ZERO_1_H_
diff --git a/test/dummy/factor_dummy_zero_12.h b/test/dummy/factor_dummy_zero_12.h
index 978592b6124abc815d4f0f2a897e0ef78e70d95b..707a99908b55bcf1204b54a75889ba9685a95b32 100644
--- a/test/dummy/factor_dummy_zero_12.h
+++ b/test/dummy/factor_dummy_zero_12.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 FACTOR_DUMMY_ZERO_12_H_
#define FACTOR_DUMMY_ZERO_12_H_
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index cf81f302567c80022f7c1c4abaf0e92f5341771f..c04043d857a43f6db5391b2c463b5328ab331a90 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.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 FACTOR_FEATURE_DUMMY_H
#define FACTOR_FEATURE_DUMMY_H
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index 5f6f21124d25ff194b042241e136f755761c4411..f4c53fa10da0908a5cd82ef3c83fe9796070abee 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.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 FACTOR_LANDMARK_DUMMY_H
#define FACTOR_LANDMARK_DUMMY_H
diff --git a/test/dummy/factor_odom_2d_autodiff.h b/test/dummy/factor_odom_2d_autodiff.h
index e765ed760472534f4de3024530ed07df7e15f0d1..9d09f97ae8614168701ad12940d950a14342f383 100644
--- a/test/dummy/factor_odom_2d_autodiff.h
+++ b/test/dummy/factor_odom_2d_autodiff.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 FACTOR_ODOM_2d_AUTODIFF_H_
#define FACTOR_ODOM_2d_AUTODIFF_H_
diff --git a/test/dummy/processor_is_motion_dummy.h b/test/dummy/processor_is_motion_dummy.h
deleted file mode 100644
index dca4808c90602d550156a9e486514480b2c7f6b1..0000000000000000000000000000000000000000
--- a/test/dummy/processor_is_motion_dummy.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/*
- * processor_is_motion_dummy.h
- *
- * Created on: Mar 8, 2021
- * Author: joanvallve
- */
-
-#ifndef TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
-#define TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_
-
-#include "core/processor/processor_base.h"
-#include "core/processor/is_motion.h"
-
-namespace wolf
-{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorIsMotionDummy);
-
-struct ParamsProcessorIsMotionDummy : public ParamsProcessorBase, public ParamsIsMotion
-{
- std::string state_structure = "PO";
-
- ParamsProcessorIsMotionDummy() = default;
- ParamsProcessorIsMotionDummy(std::string _unique_name, const ParamsServer& _server):
- ParamsProcessorBase(_unique_name, _server),
- ParamsIsMotion(_unique_name, _server)
- {
-
- };
-};
-
-WOLF_PTR_TYPEDEFS(ProcessorIsMotionDummy);
-
-class ProcessorIsMotionDummy : public ProcessorBase, public IsMotion
-{
- public:
- ProcessorIsMotionDummy(ParamsProcessorIsMotionDummyPtr _params) :
- ProcessorBase("ProcessorIsMotionDummy", 2, _params),
- IsMotion(_params->state_structure, _params)
- {}
- ~ProcessorIsMotionDummy(){};
-
- // Factory method for high level API
- WOLF_PROCESSOR_CREATE(ProcessorIsMotionDummy, ParamsProcessorIsMotionDummy);
-
- void configure(SensorBasePtr _sensor) override {};
- void processCapture(CaptureBasePtr) override {};
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {};
- bool triggerInCapture(CaptureBasePtr) const override { return false; };
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override { return false; };
- bool storeKeyFrame(FrameBasePtr) override { return false; };
- bool storeCapture(CaptureBasePtr) override { return false; };
- bool voteForKeyFrame() const override { return false; };
- TimeStamp getTimeStamp() const override {return TimeStamp(0);};
-
- VectorComposite getState(const StateStructure& _structure = "") const override
- {
- return getOdometry();
- };
-
- VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override
- {
- return getOdometry();
- };
-};
-
-} /* namespace wolf */
-
-// Register in the FactoryProcessor
-#include "core/processor/factory_processor.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorIsMotionDummy);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorIsMotionDummy);
-} // namespace wolf
-
-#endif /* TEST_DUMMY_PROCESSOR_IS_MOTION_DUMMY_H_ */
diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h
index 388f2f0dd5f13624d21115a9351680ceb92ce743..a0e456eec87ed473b11852a5981fb3ff2e494eba 100644
--- a/test/dummy/processor_loop_closure_dummy.h
+++ b/test/dummy/processor_loop_closure_dummy.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 TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_
#define TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_
@@ -80,7 +101,7 @@ class ProcessorLoopClosureDummy : public ProcessorLoopClosure
public:
unsigned int getNStoredFrames()
{
- return buffer_pack_kf_.getContainer().size();
+ return buffer_frame_.getContainer().size();
}
unsigned int getNStoredCaptures()
diff --git a/test/dummy/processor_motion_provider_dummy.h b/test/dummy/processor_motion_provider_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..c94013296126d940ca1526c153843bbe2ad6d88d
--- /dev/null
+++ b/test/dummy/processor_motion_provider_dummy.h
@@ -0,0 +1,97 @@
+//--------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--------
+/*
+ * processor_motion_provider_dummy.h
+ *
+ * Created on: Mar 8, 2021
+ * Author: joanvallve
+ */
+
+#ifndef TEST_DUMMY_PROCESSOR_MOTION_PROVIDER_DUMMY_H_
+#define TEST_DUMMY_PROCESSOR_MOTION_PROVIDER_DUMMY_H_
+
+#include <core/processor/motion_provider.h>
+#include "core/processor/processor_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsMotionProviderDummy);
+
+struct ParamsMotionProviderDummy : public ParamsProcessorBase, public ParamsMotionProvider
+{
+ std::string state_structure = "PO";
+
+ ParamsMotionProviderDummy() = default;
+ ParamsMotionProviderDummy(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server),
+ ParamsMotionProvider(_unique_name, _server)
+ {
+
+ };
+};
+
+WOLF_PTR_TYPEDEFS(MotionProviderDummy);
+
+class MotionProviderDummy : public ProcessorBase, public MotionProvider
+{
+ public:
+ MotionProviderDummy(ParamsMotionProviderDummyPtr _params) :
+ ProcessorBase("MotionProviderDummy", 2, _params),
+ MotionProvider(_params->state_structure, _params)
+ {}
+ ~MotionProviderDummy(){};
+
+ // Factory method for high level API
+ WOLF_PROCESSOR_CREATE(MotionProviderDummy, ParamsMotionProviderDummy);
+
+ void configure(SensorBasePtr _sensor) override {};
+ void processCapture(CaptureBasePtr) override {};
+ void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
+ bool triggerInCapture(CaptureBasePtr) const override { return false; };
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override { return false; };
+ bool storeKeyFrame(FrameBasePtr) override { return false; };
+ bool storeCapture(CaptureBasePtr) override { return false; };
+ bool voteForKeyFrame() const override { return false; };
+ TimeStamp getTimeStamp() const override {return TimeStamp(0);};
+
+ VectorComposite getState(const StateStructure& _structure = "") const override
+ {
+ return getOdometry();
+ };
+
+ VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override
+ {
+ return getOdometry();
+ };
+};
+
+} /* namespace wolf */
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(MotionProviderDummy);
+WOLF_REGISTER_PROCESSOR_AUTO(MotionProviderDummy);
+} // namespace wolf
+
+#endif /* TEST_DUMMY_PROCESSOR_MOTION_PROVIDER_DUMMY_H_ */
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index 8485d267034febde4f08911108c0773fd67ab892..ab58e3e0ef661f1f210a48ea82370c59b89d2082 100644
--- a/test/dummy/processor_tracker_feature_dummy.cpp
+++ b/test/dummy/processor_tracker_feature_dummy.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--------
/**
* \file processor_tracker_feature_dummy.cpp
*
diff --git a/test/dummy/processor_tracker_feature_dummy.h b/test/dummy/processor_tracker_feature_dummy.h
index 12705325ac8b8b086b10ec1f0a1a13870b76b702..d68a55e03bf427c1559add05b30d1a27c47ab87f 100644
--- a/test/dummy/processor_tracker_feature_dummy.h
+++ b/test/dummy/processor_tracker_feature_dummy.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--------
/**
* \file processor_tracker_feature_dummy.h
*
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index d36efd3f804cd71ff38fc7b835dc1b5230c59885..54525c8682b4c80d95e3e38b2ef34784e4a84cfe 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.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--------
/**
* \file processor_tracker_landmark_dummy.cpp
*
diff --git a/test/dummy/processor_tracker_landmark_dummy.h b/test/dummy/processor_tracker_landmark_dummy.h
index 58561c896a4764202654853cf852f7afb0cc954e..22060c82d0c27d4670cd94be45b652efaab0e902 100644
--- a/test/dummy/processor_tracker_landmark_dummy.h
+++ b/test/dummy/processor_tracker_landmark_dummy.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--------
/**
* \file processor_tracker_landmark_dummy.h
*
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index cf45c71c94167f934504e5f97f91080d4d011ce8..9c4df89d8d7ef2a6c0d9fd8ee568ba9d71247178 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.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--------
/*
* solver_manager_dummy.h
*
@@ -56,11 +77,13 @@ class SolverManagerDummy : public SolverManager
// The following are dummy implementations
- bool hasConverged() override { return true; }
- SizeStd iterations() override { return 1; }
- double initialCost() override { return double(1); }
- double finalCost() override { return double(0); }
- double totalTime() override { return double(0); }
+ bool hasConverged() override { return true; }
+ bool wasStopped() override { return false; }
+ unsigned int iterations() override { return 1; }
+ double initialCost() override { return double(1); }
+ double finalCost() override { return double(0); }
+ double totalTime() override { return double(0); }
+ void printProfilingDerived(std::ostream& _stream) const override {}
protected:
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
index 1e7e70f2ee476404cb96b070e72f280c7a570a6e..1f115ace871564d2f4f9ecf918ac9195f9dd580c 100644
--- a/test/dummy/tree_manager_dummy.h
+++ b/test/dummy/tree_manager_dummy.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 INCLUDE_TREE_MANAGER_DUMMY_H_
#define INCLUDE_TREE_MANAGER_DUMMY_H_
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index a99f7a19551f3e450e6b4958ded294db8980c62c..be07e98047c8074979d036074f2df6ee490e9a90 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.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--------
/**
* \file gtest_SE3.cpp
*
diff --git a/test/gtest_buffer_frame.cpp b/test/gtest_buffer_frame.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ebc17ca1312c50ddea210985cd9fa71c6e43113a
--- /dev/null
+++ b/test/gtest_buffer_frame.cpp
@@ -0,0 +1,266 @@
+//--------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--------
+/*
+ * gtest_buffer_frame.cpp
+ *
+ * Created on: Mar 5, 2018
+ * Author: jsola
+ */
+//Wolf
+#include "core/utils/utils_gtest.h"
+
+#include "core/processor/processor_odom_2d.h"
+#include "core/sensor/sensor_odom_2d.h"
+
+#include "dummy/processor_tracker_feature_dummy.h"
+#include "core/capture/capture_void.h"
+
+#include "core/problem/problem.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class BufferFrameTest : public testing::Test
+{
+ public:
+
+ BufferFrame buffer_kf;
+ FrameBasePtr f10, f20, f21, f28;
+ double tt10, tt20, tt21, tt28;
+ TimeStamp timestamp;
+ double timetolerance;
+
+ void SetUp(void) override
+ {
+ f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
+ f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
+ f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
+ f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
+
+ tt10 = 1.0;
+ tt20 = 1.0;
+ tt21 = 1.0;
+ tt28 = 1.0;
+ };
+};
+
+TEST_F(BufferFrameTest, empty)
+{
+ ASSERT_TRUE(buffer_kf.empty());
+}
+
+TEST_F(BufferFrameTest, emplace)
+{
+ buffer_kf.emplace(10, f10);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
+ buffer_kf.emplace(20, f20);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
+}
+
+TEST_F(BufferFrameTest, clear)
+{
+ buffer_kf.emplace(10, f10);
+ buffer_kf.emplace(20, f20);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
+ buffer_kf.clear();
+ ASSERT_TRUE(buffer_kf.empty());
+}
+
+//TEST_F(BufferFrameTest, print)
+//{
+// kfpackbuffer.clear();
+// kfpackbuffer.emplace(f10, tt10);
+// kfpackbuffer.emplace(f20, tt20);
+// kfpackbuffer.print();
+//}
+
+//TEST_F(BufferFrameTest, doubleCheckTimeTolerance)
+//{
+// buffer_kf.clear();
+// buffer_kf.emplace(10, f10);
+// buffer_kf.emplace(20, f20);
+// // min time tolerance > diff between time stamps. It should return true
+// ASSERT_TRUE(buffer_kf.doubleCheckTimeTolerance(10, 20, 20, 20));
+// // min time tolerance < diff between time stamps. It should return true
+// ASSERT_FALSE(buffer_kf.doubleCheckTimeTolerance(10, 1, 20, 20));
+//}
+
+//TEST_F(BufferFrameTest, select)
+//{
+// // Evaluation using two packs (p1,p2)
+// // with different time tolerances (tp1,tp2)
+// // using a query pack (q) with also different time tolerances
+// // depending on these tolerances we will get one (p1) or the other (p2)
+// // packages from the buffer (res).
+// // This can be summarized in the table hereafter:
+// //
+// // p1 p2 q | resulting pack time stamp
+// // --------------------------------
+// // 2 2 2 | nullptr
+// // 2 2 5 | nullptr
+// // 2 2 7 | nullptr
+// // 2 7 2 | nullptr
+// // 2 7 5 | 20
+// // 2 7 7 | 20
+// // 7 2 2 | nullptr
+// // 7 2 5 | nullptr
+// // 7 2 7 | 10
+// // 7 7 2 | nullptr
+// // 7 7 5 | 20
+// // 7 7 7 | 20
+//
+// buffer_kf.clear();
+//
+// // input packages
+// std::vector<int> p1 = {2, 7}; // Pack 1 time tolerances
+// std::vector<int> p2 = {2, 7}; // Pack 2 time tolerances
+// std::vector<int> q = {2, 5, 7}; // Query pack time tolerances
+//
+// // Solution matrix
+// Eigen::VectorXi res = Eigen::VectorXi::Zero(12);
+// res(4) = 20;
+// res(5) = 20;
+// res(8) = 10;
+// res(10) = 20;
+// res(11) = 20;
+//
+// // test
+// for (unsigned int ip1=0;ip1<p1.size();++ip1)
+// {
+// for (unsigned int ip2=0;ip2<p2.size();++ip2)
+// {
+// buffer_kf.emplace(f10, p1[ip1]);
+// buffer_kf.emplace(f20, p2[ip2]);
+// for (unsigned int iq=0;iq<q.size();++iq)
+// {
+// PackKeyFramePtr packQ = buffer_kf.selectPack(16, q[iq]);
+// if (packQ!=nullptr)
+// {
+// ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
+// }
+// }
+// buffer_kf.clear();
+// }
+// }
+//}
+
+//TEST_F(BufferFrameTest, selectFirstBefore)
+//{
+// buffer_kf.clear();
+//
+// buffer_kf.emplace(10, f10);
+// buffer_kf.emplace(20, f20);
+// buffer_kf.emplace(21, f21);
+//
+// // input time stamps
+// std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
+// double tt = 0.01;
+//
+// // Solution matrix
+// // q_ts | pack
+// //=================
+// // first time
+// //-----------------
+// // 9.5 nullptr
+// // 9.995 10
+// // 10,005 10
+// // 19.5 10
+// // 20.5 10
+// // 21.5 10
+// // second time
+// //-----------------
+// // 9.5 nullptr
+// // 9.995 null
+// // 10,005 null
+// // 19.5 null
+// // 20.5 20
+// // 21.5 20
+// // third time
+// //-----------------
+// // 9.5 nullptr
+// // 9.995 null
+// // 10,005 null
+// // 19.5 null
+// // 20.5 null
+// // 21.5 21
+//
+// Eigen::MatrixXd truth(3,6), res(3,6);
+// truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
+// res.setZero();
+//
+// for (int i=0; i<3; i++)
+// {
+// PackKeyFramePtr packQ;
+// int j = 0;
+// for (auto ts : q_ts)
+// {
+// packQ = buffer_kf.selectFirstPackBefore(ts, tt);
+// if (packQ)
+// res(i,j) = packQ->key_frame->getTimeStamp().get();
+//
+// j++;
+// }
+// buffer_kf.removeUpTo(packQ->key_frame->getTimeStamp());
+// }
+//
+// ASSERT_MATRIX_APPROX(res, truth, 1e-6);
+//}
+
+TEST_F(BufferFrameTest, removeUpTo)
+{
+ // Small time tolerance for all test asserts
+ double tt = 0.1;
+ buffer_kf.clear();
+ buffer_kf.emplace(10, f10);
+ buffer_kf.emplace(20, f20);
+ buffer_kf.emplace(21, f21);
+
+ // it should remove f20 and f10, thus size should be 1 after removal
+ // Specifically, only f21 should remain
+ buffer_kf.removeUpTo( f20->getTimeStamp() );
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
+ ASSERT_TRUE(buffer_kf.select(f10->getTimeStamp(),tt)==nullptr);
+ ASSERT_TRUE(buffer_kf.select(f20->getTimeStamp(),tt)==nullptr);
+ ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(),tt)!=nullptr);
+
+ // Chech removal of an imprecise time stamp
+ // Specifically, only f28 should remain
+ buffer_kf.emplace(28, f28);
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 2);
+ FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
+ // PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
+ buffer_kf.removeUpTo( f22->getTimeStamp() );
+ ASSERT_EQ(buffer_kf.size(), (unsigned int) 1);
+ ASSERT_TRUE(buffer_kf.select(f21->getTimeStamp(),tt)==nullptr);
+ ASSERT_TRUE(buffer_kf.select(f28->getTimeStamp(),tt)!=nullptr);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 3a0cbc4555c88bd40f0a656e2a73ff2a619654b6..aa36078108a936109484bd4a8d761c258105d236 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.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--------
/*
* gtest_capture_base.cpp
*
diff --git a/test/gtest_converter.cpp b/test/gtest_converter.cpp
index d068e024a3f9145f347a8645d7ba52dedd036c38..9be8c838f82822795dd4fe592ccb7620b949ea9e 100644
--- a/test/gtest_converter.cpp
+++ b/test/gtest_converter.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"
#include "core/utils/converter.h"
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
deleted file mode 100644
index 036bc2a4b80ef4eeb1a38b146096ff1f1cd81487..0000000000000000000000000000000000000000
--- a/test/gtest_eigen_predicates.cpp
+++ /dev/null
@@ -1,178 +0,0 @@
-#include "core/utils/utils_gtest.h"
-
-#include "core/utils/eigen_predicates.h"
-
-TEST(TestEigenPredicates, TestEigenDynPredZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Zero(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = Eigen::MatrixXd::Ones(4,4) * (wolf::Constants::EPS/2.);
-
- EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Zero();
- B = Eigen::Matrix4d::Random();
- C = Eigen::Matrix4d::Ones() * (wolf::Constants::EPS/2.);
-
- EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_zero(C, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredDiffZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredDiffZero)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredDiffNorm)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredDiffNorm)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_diff_norm_zero(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenDynPredIsApprox)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::MatrixXd::Random(4,4);
- B = Eigen::MatrixXd::Random(4,4);
- C = A;
-
- EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenFixPredIsApprox)
-{
- Eigen::MatrixXd A, B, C;
-
- A = Eigen::Matrix4d::Random();
- B = Eigen::Matrix4d::Random();
- C = A;
-
- EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredQuatIsApprox)
-{
- Eigen::Quaterniond A, B, C;
-
- /// @todo which version of Eigen provides this ?
-// A = Eigen::Quaterniond::UnitRandom();
-
- A.coeffs() = Eigen::Vector4d::Random().normalized();
- B.coeffs() = Eigen::Vector4d::Random().normalized();
- C = A;
-
- EXPECT_TRUE(wolf::pred_quat_is_approx(A, C, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_quat_is_approx(A, B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredQuatIsIdentity)
-{
- Eigen::Quaterniond A, B;
-
- A = Eigen::Quaterniond::Identity();
- B.coeffs() = Eigen::Vector4d::Random().normalized();
-
- EXPECT_TRUE(wolf::pred_quat_identity(A, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_quat_identity(B, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredAngleIsApprox)
-{
- double a = M_PI;
- double b = -M_PI;
- double c = 0;
-
- EXPECT_TRUE(wolf::pred_angle_is_approx(a, b, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_angle_is_approx(a, c, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-TEST(TestEigenPredicates, TestEigenPredAngleIsZero)
-{
- double a = 0;
- double b = M_PI;
- double c = 2.*M_PI;
-
- EXPECT_TRUE(wolf::pred_angle_zero(a, wolf::Constants::EPS));
- EXPECT_FALSE(wolf::pred_angle_zero(b, wolf::Constants::EPS));
- EXPECT_TRUE(wolf::pred_angle_zero(c, wolf::Constants::EPS));
-
-// PRINT_TEST_FINISHED;
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index f790bc57f70c9ab2d77e4b6fb9b2bcb822912d8c..0aa294035d9f712b706883636e46cd1e4ae62593 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.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--------
/*
* gtest_emplace.cpp
*
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index 65d779ffd8d244bb795181fef71eb978f613ef6c..e50f8c4fd64ce4e50d2feb498f1ff037d0497131 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_absolute.cpp b/test/gtest_factor_absolute.cpp
index e67a2f7407f10330a3cf30c37ed1f35d8bcbeaf5..2a98cc7a39d0d0b3ce8b6ab6de18bb01ae737eab 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.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--------
/**
* \file gtest_factor_absolute.cpp
*
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index c0cbacdec10e1fddd49db23a3c536622626b21df..7a1a330916e5ba4a5e7c28f784bd6bfd21278da7 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.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--------
/*
* gtest_factor_autodiff.cpp
*
diff --git a/test/gtest_factor_autodiff_distance_3d.cpp b/test/gtest_factor_autodiff_distance_3d.cpp
index 477520c0f0199b6c3d2c6ea2eba031122cc11561..2134768da0637461e60ffeac9473e1d43e365752 100644
--- a/test/gtest_factor_autodiff_distance_3d.cpp
+++ b/test/gtest_factor_autodiff_distance_3d.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--------
/**
* \file gtest_factor_autodiff_distance_3d.cpp
*
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index f861114e75bb24eceae0f747780bd89d2e1d3579..c8a04dcd16131c9e0549d5b34ed9d81d4c353ec8 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.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--------
/*
* gtest_factor_base.cpp
*
diff --git a/test/gtest_factor_block_difference.cpp b/test/gtest_factor_block_difference.cpp
index c314c74cefc9b901267b5b3e78c704c521a7f1b9..14724df2310a7aaed0bac11ce4334d837276e14b 100644
--- a/test/gtest_factor_block_difference.cpp
+++ b/test/gtest_factor_block_difference.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--------
/**
* \file gtest_factor_block_difference.cpp
*
@@ -50,7 +71,7 @@ class FixtureFactorBlockDifference : public testing::Test
VectorComposite x_origin(problem_->stateZero().vector("POV"), "POV", {3, 4, 3});
// Eigen::Matrix9d cov_prior = 1e-3 * Eigen::Matrix9d::Identity();
VectorComposite cov_prior("POV", {Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)),Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1))});
- KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0, 0.1);
+ KF0_ = problem_->setPriorFactor(x_origin, cov_prior, t0);
//CaptureBasePtr capV0 = CaptureBase::emplace<CaptureBase>(KF0_, "Vel0", t0);
//FeatureBasePtr featV0 = FeatureBase::emplace<FeatureBase>(capV0, "Vel0", x_origin.tail(3), cov_prior.bottomRightCorner<3,3>());
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index a2e7783be9bbb602c9e9e5376c1f13426234d4d9..4f280a6ff5482d8d02961438b0aaa119990bf78d 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.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--------
/**
* \file gtest_factor_diff_drive.cpp
*
@@ -466,7 +487,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
VectorComposite s0(Vector3d(0.1,0.1,0.1), "PO", {2,1});
// set prior at t=0 and processor origin
- auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
+ auto F0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
@@ -600,7 +621,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
// set prior at t=0 and processor origin
- auto F0 = problem->setPriorFactor(x0, s0, t, 0.1);
+ auto F0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(F0);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
diff --git a/test/gtest_factor_odom_2d_autodiff.cpp b/test/gtest_factor_odom_2d_autodiff.cpp
index 45315ddf2136a927674903c884d5715f6b1ae631..fd8aebb18f89a0d5276396d2940591ab7b9dcdcd 100644
--- a/test/gtest_factor_odom_2d_autodiff.cpp
+++ b/test/gtest_factor_odom_2d_autodiff.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"
#include "dummy/factor_odom_2d_autodiff.h"
diff --git a/test/gtest_factor_pose_2d.cpp b/test/gtest_factor_pose_2d.cpp
index d38ec528460e61a13c38970d142ac63b19f17b42..498df49f23cfea49b7cb80e223b9e2ccfbe65692 100644
--- a/test/gtest_factor_pose_2d.cpp
+++ b/test/gtest_factor_pose_2d.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--------
/**
* \file gtest_factor_pose_2d.cpp
*
diff --git a/test/gtest_factor_pose_3d.cpp b/test/gtest_factor_pose_3d.cpp
index 8bf5b82ed588a2466cbab42c35184c7158aed5c1..544ed512744a9b7d6b6e1e2ca99cf39cbc5ed164 100644
--- a/test/gtest_factor_pose_3d.cpp
+++ b/test/gtest_factor_pose_3d.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--------
/**
* \file gtest_factor_fix_3d.cpp
*
diff --git a/test/gtest_factor_relative_pose_2d.cpp b/test/gtest_factor_relative_pose_2d.cpp
index b4c11c8bb0316332910270b07d3aa11f2d8bad64..cc4030a9e51617399f2b256047f24eb30a93f5df 100644
--- a/test/gtest_factor_relative_pose_2d.cpp
+++ b/test/gtest_factor_relative_pose_2d.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"
#include "core/ceres_wrapper/solver_ceres.h"
diff --git a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp b/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
index eb1f5eb4f3a2d6eacc27cf52aa91a30693945358..ee2bc076da1c48876250197c2fa1179ede489306 100644
--- a/test/gtest_factor_relative_pose_2d_with_extrinsics.cpp
+++ b/test/gtest_factor_relative_pose_2d_with_extrinsics.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"
#include "core/ceres_wrapper/solver_ceres.h"
diff --git a/test/gtest_factor_relative_pose_3d.cpp b/test/gtest_factor_relative_pose_3d.cpp
index 5771b1246ee1746ed8de3b59d56cec8142ea5f90..9369c090efecad4b74435214230fca2b3b01f959 100644
--- a/test/gtest_factor_relative_pose_3d.cpp
+++ b/test/gtest_factor_relative_pose_3d.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--------
/**
* \file gtest_factor_relative_pose_3d.cpp
*
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
index 0834d95071973788df1aa053baba80adf3e95bf4..be2eb8584a83b27504239c8b784119e07c4e9528 100644
--- a/test/gtest_factor_velocity_local_direction_3d.cpp
+++ b/test/gtest_factor_velocity_local_direction_3d.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"
#include "core/ceres_wrapper/solver_ceres.h"
diff --git a/test/gtest_factory_state_block.cpp b/test/gtest_factory_state_block.cpp
index 96545d1bec4e39f906ef499c3d46c9398fa586c6..33b358362e0247c570ccd6ae48d64bf7a89ef31b 100644
--- a/test/gtest_factory_state_block.cpp
+++ b/test/gtest_factory_state_block.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--------
/*
* gtest_factory_state_block.cpp
*
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index bbb978af9c69260bb409ee28e0c9da88214609f5..1aa20b956c4bcebdd81e24fa0955812403f62930 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.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--------
/*
* gtest_feature_base.cpp
*
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index b37ce9708ffc0799ce95685a8a56d504b01c61a5..26c9b3cc0fe7f5aff4eb533c93826c84cb9d0d55 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.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--------
/*
* gtest_frame_base.cpp
*
diff --git a/test/gtest_graph_search.cpp b/test/gtest_graph_search.cpp
index 9693846e7994bf071a1aed368c99626c2311d43a..43d4b57242a3bb7d1e4532b893d34fcb47fcfcb2 100644
--- a/test/gtest_graph_search.cpp
+++ b/test/gtest_graph_search.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--------
/*
* gtest_graph_search.cpp
*
diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp
index 71e271a92641baffc54a01fd70c79922bbbe0d33..e60bc875310148a2fc0a127884b4fb1b3b77ab6a 100644
--- a/test/gtest_has_state_blocks.cpp
+++ b/test/gtest_has_state_blocks.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--------
/**
* \file gtest_has_state_blocks.cpp
*
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index 8b6ed4fc1e2ca38cc5618eaf0ee587ff408f5d4d..a709195a5e179f6c64bfa37cabf9fa8e2eb7e375 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.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--------
/*
* \file test_local_param_quat.cpp
*
diff --git a/test/gtest_logging.cpp b/test/gtest_logging.cpp
index 58480003d32db34aec7b643bfa39c4b8259a2d3b..d67ab498bd30238c90c082d9536a37bd4014d069 100644
--- a/test/gtest_logging.cpp
+++ b/test/gtest_logging.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--------
/**
* \file test_wolf_logging.cpp
*
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
index ce406cd1a8b21c4f63251b2f28336b0acca9e1ad..92d97edcb3e98596d7bcccdacf5b26b9607f6f15 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.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"
#include "core/common/wolf.h"
#include "core/math/covariance.h"
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index dd3f75031499911304c36a1e8a4f5fa9f7c10e04..5c4b2b7893837316736ab7a69a49f1b08c169b9b 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.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--------
/**
* \file test_map_yaml.cpp
*
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index f47e9bd12826c323be8f61c882ed638ba35a5745..108147b82c51e16ff7f42ea29388783f4b487278 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.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--------
/*
* gtest_motion_buffer.cpp
*
diff --git a/test/gtest_is_motion.cpp b/test/gtest_motion_provider.cpp
similarity index 70%
rename from test/gtest_is_motion.cpp
rename to test/gtest_motion_provider.cpp
index 0c8b8fa620a1f8b5261d9a5ca13a6299c896d6d9..982bc05a33ee7e998c823f9759533aad259b0792 100644
--- a/test/gtest_is_motion.cpp
+++ b/test/gtest_motion_provider.cpp
@@ -1,8 +1,30 @@
+//--------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--------
//Wolf
+#include "dummy/processor_motion_provider_dummy.h"
+
+#include "core/processor/motion_provider.h"
#include "core/utils/utils_gtest.h"
-#include "core/processor/is_motion.h"
-#include "dummy/processor_is_motion_dummy.h"
#include "core/sensor/sensor_odom_2d.h"
#include "core/problem/problem.h"
@@ -15,13 +37,13 @@ using namespace wolf;
using namespace Eigen;
-class IsMotionTest : public testing::Test
+class MotionProviderTest : public testing::Test
{
public:
ProblemPtr problem;
SensorBasePtr sen;
ProcessorBasePtr prc1, prc2, prc3;
- IsMotionPtr im1, im2, im3;
+ MotionProviderPtr im1, im2, im3;
std::string wolf_root = _WOLF_ROOT_DIR;
double dt = 0.01;
@@ -38,37 +60,37 @@ class IsMotionTest : public testing::Test
wolf_root + "/test/yaml/sensor_odom_2d.yaml");
// Install 3 odom processors
- ParamsProcessorIsMotionDummyPtr prc1_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ ParamsMotionProviderDummyPtr prc1_params = std::make_shared<ParamsMotionProviderDummy>();
prc1_params->time_tolerance = dt/2;
prc1_params->state_structure = "PO";
prc1_params->state_getter = false;
- prc1 = problem->installProcessor("ProcessorIsMotionDummy",
+ prc1 = problem->installProcessor("MotionProviderDummy",
"not getter processor",
sen,
prc1_params);
- im1 = std::dynamic_pointer_cast<IsMotion>(prc1);
+ im1 = std::dynamic_pointer_cast<MotionProvider>(prc1);
- ParamsProcessorIsMotionDummyPtr prc2_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ ParamsMotionProviderDummyPtr prc2_params = std::make_shared<ParamsMotionProviderDummy>();
prc2_params->time_tolerance = dt/2;
prc2_params->state_structure = "PO";
prc2_params->state_getter = true;
prc2_params->state_priority = 1;
- prc2 = problem->installProcessor("ProcessorIsMotionDummy",
+ prc2 = problem->installProcessor("MotionProviderDummy",
"getter processor",
sen,
prc2_params);
- im2 = std::dynamic_pointer_cast<IsMotion>(prc2);
+ im2 = std::dynamic_pointer_cast<MotionProvider>(prc2);
- ParamsProcessorIsMotionDummyPtr prc3_params = std::make_shared<ParamsProcessorIsMotionDummy>();
+ ParamsMotionProviderDummyPtr prc3_params = std::make_shared<ParamsMotionProviderDummy>();
prc3_params->time_tolerance = dt/2;
prc3_params->state_structure = "POV";
prc3_params->state_getter = true;
prc3_params->state_priority = 1;
- prc3 = problem->installProcessor("ProcessorIsMotionDummy",
+ prc3 = problem->installProcessor("MotionProviderDummy",
"getter processor low priority",
sen,
prc3_params);
- im3 = std::dynamic_pointer_cast<IsMotion>(prc3);
+ im3 = std::dynamic_pointer_cast<MotionProvider>(prc3);
}
};
@@ -82,12 +104,12 @@ class IsMotionTest : public testing::Test
* - Problem::getState/getOdometry (state_getter, state_priority)
*/
-TEST_F(IsMotionTest, install)
+TEST_F(MotionProviderTest, install)
{
- // All isMotion() = true
- ASSERT_TRUE (prc1->isMotion());
- ASSERT_TRUE (prc2->isMotion());
- ASSERT_TRUE (prc3->isMotion());
+ // All MotionProvider() = true
+ ASSERT_TRUE (prc1->isMotionProvider());
+ ASSERT_TRUE (prc2->isMotionProvider());
+ ASSERT_TRUE (prc3->isMotionProvider());
ASSERT_TRUE (im1 != nullptr);
ASSERT_TRUE (im2 != nullptr);
ASSERT_TRUE (im3 != nullptr);
@@ -102,13 +124,13 @@ TEST_F(IsMotionTest, install)
ASSERT_EQ(im2->getStateStructure(), "PO");
ASSERT_EQ(im3->getStateStructure(), "POV");
- // Only 2 and 3 in problem::processor_is_motion_map_
- ASSERT_EQ(problem->getProcessorIsMotionMap().size(), 2);
- ASSERT_EQ(problem->getProcessorIsMotionMap().begin()->second, im2);
- ASSERT_EQ(std::next(problem->getProcessorIsMotionMap().begin())->second, im3);
+ // Only 2 and 3 in problem::motion_provider_map_
+ ASSERT_EQ(problem->getMotionProviderMap().size(), 2);
+ ASSERT_EQ(problem->getMotionProviderMap().begin()->second, im2);
+ ASSERT_EQ(std::next(problem->getMotionProviderMap().begin())->second, im3);
}
-TEST_F(IsMotionTest, odometry)
+TEST_F(MotionProviderTest, odometry)
{
VectorComposite odom_p("P"); // missing P key
odom_p['P'] = Vector2d::Zero();
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 3aad59381e4fe9b37df987d18314fae17903cbde..225a49326c54d4e743bd7cd6c43bf839785b2e3d 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.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--------
/**
* \file test_odom_2d.cpp
*
@@ -125,7 +146,7 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d)
SolverCeres solver(Pr);
// KF0 and absolute prior
- FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0, dt/2);
+ FrameBasePtr F0 = Pr->setPriorFactor(x0, s0,t0);
// KF1 and motion from KF0
t += dt;
@@ -218,7 +239,7 @@ TEST(Odom2d, VoteForKfAndSolve)
SolverCeres solver(problem);
// Origin Key Frame
- auto KF = problem->setPriorFactor(x0, s0, t, dt/2);
+ auto KF = problem->setPriorFactor(x0, s0, t);
processor_odom2d->setOrigin(KF);
solver.solve(SolverManager::ReportVerbosity::BRIEF);
@@ -356,7 +377,7 @@ TEST(Odom2d, KF_callback)
SolverCeres solver(problem);
// Origin Key Frame
- FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0, dt/2);
+ FrameBasePtr keyframe_0 = problem->setPriorFactor(x0, x0_cov, t0);
processor_odom2d->setOrigin(keyframe_0);
// Check covariance values
@@ -426,7 +447,7 @@ TEST(Odom2d, KF_callback)
ASSERT_EQ(problem->getLastFrame()->getTimeStamp().getNanoSeconds(), 80000000);
ASSERT_TRUE(problem->check(0));
- processor_odom2d->keyFrameCallback(keyframe_2, dt/2);
+ processor_odom2d->keyFrameCallback(keyframe_2);
ASSERT_TRUE(problem->check(0));
t += dt;
capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
@@ -458,7 +479,7 @@ TEST(Odom2d, KF_callback)
FrameBasePtr keyframe_1 = problem->emplaceFrame(t_split, x_split);
ASSERT_TRUE(problem->check(0));
- processor_odom2d->keyFrameCallback(keyframe_1, dt/2);
+ processor_odom2d->keyFrameCallback(keyframe_1);
ASSERT_TRUE(problem->check(0));
t += dt;
capture = std::make_shared<CaptureOdom2d>(t, sensor_odom2d, data, data_cov, nullptr);
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
deleted file mode 100644
index d1fce41e50c72c91fb520d748a8154d5d516a1e6..0000000000000000000000000000000000000000
--- a/test/gtest_pack_KF_buffer.cpp
+++ /dev/null
@@ -1,246 +0,0 @@
-/*
- * gtest_pack_KF_buffer.cpp
- *
- * Created on: Mar 5, 2018
- * Author: jsola
- */
-//Wolf
-#include "core/utils/utils_gtest.h"
-
-#include "core/processor/processor_odom_2d.h"
-#include "core/sensor/sensor_odom_2d.h"
-
-#include "dummy/processor_tracker_feature_dummy.h"
-#include "core/capture/capture_void.h"
-
-#include "core/problem/problem.h"
-
-// STL
-#include <iterator>
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-class BufferPackKeyFrameTest : public testing::Test
-{
- public:
-
- BufferPackKeyFrame pack_kf_buffer;
- FrameBasePtr f10, f20, f21, f28;
- double tt10, tt20, tt21, tt28;
- TimeStamp timestamp;
- double timetolerance;
-
- void SetUp(void) override
- {
- f10 = std::make_shared<FrameBase>(TimeStamp(10),nullptr,nullptr,nullptr);
- f20 = std::make_shared<FrameBase>(TimeStamp(20),nullptr,nullptr,nullptr);
- f21 = std::make_shared<FrameBase>(TimeStamp(21),nullptr,nullptr,nullptr);
- f28 = std::make_shared<FrameBase>(TimeStamp(28),nullptr,nullptr,nullptr);
-
- tt10 = 1.0;
- tt20 = 1.0;
- tt21 = 1.0;
- tt28 = 1.0;
- };
-};
-
-TEST_F(BufferPackKeyFrameTest, empty)
-{
- ASSERT_TRUE(pack_kf_buffer.empty());
-}
-
-TEST_F(BufferPackKeyFrameTest, add)
-{
- pack_kf_buffer.add(f10, tt10);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
- pack_kf_buffer.add(f20, tt20);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
-}
-
-TEST_F(BufferPackKeyFrameTest, clear)
-{
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
- pack_kf_buffer.clear();
- ASSERT_TRUE(pack_kf_buffer.empty());
-}
-
-//TEST_F(BufferPackKeyFrameTest, print)
-//{
-// kfpackbuffer.clear();
-// kfpackbuffer.add(f10, tt10);
-// kfpackbuffer.add(f20, tt20);
-// kfpackbuffer.print();
-//}
-
-TEST_F(BufferPackKeyFrameTest, checkTimeTolerance)
-{
- pack_kf_buffer.clear();
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- // min time tolerance > diff between time stamps. It should return true
- ASSERT_TRUE(pack_kf_buffer.checkTimeTolerance(10, 20, 20, 20));
- // min time tolerance < diff between time stamps. It should return true
- ASSERT_FALSE(pack_kf_buffer.checkTimeTolerance(10, 1, 20, 20));
-}
-
-TEST_F(BufferPackKeyFrameTest, selectPack)
-{
- // Evaluation using two packs (p1,p2)
- // with different time tolerances (tp1,tp2)
- // using a query pack (q) with also different time tolerances
- // depending on these tolerances we will get one (p1) or the other (p2)
- // packages from the buffer (res).
- // This can be summarized in the table hereafter:
- //
- // p1 p2 q | resulting pack time stamp
- // --------------------------------
- // 2 2 2 | nullptr
- // 2 2 5 | nullptr
- // 2 2 7 | nullptr
- // 2 7 2 | nullptr
- // 2 7 5 | 20
- // 2 7 7 | 20
- // 7 2 2 | nullptr
- // 7 2 5 | nullptr
- // 7 2 7 | 10
- // 7 7 2 | nullptr
- // 7 7 5 | 20
- // 7 7 7 | 20
-
- pack_kf_buffer.clear();
-
- // input packages
- std::vector<int> p1 = {2, 7}; // Pack 1 time tolerances
- std::vector<int> p2 = {2, 7}; // Pack 2 time tolerances
- std::vector<int> q = {2, 5, 7}; // Query pack time tolerances
-
- // Solution matrix
- Eigen::VectorXi res = Eigen::VectorXi::Zero(12);
- res(4) = 20;
- res(5) = 20;
- res(8) = 10;
- res(10) = 20;
- res(11) = 20;
-
- // test
- for (unsigned int ip1=0;ip1<p1.size();++ip1)
- {
- for (unsigned int ip2=0;ip2<p2.size();++ip2)
- {
- pack_kf_buffer.add(f10, p1[ip1]);
- pack_kf_buffer.add(f20, p2[ip2]);
- for (unsigned int iq=0;iq<q.size();++iq)
- {
- PackKeyFramePtr packQ = pack_kf_buffer.selectPack(16, q[iq]);
- if (packQ!=nullptr)
- {
- ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
- }
- }
- pack_kf_buffer.clear();
- }
- }
-}
-
-TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore)
-{
- pack_kf_buffer.clear();
-
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- pack_kf_buffer.add(f21, tt21);
-
- // input time stamps
- std::vector<TimeStamp> q_ts = {9.5, 9.995, 10.005, 19.5, 20.5, 21.5};
- double tt = 0.01;
-
- // Solution matrix
- // q_ts | pack
- //=================
- // first time
- //-----------------
- // 9.5 nullptr
- // 9.995 10
- // 10,005 10
- // 19.5 10
- // 20.5 10
- // 21.5 10
- // second time
- //-----------------
- // 9.5 nullptr
- // 9.995 null
- // 10,005 null
- // 19.5 null
- // 20.5 20
- // 21.5 20
- // third time
- //-----------------
- // 9.5 nullptr
- // 9.995 null
- // 10,005 null
- // 19.5 null
- // 20.5 null
- // 21.5 21
-
- Eigen::MatrixXd truth(3,6), res(3,6);
- truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
- res.setZero();
-
- for (int i=0; i<3; i++)
- {
- PackKeyFramePtr packQ;
- int j = 0;
- for (auto ts : q_ts)
- {
- packQ = pack_kf_buffer.selectFirstPackBefore(ts, tt);
- if (packQ)
- res(i,j) = packQ->key_frame->getTimeStamp().get();
-
- j++;
- }
- pack_kf_buffer.removeUpTo(packQ->key_frame->getTimeStamp());
- }
-
- ASSERT_MATRIX_APPROX(res, truth, 1e-6);
-}
-
-TEST_F(BufferPackKeyFrameTest, removeUpTo)
-{
- // Small time tolerance for all test asserts
- double tt = 0.1;
- pack_kf_buffer.clear();
- pack_kf_buffer.add(f10, tt10);
- pack_kf_buffer.add(f20, tt20);
- pack_kf_buffer.add(f21, tt21);
-
- // it should remove f20 and f10, thus size should be 1 after removal
- // Specifically, only f21 should remain
- PackKeyFramePtr pack20 = std::make_shared<PackKeyFrame>(f20,tt20);
- pack_kf_buffer.removeUpTo( pack20->key_frame->getTimeStamp() );
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f10->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f20->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f21->getTimeStamp(),tt)!=nullptr);
-
- // Chech removal of an imprecise time stamp
- // Specifically, only f28 should remain
- pack_kf_buffer.add(f28, tt28);
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 2);
- FrameBasePtr f22 = std::make_shared<FrameBase>(TimeStamp(22),nullptr,nullptr,nullptr);
- PackKeyFramePtr pack22 = std::make_shared<PackKeyFrame>(f22,5);
- pack_kf_buffer.removeUpTo( pack22->key_frame->getTimeStamp() );
- ASSERT_EQ(pack_kf_buffer.size(), (unsigned int) 1);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f21->getTimeStamp(),tt)==nullptr);
- ASSERT_TRUE(pack_kf_buffer.selectPack(f28->getTimeStamp(),tt)!=nullptr);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 89670c6229a298741f61a4dceba52799297f22d4..194671976d7cf3957db6d7be7046d77f4f41e14a 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.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--------
/*
* gtest_param_prior.cpp
*
diff --git a/test/gtest_param_server.cpp b/test/gtest_param_server.cpp
index bf427f782f1fbb6e790a498535a8d1c03f56af55..e8116c8ff30c53e74f7824913554e6fd3fb9b524 100644
--- a/test/gtest_param_server.cpp
+++ b/test/gtest_param_server.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"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
diff --git a/test/gtest_parser_yaml.cpp b/test/gtest_parser_yaml.cpp
index df50e8f75bb0bdad8c015e83e87376a31f75573e..bb37da59d2ed7c662375470f0491217c874b6005 100644
--- a/test/gtest_parser_yaml.cpp
+++ b/test/gtest_parser_yaml.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"
#include "core/utils/converter.h"
#include "core/common/wolf.h"
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 6d0e7014d1a5d7b5f719cfae79953647ec704b09..d5fa0054e26e64664dba42b3d0fc66a717bd8709 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.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--------
/*
* gtest_problem.cpp
*
@@ -64,7 +85,7 @@ TEST(Problem, Processor)
ProblemPtr P = Problem::create("PO", 3);
// check motion processor is null
- ASSERT_FALSE(P->getProcessorIsMotion());
+ ASSERT_TRUE(P->getMotionProviderMap().empty());
// add a motion sensor and processor
auto Sm = SensorBase::emplace<SensorOdom3d>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3d());
@@ -73,7 +94,7 @@ TEST(Problem, Processor)
auto Pm = ProcessorBase::emplace<ProcessorOdom3d>(Sm, std::make_shared<ParamsProcessorOdom3d>());
// check motion processor IS NOT by emplace
- ASSERT_EQ(P->getProcessorIsMotion(), Pm);
+ ASSERT_EQ(P->getMotionProviderMap().begin()->second, Pm);
}
TEST(Problem, Installers)
@@ -88,16 +109,16 @@ TEST(Problem, Installers)
auto pt = P->installProcessor("ProcessorTrackerFeatureDummy", "dummy", "odometer");
// check motion processor IS NOT set
- ASSERT_FALSE(P->getProcessorIsMotion());
+ ASSERT_TRUE(P->getMotionProviderMap().empty());
// install processor motion
ProcessorBasePtr pm = P->installProcessor("ProcessorOdom3d", "odom integrator", "odometer", wolf_root + "/test/yaml/processor_odom_3d.yaml");
// check motion processor is set
- ASSERT_TRUE(P->getProcessorIsMotion() != nullptr);
+ ASSERT_FALSE(P->getMotionProviderMap().empty());
// check motion processor is correct
- ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getProcessorIsMotion()), pm);
+ ASSERT_EQ(std::dynamic_pointer_cast<ProcessorMotion>(P->getMotionProviderMap().begin()->second), pm);
}
TEST(Problem, SetOrigin_PO_2d)
@@ -109,7 +130,7 @@ TEST(Problem, SetOrigin_PO_2d)
// Eigen::MatrixXd P0(Eigen::MatrixXd::Identity(3,3) * 0.1); // P0 is 0.1*Id
VectorComposite s0(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- P->setPriorFactor(x0, s0, t0, 1.0);
+ P->setPriorFactor(x0, s0, t0);
WOLF_INFO("printing.-..");
P->print(4,1,1,1);
@@ -168,7 +189,7 @@ TEST(Problem, SetOrigin_PO_3d)
Eigen::VectorXd vec6(6); vec6 << sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1), sqrt(0.1);
VectorComposite s0(vec6, "PO", {3,3});
- P->setPriorFactor(x0, s0, t0, 1.0);
+ P->setPriorFactor(x0, s0, t0);
// check that no sensor has been added
ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
index ec7700ae21723253d5c5931d84ff4c151f8644cb..7e1e8624ec6c9d10711875ca85fe256adcdf2ba6 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.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--------
/**
* \file gtest_factor_pose_with_extrinsics.cpp
*
@@ -175,9 +196,9 @@ class FactorPose3dWithExtrinsics_ProcessorWithKeyFrameCallbackFirst_Test : publi
KF1_ = problem_->emplaceFrame(1, pose1_);
KF2_ = problem_->emplaceFrame(2, pose2_);
KF3_ = problem_->emplaceFrame(3, pose3_);
- problem_->keyFrameCallback(KF1_, nullptr, 0.5);
- problem_->keyFrameCallback(KF2_, nullptr, 0.5);
- problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF1_, nullptr);
+ problem_->keyFrameCallback(KF2_, nullptr);
+ problem_->keyFrameCallback(KF3_, nullptr);
///////////////////
// Create factor graph
@@ -238,9 +259,9 @@ class FactorPose3dWithExtrinsics_ProcessorWithProcessFirst_Test : public FactorP
cap_mocap3->process();
// keyframe callback called after all mocap captures have been processed
- problem_->keyFrameCallback(KF1_, nullptr, 0.5);
- problem_->keyFrameCallback(KF2_, nullptr, 0.5);
- problem_->keyFrameCallback(KF3_, nullptr, 0.5);
+ problem_->keyFrameCallback(KF1_, nullptr);
+ problem_->keyFrameCallback(KF2_, nullptr);
+ problem_->keyFrameCallback(KF3_, nullptr);
}
void TearDown() override{};
@@ -323,4 +344,4 @@ int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
-}
\ No newline at end of file
+}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 4497954899991b26100d23af38130e8218ed4a14..89341815653b03ed0d189b7d91c8e1c759f9bd9f 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.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--------
/*
* gtest_capture_base.cpp
*
@@ -23,7 +44,7 @@
using namespace wolf;
using namespace Eigen;
-TEST(ProcessorBase, IsMotion)
+TEST(ProcessorBase, MotionProvider)
{
using namespace wolf;
using std::shared_ptr;
@@ -59,8 +80,8 @@ TEST(ProcessorBase, IsMotion)
sens_odo,
proc_odo_params);
- ASSERT_FALSE(proc_trk->isMotion());
- ASSERT_TRUE (proc_odo->isMotion());
+ ASSERT_FALSE(proc_trk->isMotionProvider());
+ ASSERT_TRUE (proc_odo->isMotionProvider());
}
@@ -106,7 +127,7 @@ TEST(ProcessorBase, KeyFrameCallback)
VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P = Matrix3d::Identity() * 0.1;
VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- problem->setPriorFactor(x, P, t, dt/2); // KF1
+ problem->setPriorFactor(x, P, t); // KF1
CaptureOdom2dPtr capt_odo = make_shared<CaptureOdom2d>(t, sens_odo, Vector2d(0.5,0));
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index fc30487fe96765f36caea276defddc7abee9a6ac..0fcf734675b5b4f3f4fa5d3a46e0b6f5d5fa0cc0 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.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--------
/**
* \file gtest_processor_diff_drive.cpp
*
@@ -314,7 +335,7 @@ TEST_F(ProcessorDiffDriveTest, process)
// Matrix3d P; P.setIdentity();
VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t);
processor->setOrigin(F0);
// 1. left turn 90 deg in N steps of 90/N deg --> ends up in (1.5, 1.5, pi/2)
@@ -345,7 +366,7 @@ TEST_F(ProcessorDiffDriveTest, linear)
// Matrix3d P; P.setIdentity();
VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t);
processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
@@ -375,7 +396,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
// Matrix3d P; P.setIdentity();
VectorComposite P(Vector3d(1,1,1), "PO", {2,1});
- auto F0 = problem->setPriorFactor(x, P, t, 0.1);
+ auto F0 = problem->setPriorFactor(x, P, t);
processor->setOrigin(F0);
// Straight one turn of the wheels, in one go
diff --git a/test/gtest_processor_fix_wing_model.cpp b/test/gtest_processor_fixed_wing_model.cpp
similarity index 72%
rename from test/gtest_processor_fix_wing_model.cpp
rename to test/gtest_processor_fixed_wing_model.cpp
index e817a9c400c53cdd569c72de90c672ac703fdaf4..b2bcb1990ccd266756c7f106478b79aadff3cc88 100644
--- a/test/gtest_processor_fix_wing_model.cpp
+++ b/test/gtest_processor_fixed_wing_model.cpp
@@ -1,13 +1,34 @@
+//--------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"
#include "core/problem/problem.h"
#include "core/ceres_wrapper/solver_ceres.h"
-#include "core/processor/processor_fix_wing_model.h"
#include "core/state_block/state_quaternion.h"
// STL
#include <iterator>
#include <iostream>
+#include "../include/core/processor/processor_fixed_wing_model.h"
using namespace wolf;
using namespace Eigen;
@@ -37,11 +58,11 @@ class ProcessorFixWingModelTest : public testing::Test
2);
// Emplace processor
- ParamsProcessorFixWingModelPtr params = std::make_shared<ParamsProcessorFixWingModel>();
+ ParamsProcessorFixedWingModelPtr params = std::make_shared<ParamsProcessorFixedWingModel>();
params->velocity_local = (Vector3d() << 1, 0, 0).finished();
params->angle_stdev = 0.1;
params->min_vel_norm = 0;
- processor = ProcessorBase::emplace<ProcessorFixWingModel>(sensor, params);
+ processor = ProcessorBase::emplace<ProcessorFixedWingModel>(sensor, params);
}
FrameBasePtr emplaceFrame(TimeStamp ts, const Vector10d& x)
@@ -62,7 +83,7 @@ TEST_F(ProcessorFixWingModelTest, keyFrameCallback)
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// check one capture
ASSERT_EQ(frm1->getCapturesOf(sensor).size(), 1);
@@ -85,10 +106,10 @@ TEST_F(ProcessorFixWingModelTest, keyFrameCallbackRepeated)
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// repeated keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// check one capture
ASSERT_EQ(frm1->getCapturesOf(sensor).size(), 1);
@@ -111,7 +132,7 @@ TEST_F(ProcessorFixWingModelTest, solve_origin)
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// perturb
frm1->getP()->fix();
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
index aec8092de22eaee6a4577b43d8b7a10f8528582b..9623ecbaa1dd1560650c8cb9e96c044c87d18e9b 100644
--- a/test/gtest_processor_loop_closure.cpp
+++ b/test/gtest_processor_loop_closure.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"
#include "core/problem/problem.h"
@@ -74,7 +95,7 @@ TEST_F(ProcessorLoopClosureTest, frame_stored)
auto frm1 = emplaceFrame(1, Vector3d::Zero());
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_EQ(processor->getNStoredFrames(), 1);
EXPECT_EQ(processor->getNStoredCaptures(), 0);
@@ -115,7 +136,7 @@ TEST_F(ProcessorLoopClosureTest, captureCallbackCase2)
auto cap1 = createCapture(1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// captureCallback
processor->captureCallback(cap1);
@@ -134,7 +155,7 @@ TEST_F(ProcessorLoopClosureTest, captureCallbackCase3)
auto cap1 = createCapture(2);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
// captureCallback
processor->captureCallback(cap1);
@@ -152,7 +173,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase1)
auto cap1 = emplaceCapture(frm1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
EXPECT_EQ(processor->getNStoredFrames(), 0);
@@ -170,7 +191,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase2)
processor->captureCallback(cap1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
@@ -189,7 +210,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase3)
processor->captureCallback(cap1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_TRUE(cap1->getFrame() == nullptr);
EXPECT_EQ(cap1->getFeatureList().size(), 0);
@@ -208,7 +229,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase4)
processor->captureCallback(cap1);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
EXPECT_TRUE(cap1->getFrame() == nullptr);
EXPECT_EQ(cap1->getFeatureList().size(), 0);
@@ -228,11 +249,11 @@ TEST_F(ProcessorLoopClosureTest, captureCallbackMatch)
auto cap4 = createCapture(4);
// keyframecallback
- problem->keyFrameCallback(frm1, nullptr, 0.5);
- problem->keyFrameCallback(frm2, nullptr, 0.5);
- problem->keyFrameCallback(frm3, nullptr, 0.5);
- problem->keyFrameCallback(frm4, nullptr, 0.5);
- problem->keyFrameCallback(frm5, nullptr, 0.5);
+ problem->keyFrameCallback(frm1, nullptr);
+ problem->keyFrameCallback(frm2, nullptr);
+ problem->keyFrameCallback(frm3, nullptr);
+ problem->keyFrameCallback(frm4, nullptr);
+ problem->keyFrameCallback(frm5, nullptr);
// captureCallback
processor->captureCallback(cap4);
@@ -269,7 +290,7 @@ TEST_F(ProcessorLoopClosureTest, keyFrameCallbackMatch)
processor->captureCallback(cap5);
// keyframecallback
- problem->keyFrameCallback(frm2, nullptr, 0.5);
+ problem->keyFrameCallback(frm2, nullptr);
EXPECT_TRUE(cap1->getFrame() == nullptr);
EXPECT_TRUE(cap2->getFrame() == frm2);
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 92bf678d4ba06af85c6c0da5724f9b7f4908f186..a0d840d113f4d145ff4f0f323eb1a81642b9b01f 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.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--------
/*
* gtest_processor_motion.cpp
*
@@ -28,12 +49,10 @@ class ProcessorOdom2dPublic : public ProcessorOdom2d
void splitBuffer(const wolf::CaptureMotionPtr& capture_source,
TimeStamp ts_split,
- const FrameBasePtr& keyframe_target,
const wolf::CaptureMotionPtr& capture_target)
{
ProcessorOdom2d::splitBuffer(capture_source,
ts_split,
- keyframe_target,
capture_target);
}
};
@@ -166,7 +185,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFactorPrior)
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P0; P0.setIdentity();
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ auto KF_0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(KF_0);
data << 1, 0; // advance straight
@@ -193,7 +212,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraightFixPrior)
// Matrix3d P0; P0.setIdentity();
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ auto KF_0 = problem->setPriorFix(x0, t);
processor->setOrigin(KF_0);
data << 1, 0; // advance straight
@@ -243,7 +262,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFactorPrior)
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P0; P0.setIdentity();
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ auto KF_0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -270,7 +289,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircleFixPrior)
// Matrix3d P0; P0.setIdentity();
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ auto KF_0 = problem->setPriorFix(x0, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -326,7 +345,6 @@ TEST_F(ProcessorMotion_test, mergeCaptures)
processor->splitBuffer(C_source,
t_target,
- F_target,
C_target);
C_target->getBuffer().print(1,1,1,0);
@@ -400,7 +418,6 @@ TEST_F(ProcessorMotion_test, splitBufferAutoPrior)
processor->splitBuffer(C_source,
t_target,
- F_target,
C_target);
C_target->getBuffer().print(1,1,1,0);
@@ -415,7 +432,7 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
// Matrix3d P0; P0.setIdentity();
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFactor(x0, s0, t, 0.01);
+ auto KF_0 = problem->setPriorFactor(x0, s0, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -445,7 +462,6 @@ TEST_F(ProcessorMotion_test, splitBufferFactorPrior)
processor->splitBuffer(C_source,
t_target,
- F_target,
C_target);
C_target->getBuffer().print(1,1,1,0);
@@ -460,7 +476,7 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
// Matrix3d P0; P0.setIdentity();
VectorComposite x0(Vector3d(0,0,0), "PO", {2,1});
VectorComposite s0(Vector3d(1,1,1), "PO", {2,1});
- auto KF_0 = problem->setPriorFix(x0, t, 0.01);
+ auto KF_0 = problem->setPriorFix(x0, t);
processor->setOrigin(KF_0);
data << 1, 2*M_PI/10; // advance in circle
@@ -490,7 +506,6 @@ TEST_F(ProcessorMotion_test, splitBufferFixPrior)
processor->splitBuffer(C_source,
t_target,
- F_target,
C_target);
C_target->getBuffer().print(1,1,1,0);
diff --git a/test/gtest_processor_odom_3d.cpp b/test/gtest_processor_odom_3d.cpp
index 876a4450d65e0da3231f1f9b91b9e56e3b97ac69..a5e0beabb5447222a27bd02138144997d3389a00 100644
--- a/test/gtest_processor_odom_3d.cpp
+++ b/test/gtest_processor_odom_3d.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--------
/**
* \file gtest_odom_3d.cpp
*
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 49593d64afa531c28a8a490de0ba562ed1c13bb2..7a142212ad896a2fd2b6d4a930ce9b0b3e475d13 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.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--------
// wolf includes
#include "core/utils/utils_gtest.h"
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index d0fe1b1d8a748df603db674cf7604720c69e4d71..8017ad6049b31efec2f1efa5d5126c542429ab0a 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.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--------
// wolf includes
#include "core/utils/utils_gtest.h"
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index a8a0efd094462347f379bf4507d8dfa38e591038..bdc5e696b1e4a012abdee9fc93b16bd647b85172 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.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--------
/**
* \file gtest_rotation.cpp
*
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 3472a400fcbd571f6c7ab1232bbe09e1319d0e13..ac13388e27fbbef04fb9d1d9ab50e8e1c888b2c0 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.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--------
/**
* \file gtest_sensor_base.cpp
*
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index e2936a8ddec8c06a6fc28df0f8c7dc5ed1c90f97..a28d52df1b18090bb903c564b4daff1839e4036e 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.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--------
/**
* \file gtest_sensor_diff_drive.cpp
*
diff --git a/test/gtest_sensor_pose.cpp b/test/gtest_sensor_pose.cpp
index 64d6b73a1a77ddf4a1ee7a85f72164d4174c8ff6..4294d2ab663a7d7e171c3b663a6cf2575c6c5049 100644
--- a/test/gtest_sensor_pose.cpp
+++ b/test/gtest_sensor_pose.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--------
/**
* \file gtest_sensor_pose.cpp
*
diff --git a/test/gtest_shared_from_this.cpp b/test/gtest_shared_from_this.cpp
index 9506f844b32ddcf5d986d4a8067d77ad23030f71..f8521e4f921264a3746c11b5592d670ffe2026e5 100644
--- a/test/gtest_shared_from_this.cpp
+++ b/test/gtest_shared_from_this.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"
#include "core/common/node_base.h"
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index e5efa2dff18a27131a13cb173e064edad2d0a3c2..89bf93042391ae90c0f12560922795029ff6d704 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.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--------
/*
* gtest_solver_ptr.cpp
*
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
index 3cd41dde30bc72ecc16fc27d70b8cde5fe327a7b..c0004ad3992e256e7a3e5d883ebaea1707295775 100644
--- a/test/gtest_solver_ceres_multithread.cpp
+++ b/test/gtest_solver_ceres_multithread.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--------
/*
* gtest_solver_ptr.cpp
*
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 2eb3f46e927270a7e61abb03734d01898722f471..e66342ba48d3ba7e2c6248e249e364540c1c9537 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.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--------
/*
* gtest_solver_manager.cpp
*
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index 4e772c1bb74747d80a6ceda04d62a679f323a4e3..8c21ca410b7b9111f11994ebad9f74cfc91cfdba 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.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--------
/*
* gtest_solver_manager.cpp
*
diff --git a/test/gtest_state_block.cpp b/test/gtest_state_block.cpp
index dbcc4b6745e1c4ed99c1b1843c04fcbd7c1b546d..9bce46a4856e123124e3b62aed99c4e2a09a7b01 100644
--- a/test/gtest_state_block.cpp
+++ b/test/gtest_state_block.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--------
/*
* gtest_state_block.cpp
*
diff --git a/test/gtest_state_composite.cpp b/test/gtest_state_composite.cpp
index f4bcea67bd4e51af793e026f1496f2591fcfe98a..74dfe643ab18b1658a774fa782fe22e0a9284a66 100644
--- a/test/gtest_state_composite.cpp
+++ b/test/gtest_state_composite.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--------
/*
* gtest_state_composite.cpp
*
diff --git a/test/gtest_time_stamp.cpp b/test/gtest_time_stamp.cpp
index f8f70272b239a06f30ce5fdf6eed32308ae95689..9a328135093606fb099436f2e60f9c24e1effb0d 100644
--- a/test/gtest_time_stamp.cpp
+++ b/test/gtest_time_stamp.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"
#include "core/common/time_stamp.h"
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index a14c436ddc0864f7b55e1ef9db1fd451ecf494ea..130a3495c8469a0b4dbcecb8ebbc68b55412835c 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.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--------
/**
* \file gtest_track_matrix.cpp
*
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 7c483113468573388e5cca87ebf1bf243f41a911..12b8d2a1a637260a8c7bb47f34fdc682a8a6e1eb 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.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--------
/*
* gtest_trajectory.cpp
*
diff --git a/test/gtest_tree_manager.cpp b/test/gtest_tree_manager.cpp
index b8b86469e6e68843dc650feb5bea1e38c9db96ce..914b8a7c3a989c52f86abbb2b8274732acf3aa2f 100644
--- a/test/gtest_tree_manager.cpp
+++ b/test/gtest_tree_manager.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"
#include "core/problem/problem.h"
@@ -94,7 +115,7 @@ TEST(TreeManager, keyFrameCallback)
ASSERT_EQ(GM->n_KF_, 0);
auto F0 = P->emplaceFrame(0, "PO", 3, VectorXd(7) );
- P->keyFrameCallback(F0, nullptr, 0);
+ P->keyFrameCallback(F0, nullptr);
ASSERT_EQ(GM->n_KF_, 1);
}
diff --git a/test/gtest_tree_manager_sliding_window.cpp b/test/gtest_tree_manager_sliding_window.cpp
index 5ec1cbe4d42bb436cb2521793a074613f6ae4a3c..e4b1c92bb4f7127ee093be73e5ce621516d884c2 100644
--- a/test/gtest_tree_manager_sliding_window.cpp
+++ b/test/gtest_tree_manager_sliding_window.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"
#include "core/factor/factor_relative_pose_3d.h"
@@ -90,7 +111,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// FRAME 2 ----------------------------------------------------------
auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
- P->keyFrameCallback(F2, nullptr, 0);
+ P->keyFrameCallback(F2, nullptr);
// absolute factor
auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
@@ -109,7 +130,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// FRAME 3 ----------------------------------------------------------
auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
- P->keyFrameCallback(F3, nullptr, 0);
+ P->keyFrameCallback(F3, nullptr);
// absolute factor
auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
@@ -129,7 +150,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// FRAME 4 ----------------------------------------------------------
auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
- P->keyFrameCallback(F4, nullptr, 0);
+ P->keyFrameCallback(F4, nullptr);
// absolute factor
auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
@@ -152,7 +173,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowFixViral)
// FRAME 5 ----------------------------------------------------------
auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
- P->keyFrameCallback(F5, nullptr, 0);
+ P->keyFrameCallback(F5, nullptr);
// absolute factor
auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
@@ -199,7 +220,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// FRAME 2 ----------------------------------------------------------
auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
- P->keyFrameCallback(F2, nullptr, 0);
+ P->keyFrameCallback(F2, nullptr);
// absolute factor
auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
@@ -218,7 +239,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// FRAME 3 ----------------------------------------------------------
auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
- P->keyFrameCallback(F3, nullptr, 0);
+ P->keyFrameCallback(F3, nullptr);
// absolute factor
auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
@@ -238,7 +259,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// FRAME 4 ----------------------------------------------------------
auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
- P->keyFrameCallback(F4, nullptr, 0);
+ P->keyFrameCallback(F4, nullptr);
// absolute factor
auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
@@ -261,7 +282,7 @@ TEST(TreeManagerSlidingWindow, slidingWindowNoFixNoViral)
// FRAME 5 ----------------------------------------------------------
auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
- P->keyFrameCallback(F5, nullptr, 0);
+ P->keyFrameCallback(F5, nullptr);
// absolute factor
auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
diff --git a/test/gtest_tree_manager_sliding_window_dual_rate.cpp b/test/gtest_tree_manager_sliding_window_dual_rate.cpp
index 65092ed3b9198ff7e6304750ee0c3e06233cf5a5..e394a7c59279bb4d0e52c94b7eef479406ce5797 100644
--- a/test/gtest_tree_manager_sliding_window_dual_rate.cpp
+++ b/test/gtest_tree_manager_sliding_window_dual_rate.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"
#include "core/factor/factor_relative_pose_3d.h"
@@ -120,7 +141,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
- P->keyFrameCallback(F2, nullptr, 0);
+ P->keyFrameCallback(F2, nullptr);
// absolute factor
auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
@@ -155,7 +176,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
- P->keyFrameCallback(F3, nullptr, 0);
+ P->keyFrameCallback(F3, nullptr);
// absolute factor
auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
@@ -200,7 +221,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
- P->keyFrameCallback(F4, nullptr, 0);
+ P->keyFrameCallback(F4, nullptr);
// absolute factor
auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
@@ -254,7 +275,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
- P->keyFrameCallback(F5, nullptr, 0);
+ P->keyFrameCallback(F5, nullptr);
// absolute factor
auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
@@ -316,7 +337,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
- P->keyFrameCallback(F6, nullptr, 0);
+ P->keyFrameCallback(F6, nullptr);
// absolute factor
auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
@@ -387,7 +408,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
- P->keyFrameCallback(F7, nullptr, 0);
+ P->keyFrameCallback(F7, nullptr);
// absolute factor
auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
@@ -466,7 +487,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowFixViral)
* fix fix
*/
auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
- P->keyFrameCallback(F8, nullptr, 0);
+ P->keyFrameCallback(F8, nullptr);
// absolute factor
auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
@@ -594,7 +615,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* ( ) ( ) ( )(F1)(F2)
*/
auto F2 = P->emplaceFrame(TimeStamp(2), "PO", 3, state);
- P->keyFrameCallback(F2, nullptr, 0);
+ P->keyFrameCallback(F2, nullptr);
// absolute factor
auto C2 = CaptureBase::emplace<CaptureVoid>(F2, TimeStamp(2), nullptr);
@@ -628,7 +649,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* ( ) ( ) (F1)(F2)(F3)
*/
auto F3 = P->emplaceFrame(TimeStamp(3), "PO", 3, state);
- P->keyFrameCallback(F3, nullptr, 0);
+ P->keyFrameCallback(F3, nullptr);
// absolute factor
auto C3 = CaptureBase::emplace<CaptureVoid>(F3, TimeStamp(3), nullptr);
@@ -671,7 +692,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* ( ) (F1)(F2)(F3)(F4)
*/
auto F4 = P->emplaceFrame(TimeStamp(4), "PO", 3, state);
- P->keyFrameCallback(F4, nullptr, 0);
+ P->keyFrameCallback(F4, nullptr);
// absolute factor
auto C4 = CaptureBase::emplace<CaptureVoid>(F4, TimeStamp(4), nullptr);
@@ -723,7 +744,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* ( ) (F1) (F3)(F4)(F5)
*/
auto F5 = P->emplaceFrame(TimeStamp(5), "PO", 3, state);
- P->keyFrameCallback(F5, nullptr, 0);
+ P->keyFrameCallback(F5, nullptr);
// absolute factor
auto C5 = CaptureBase::emplace<CaptureVoid>(F5, TimeStamp(5), nullptr);
@@ -783,7 +804,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* (F1) (F3)(F4)(F5)(F6)
*/
auto F6 = P->emplaceFrame(TimeStamp(6), "PO", 3, state);
- P->keyFrameCallback(F6, nullptr, 0);
+ P->keyFrameCallback(F6, nullptr);
// absolute factor
auto C6 = CaptureBase::emplace<CaptureVoid>(F6, TimeStamp(6), nullptr);
@@ -852,7 +873,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* (F1) (F3) (F5)(F6)(F7)
*/
auto F7 = P->emplaceFrame(TimeStamp(7), "PO", 3, state);
- P->keyFrameCallback(F7, nullptr, 0);
+ P->keyFrameCallback(F7, nullptr);
// absolute factor
auto C7 = CaptureBase::emplace<CaptureVoid>(F7, TimeStamp(7), nullptr);
@@ -929,7 +950,7 @@ TEST(TreeManagerSlidingWindowDualRate, slidingWindowNoFixNoViral)
* (F3) (F5)(F6)(F7)(F8)
*/
auto F8 = P->emplaceFrame(TimeStamp(8), "PO", 3, state);
- P->keyFrameCallback(F8, nullptr, 0);
+ P->keyFrameCallback(F8, nullptr);
// absolute factor
auto C8 = CaptureBase::emplace<CaptureVoid>(F8, TimeStamp(8), nullptr);
diff --git a/test/gtest_yaml_conversions.cpp b/test/gtest_yaml_conversions.cpp
index 04acb34ee69f1da8f368966ddef0acaa6c93a7c8..21a4f89781b017807033f8c33d59dff716ec68ac 100644
--- a/test/gtest_yaml_conversions.cpp
+++ b/test/gtest_yaml_conversions.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--------
/**
* \file test_yaml_conversions.cpp
*
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
index 11b3c1c7b207cf96382cac09c70fbbbcaf843b66..81744792993a9cb21e3cd7be51b3870a3cf59dd2 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate3.yaml
@@ -1,10 +1,6 @@
config:
solver:
- period: 1
- verbose: 2
- update_immediately: false
- max_num_iterations: 10
- n_threads: 2
+ follow: "test/yaml/solver.yaml"
problem:
frame_structure: "PO"
dimension: 3
diff --git a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
index 7716b669a95101f5863597dbb33a252f8e7cd71b..099c1d228b7bbba68277b3d2dab8289f96c94429 100644
--- a/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
+++ b/test/yaml/params_tree_manager_sliding_window_dual_rate_baseline.yaml
@@ -1,9 +1,6 @@
config:
solver:
- period: 1
- verbose: 2
- update_immediately: false
- max_num_iterations: 10
+ follow: "test/yaml/solver.yaml"
problem:
frame_structure: "PO"
dimension: 3
diff --git a/test/yaml/solver.yaml b/test/yaml/solver.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..53a698323208a61dc63a35084985c13fdcebdd98
--- /dev/null
+++ b/test/yaml/solver.yaml
@@ -0,0 +1,13 @@
+period: 1
+verbose: 2
+interrupt_on_problem_change: false
+max_num_iterations: 1000
+n_threads: 2
+function_tolerance: 0.000001
+gradient_tolerance: 0.0000000001
+minimizer: "LEVENBERG_MARQUARDT"
+use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG
+max_consecutive_nonmonotonic_steps: 5 # only if use_nonmonotonic_steps = true
+compute_cov: true
+cov_period: 1 #only if compute_cov
+cov_enum: 2 #only if compute_cov
\ No newline at end of file
diff --git a/wolf_scripts/license_header_2022.txt b/wolf_scripts/license_header_2022.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0c987025f9dba3e7af993051b9bdf4b5ff400e0d
--- /dev/null
+++ b/wolf_scripts/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/wolf_scripts/license_manager.sh b/wolf_scripts/license_manager.sh
new file mode 100755
index 0000000000000000000000000000000000000000..2e90fb0ad205c6a94908aecb873fd83ebe8804c6
--- /dev/null
+++ b/wolf_scripts/license_manager.sh
@@ -0,0 +1,145 @@
+#!/bin/bash
+
+# ------ WOLF license_manager script ------
+#
+# This script is used for managing the license headers of code files (.h, .c, .cpp, .hpp)
+# This file is automatically called by the CI in gitlab.
+
+line_start_mark="//--------LICENSE_START--------"
+line_end_mark="//--------LICENSE_END--------"
+
+#options
+tmp=false
+mode="none"
+path=""
+exclude_folder=""
+#recursive=true
+license=""
+
+for i in "$@"; do
+ case $i in
+ --temp)
+ tmp=true
+ shift # past argument=value
+ ;;
+ --path=*)
+ path="${i#*=}"
+ shift # past argument=value
+ ;;
+ --license-header=*)
+ license="${i#*=}"
+ shift # past argument=value
+ ;;
+ --add)
+ mode="add"
+ if [ $mode == "update" ]; then
+ echo "Error: Script cannot be called with both options: --update or --add"
+ exit 1
+ fi
+ shift # past argument=value
+ ;;
+ --update)
+ mode="update"
+ if [ $mode == "add" ]; then
+ echo "Error: Script cannot be called with both options: --update or --add"
+ exit 1
+ fi
+ shift # past argument=value
+ ;;
+ --exclude=*)
+ exclude_folder="${i#*=}"
+ shift # past argument=value
+ ;;
+ *)
+ # unknown option
+ ;;
+ esac
+done
+
+# check options
+if [ "$path" == "" ]; then
+ echo 'Please, provide the path to the folder containing the code with --path=/my/folder/code'
+ exit 1
+else
+ if [ -d "$path" ]; then
+ echo "Valid path: ${path}"
+ else
+ echo "Error: ${path} not found. Can not continue."
+ exit 1
+ fi
+fi
+
+if [ "$license" == "" ]; then
+ echo 'Error: Please, provide the path to the folder containing the code with --license-header=/my/license/header/file.txt'
+ exit 1
+else
+ if [ -f "$license" ]; then
+ echo "Valid license header file: ${license} containing:"
+ cat ${license}
+ else
+ echo "Error: License header file ${license} not found. Can not continue."
+ exit 1
+ fi
+fi
+
+if [ "$exclude_folder" == "" ]; then
+ echo "No folders will be excluded"
+else
+ if [ -d "${path}/${exclude_folder}" ]; then
+ echo "Valid remove folder path: ${path}/${exclude_folder}"
+ fi
+fi
+
+if [ $mode == "none" ]; then
+ echo "Error: Script should be called with one of the following options: --update or --add"
+ exit 1
+else
+ echo "mode: ${mode}"
+fi
+
+# PATH (AND tmp FOLDER)
+folder=$path
+if [ $tmp == true ]; then
+ echo "Creating temporary folder: ${path}_tmp"
+ mkdir -pv ${path}_tmp
+ cp -a $path/. ${path}_tmp
+ folder=${path}_tmp
+fi
+
+# LIST OF FILES TO BE ITERATED ON
+if [ "$exclude_folder" == "" ]; then
+ file_list=$(find $folder -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
+else
+ file_list=$(find $folder -path ${folder}/${exclude_folder} -prune -name '*.c' -or -name '*.cpp' -or -name '*.h' -or -name '*.hpp')
+fi
+
+# DETECT AND REMOVE EXISTING LICENSE
+if [ "$mode" == "update" ]; then
+ echo "Recursely removing license header from all files (.c, .cpp, .h, .hpp):"
+ for i in $file_list
+ do
+ if grep -Fxq ${line_start_mark} $i
+ then
+ echo " - ${i}"
+ line_start="$(grep -wn $line_start_mark ${i} | head -n 1 | cut -d: -f1)"
+ line_end="$(grep -wn $line_end_mark ${i} | head -n 1 | cut -d: -f1)"
+ #echo ${line_start}
+ #echo ${line_end}
+ awk -v m=$line_start -v n=$line_end 'm <= NR && NR <= n {next} {print}' $i > tmpfile && mv tmpfile $i
+ #cat $i
+ fi
+ done
+fi
+
+# ADD CONTENT OF license-file AT THE BEGINNING OF CODE FILES
+echo "Recursively adding license header to all files (.c, .cpp, .h, .hpp):"
+for i in $file_list
+do
+ if grep -Fxq ${line_start_mark} $i; then
+ :
+ else
+ echo " - ${i}"
+ ( echo ${line_start_mark}$'\n//'; cat ${license}; echo $'//\n'${line_end_mark}; cat $i ) > temp_file
+ mv temp_file $i
+ fi
+done
\ No newline at end of file
diff --git a/wolf_scripts/templates/class_template.cpp b/wolf_scripts/templates/class_template.cpp
index d33d26f0ed62425a580339c63760551db8f8920d..035249b8f8f967a469d2b79dc586a915bf9da342 100644
--- a/wolf_scripts/templates/class_template.cpp
+++ b/wolf_scripts/templates/class_template.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 "header_file"
namespace wolf {
diff --git a/wolf_scripts/templates/class_template.h b/wolf_scripts/templates/class_template.h
index 79158895d26a8cb3b02ccb3694c9687b595ae125..a86b1d70ced441fbe57d4c6c16d1cf3bde95b7c8 100644
--- a/wolf_scripts/templates/class_template.h
+++ b/wolf_scripts/templates/class_template.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 _name_cap_H_
#define _name_cap_H_
diff --git a/wolf_scripts/templates/gtest_template.cpp b/wolf_scripts/templates/gtest_template.cpp
index 2acdaa25f334608ea1e9b9ce2cdec7e4642f6b84..9fc9750abb050ebff9a41d10fc62f411aeef3a6e 100644
--- a/wolf_scripts/templates/gtest_template.cpp
+++ b/wolf_scripts/templates/gtest_template.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"
#include "wolf.h"
diff --git a/wolf_scripts/wolf_update.sh b/wolf_scripts/wolf_update.sh
index 7a0ceac5be717ff17fbe5acab186fda636de7ec4..39b0c1d156c4d5115dca76040535a276210a1bfb 100755
--- a/wolf_scripts/wolf_update.sh
+++ b/wolf_scripts/wolf_update.sh
@@ -5,7 +5,7 @@ declare -A success
cd $1
abs_path=$(pwd)
# for folder in $(cd $abs_path && ls -d *); do
-for folder in wolf IMU laser gnss vision apriltag; do
+for folder in wolf imu laser gnss vision apriltag; do
echo ""
echo "==========================================================================================================================="
echo "${CYAN} Updating & installing $folder ${NC}"