From 00816f4b190c556548f690e3a282794be328a84c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 17 Jan 2022 13:05:50 +0100
Subject: [PATCH] After 2nd RA-L submission
---
.gitlab-ci.yml | 100 +++++-
CMakeLists.txt | 285 ++++++++----------
cmake_modules/wolfcoreConfig.cmake | 7 +-
demos/demo_analytic_autodiff_factor.cpp | 21 ++
demos/demo_wolf_imported_graph.cpp | 21 ++
demos/hello_wolf/capture_range_bearing.cpp | 21 ++
demos/hello_wolf/capture_range_bearing.h | 21 ++
demos/hello_wolf/factor_bearing.h | 21 ++
demos/hello_wolf/factor_range_bearing.h | 21 ++
demos/hello_wolf/feature_range_bearing.cpp | 21 ++
demos/hello_wolf/feature_range_bearing.h | 21 ++
demos/hello_wolf/hello_wolf.cpp | 23 +-
demos/hello_wolf/hello_wolf_autoconf.cpp | 51 +++-
demos/hello_wolf/landmark_point_2d.cpp | 21 ++
demos/hello_wolf/landmark_point_2d.h | 21 ++
demos/hello_wolf/processor_range_bearing.cpp | 48 ++-
demos/hello_wolf/processor_range_bearing.h | 25 +-
demos/hello_wolf/sensor_range_bearing.cpp | 21 ++
demos/hello_wolf/sensor_range_bearing.h | 21 ++
demos/hello_wolf/yaml/hello_wolf_config.yaml | 9 +-
demos/solver/test_SPQR.cpp | 21 ++
demos/solver/test_ccolamd.cpp | 21 ++
demos/solver/test_ccolamd_blocks.cpp | 21 ++
demos/solver/test_iQR.cpp | 21 ++
demos/solver/test_iQR_wolf.cpp | 21 ++
demos/solver/test_iQR_wolf2.cpp | 21 ++
.../test_incremental_ccolamd_blocks.cpp | 21 ++
demos/solver/test_permutations.cpp | 21 ++
include/core/capture/capture_base.h | 23 ++
include/core/capture/capture_diff_drive.h | 21 ++
include/core/capture/capture_motion.h | 21 ++
include/core/capture/capture_odom_2d.h | 21 ++
include/core/capture/capture_odom_3d.h | 21 ++
include/core/capture/capture_pose.h | 21 ++
include/core/capture/capture_void.h | 21 ++
.../ceres_wrapper/cost_function_wrapper.h | 21 ++
.../create_numeric_diff_cost_function.h | 21 ++
.../ceres_wrapper/iteration_update_callback.h | 21 ++
.../local_parametrization_wrapper.h | 21 ++
include/core/ceres_wrapper/qr_manager.h | 21 ++
include/core/ceres_wrapper/solver_ceres.h | 72 ++++-
include/core/ceres_wrapper/sparse_utils.h | 21 ++
include/core/common/factory.h | 21 ++
include/core/common/node_base.h | 21 ++
include/core/common/params_base.h | 21 ++
include/core/common/time_stamp.h | 21 ++
include/core/common/wolf.h | 21 ++
include/core/factor/factor_analytic.h | 21 ++
include/core/factor/factor_autodiff.h | 21 ++
include/core/factor/factor_base.h | 21 ++
include/core/factor/factor_block_absolute.h | 21 ++
include/core/factor/factor_block_difference.h | 21 ++
include/core/factor/factor_diff_drive.h | 21 ++
include/core/factor/factor_distance_3d.h | 25 +-
include/core/factor/factor_pose_2d.h | 21 ++
include/core/factor/factor_pose_3d.h | 21 ++
.../factor/factor_pose_3d_with_extrinsics.h | 21 ++
.../core/factor/factor_quaternion_absolute.h | 21 ++
include/core/factor/factor_relative_pose_2d.h | 21 ++
.../factor_relative_pose_2d_with_extrinsics.h | 21 ++
include/core/factor/factor_relative_pose_3d.h | 21 ++
.../factor_velocity_local_direction_3d.h | 21 ++
include/core/feature/feature_base.h | 21 ++
include/core/feature/feature_diff_drive.h | 35 ++-
include/core/feature/feature_match.h | 21 ++
include/core/feature/feature_motion.h | 21 ++
include/core/feature/feature_odom_2d.h | 21 ++
include/core/feature/feature_pose.h | 21 ++
include/core/frame/frame_base.h | 21 ++
include/core/hardware/hardware_base.h | 21 ++
include/core/landmark/landmark_base.h | 21 ++
include/core/landmark/landmark_match.h | 21 ++
include/core/map/map_base.h | 21 ++
include/core/math/SE2.h | 21 ++
include/core/math/SE3.h | 21 ++
include/core/math/covariance.h | 21 ++
include/core/math/rotations.h | 21 ++
include/core/problem/problem.h | 75 +++--
include/core/processor/factory_processor.h | 21 ++
include/core/processor/is_motion.h | 110 -------
include/core/processor/motion_buffer.h | 21 ++
include/core/processor/motion_provider.h | 131 ++++++++
include/core/processor/processor_base.h | 172 +++++------
include/core/processor/processor_diff_drive.h | 21 ++
.../core/processor/processor_fix_wing_model.h | 25 +-
include/core/processor/processor_logging.h | 21 ++
.../core/processor/processor_loop_closure.h | 25 +-
include/core/processor/processor_motion.h | 47 ++-
include/core/processor/processor_odom_2d.h | 21 ++
include/core/processor/processor_odom_3d.h | 21 ++
include/core/processor/processor_pose.h | 27 +-
include/core/processor/processor_tracker.h | 68 ++---
.../processor/processor_tracker_feature.h | 21 ++
.../processor/processor_tracker_landmark.h | 21 ++
include/core/processor/track_matrix.h | 21 ++
include/core/sensor/factory_sensor.h | 21 ++
include/core/sensor/sensor_base.h | 21 ++
include/core/sensor/sensor_diff_drive.h | 21 ++
include/core/sensor/sensor_model.h | 21 ++
include/core/sensor/sensor_odom_2d.h | 21 ++
include/core/sensor/sensor_odom_3d.h | 21 ++
include/core/sensor/sensor_pose.h | 21 ++
include/core/solver/factory_solver.h | 21 ++
include/core/solver/solver_manager.h | 47 ++-
.../solver_suitesparse/ccolamd_ordering.h | 21 ++
.../solver_suitesparse/cost_function_base.h | 21 ++
.../solver_suitesparse/cost_function_sparse.h | 21 ++
.../cost_function_sparse_base.h | 21 ++
include/core/solver_suitesparse/qr_solver.h | 21 ++
include/core/solver_suitesparse/solver_QR.h | 21 ++
.../core/solver_suitesparse/solver_manager.h | 21 ++
.../core/solver_suitesparse/sparse_utils.h | 21 ++
.../core/state_block/factory_state_block.h | 21 ++
include/core/state_block/has_state_blocks.h | 21 ++
.../state_block/local_parametrization_angle.h | 21 ++
.../state_block/local_parametrization_base.h | 21 ++
.../local_parametrization_homogeneous.h | 21 ++
.../local_parametrization_quaternion.h | 21 ++
include/core/state_block/state_angle.h | 21 ++
include/core/state_block/state_block.h | 21 ++
include/core/state_block/state_composite.h | 21 ++
.../core/state_block/state_homogeneous_3d.h | 21 ++
include/core/state_block/state_quaternion.h | 21 ++
include/core/trajectory/trajectory_base.h | 21 ++
.../core/tree_manager/factory_tree_manager.h | 21 ++
include/core/tree_manager/tree_manager_base.h | 23 +-
.../tree_manager_sliding_window.h | 21 ++
.../tree_manager_sliding_window_dual_rate.h | 21 ++
include/core/utils/check_log.h | 21 ++
include/core/utils/converter.h | 21 ++
include/core/utils/converter_utils.h | 23 +-
include/core/utils/eigen_assert.h | 21 ++
include/core/utils/eigen_predicates.h | 21 ++
include/core/utils/graph_search.h | 21 ++
include/core/utils/loader.h | 23 +-
include/core/utils/logging.h | 21 ++
include/core/utils/make_unique.h | 21 ++
include/core/utils/params_server.h | 21 ++
include/core/utils/singleton.h | 21 ++
include/core/utils/utils_gtest.h | 21 ++
include/core/yaml/parser_yaml.h | 21 ++
include/core/yaml/yaml_conversion.h | 21 ++
prova.txt | 0
src/capture/capture_base.cpp | 52 +++-
src/capture/capture_diff_drive.cpp | 21 ++
src/capture/capture_motion.cpp | 26 +-
src/capture/capture_odom_2d.cpp | 21 ++
src/capture/capture_odom_3d.cpp | 21 ++
src/capture/capture_pose.cpp | 21 ++
src/capture/capture_void.cpp | 21 ++
.../local_parametrization_wrapper.cpp | 21 ++
src/ceres_wrapper/qr_manager.cpp | 21 ++
src/ceres_wrapper/solver_ceres.cpp | 21 ++
src/common/node_base.cpp | 21 ++
src/common/time_stamp.cpp | 21 ++
src/factor/factor_analytic.cpp | 21 ++
src/factor/factor_base.cpp | 21 ++
src/feature/feature_base.cpp | 21 ++
src/feature/feature_diff_drive.cpp | 21 ++
src/feature/feature_motion.cpp | 21 ++
src/feature/feature_odom_2d.cpp | 21 ++
src/feature/feature_pose.cpp | 21 ++
src/frame/frame_base.cpp | 21 ++
src/hardware/hardware_base.cpp | 21 ++
src/landmark/landmark_base.cpp | 21 ++
src/map/map_base.cpp | 21 ++
src/problem/problem.cpp | 171 ++++++-----
src/processor/is_motion.cpp | 23 --
src/processor/motion_buffer.cpp | 21 ++
src/processor/motion_provider.cpp | 44 +++
src/processor/processor_base.cpp | 187 ++++--------
src/processor/processor_diff_drive.cpp | 27 +-
src/processor/processor_fix_wing_model.cpp | 23 +-
src/processor/processor_loop_closure.cpp | 57 ++--
src/processor/processor_motion.cpp | 159 +++++-----
src/processor/processor_odom_2d.cpp | 24 +-
src/processor/processor_odom_3d.cpp | 34 ++-
src/processor/processor_pose.cpp | 56 ++--
src/processor/processor_tracker.cpp | 175 ++++++-----
src/processor/processor_tracker_feature.cpp | 21 ++
src/processor/processor_tracker_landmark.cpp | 21 ++
src/processor/track_matrix.cpp | 21 ++
src/sensor/sensor_base.cpp | 21 ++
src/sensor/sensor_diff_drive.cpp | 21 ++
src/sensor/sensor_model.cpp | 21 ++
src/sensor/sensor_odom_2d.cpp | 21 ++
src/sensor/sensor_odom_3d.cpp | 21 ++
src/sensor/sensor_pose.cpp | 21 ++
src/solver/solver_manager.cpp | 35 +++
src/solver_suitesparse/solver_manager.cpp | 21 ++
src/state_block/has_state_blocks.cpp | 21 ++
.../local_parametrization_base.cpp | 21 ++
.../local_parametrization_homogeneous.cpp | 21 ++
.../local_parametrization_quaternion.cpp | 21 ++
src/state_block/state_block.cpp | 21 ++
src/state_block/state_composite.cpp | 21 ++
src/trajectory/trajectory_base.cpp | 21 ++
.../tree_manager_sliding_window.cpp | 21 ++
.../tree_manager_sliding_window_dual_rate.cpp | 31 +-
src/utils/check_log.cpp | 21 ++
src/utils/converter_utils.cpp | 23 +-
src/utils/graph_search.cpp | 21 ++
src/utils/loader.cpp | 23 +-
src/utils/params_server.cpp | 21 ++
src/yaml/parser_yaml.cpp | 21 ++
src/yaml/processor_odom_3d_yaml.cpp | 21 ++
src/yaml/sensor_odom_2d_yaml.cpp | 21 ++
src/yaml/sensor_odom_3d_yaml.cpp | 21 ++
src/yaml/sensor_pose_yaml.cpp | 21 ++
test/CMakeLists.txt | 12 +-
test/dummy/factor_dummy_zero_1.h | 21 ++
test/dummy/factor_dummy_zero_12.h | 21 ++
test/dummy/factor_feature_dummy.h | 21 ++
test/dummy/factor_landmark_dummy.h | 21 ++
test/dummy/factor_odom_2d_autodiff.h | 21 ++
test/dummy/processor_is_motion_dummy.h | 76 -----
test/dummy/processor_loop_closure_dummy.h | 23 +-
test/dummy/processor_motion_provider_dummy.h | 97 ++++++
.../dummy/processor_tracker_feature_dummy.cpp | 21 ++
test/dummy/processor_tracker_feature_dummy.h | 21 ++
.../processor_tracker_landmark_dummy.cpp | 21 ++
test/dummy/processor_tracker_landmark_dummy.h | 21 ++
test/dummy/solver_manager_dummy.h | 21 ++
test/dummy/tree_manager_dummy.h | 21 ++
test/gtest_SE3.cpp | 21 ++
test/gtest_buffer_frame.cpp | 266 ++++++++++++++++
test/gtest_capture_base.cpp | 21 ++
test/gtest_converter.cpp | 21 ++
test/gtest_eigen_predicates.cpp | 21 ++
test/gtest_emplace.cpp | 21 ++
test/gtest_example.cpp | 21 ++
test/gtest_factor_absolute.cpp | 21 ++
test/gtest_factor_autodiff.cpp | 21 ++
test/gtest_factor_autodiff_distance_3d.cpp | 21 ++
test/gtest_factor_base.cpp | 21 ++
test/gtest_factor_block_difference.cpp | 23 +-
test/gtest_factor_diff_drive.cpp | 25 +-
test/gtest_factor_odom_2d_autodiff.cpp | 21 ++
test/gtest_factor_pose_2d.cpp | 21 ++
test/gtest_factor_pose_3d.cpp | 21 ++
test/gtest_factor_relative_pose_2d.cpp | 21 ++
...actor_relative_pose_2d_with_extrinsics.cpp | 21 ++
test/gtest_factor_relative_pose_3d.cpp | 21 ++
...est_factor_velocity_local_direction_3d.cpp | 21 ++
test/gtest_factory_state_block.cpp | 21 ++
test/gtest_feature_base.cpp | 21 ++
test/gtest_frame_base.cpp | 21 ++
test/gtest_graph_search.cpp | 21 ++
test/gtest_has_state_blocks.cpp | 21 ++
test/gtest_local_param.cpp | 21 ++
test/gtest_logging.cpp | 21 ++
test/gtest_make_posdef.cpp | 21 ++
test/gtest_map_yaml.cpp | 21 ++
test/gtest_motion_buffer.cpp | 21 ++
...s_motion.cpp => gtest_motion_provider.cpp} | 68 +++--
test/gtest_odom_2d.cpp | 31 +-
test/gtest_pack_KF_buffer.cpp | 246 ---------------
test/gtest_param_prior.cpp | 21 ++
test/gtest_param_server.cpp | 21 ++
test/gtest_parser_yaml.cpp | 21 ++
test/gtest_problem.cpp | 35 ++-
...sor_and_factor_pose_3d_with_extrinsics.cpp | 35 ++-
test/gtest_processor_base.cpp | 29 +-
test/gtest_processor_diff_drive.cpp | 27 +-
test/gtest_processor_fix_wing_model.cpp | 29 +-
test/gtest_processor_loop_closure.cpp | 47 ++-
test/gtest_processor_motion.cpp | 39 ++-
test/gtest_processor_odom_3d.cpp | 21 ++
.../gtest_processor_tracker_feature_dummy.cpp | 21 ++
...gtest_processor_tracker_landmark_dummy.cpp | 21 ++
test/gtest_rotation.cpp | 21 ++
test/gtest_sensor_base.cpp | 21 ++
test/gtest_sensor_diff_drive.cpp | 21 ++
test/gtest_sensor_pose.cpp | 21 ++
test/gtest_shared_from_this.cpp | 21 ++
test/gtest_solver_ceres.cpp | 21 ++
test/gtest_solver_ceres_multithread.cpp | 21 ++
test/gtest_solver_manager.cpp | 21 ++
test/gtest_solver_manager_multithread.cpp | 21 ++
test/gtest_state_block.cpp | 21 ++
test/gtest_state_composite.cpp | 21 ++
test/gtest_time_stamp.cpp | 21 ++
test/gtest_track_matrix.cpp | 21 ++
test/gtest_trajectory.cpp | 21 ++
test/gtest_tree_manager.cpp | 23 +-
test/gtest_tree_manager_sliding_window.cpp | 37 ++-
..._tree_manager_sliding_window_dual_rate.cpp | 49 ++-
test/gtest_yaml_conversions.cpp | 21 ++
...ree_manager_sliding_window_dual_rate3.yaml | 6 +-
...ger_sliding_window_dual_rate_baseline.yaml | 5 +-
test/yaml/solver.yaml | 13 +
wolf_scripts/license_header_2022.txt | 17 ++
wolf_scripts/license_manager.sh | 145 +++++++++
wolf_scripts/templates/class_template.cpp | 21 ++
wolf_scripts/templates/class_template.h | 21 ++
wolf_scripts/templates/gtest_template.cpp | 21 ++
wolf_scripts/wolf_update.sh | 2 +-
297 files changed, 7428 insertions(+), 1413 deletions(-)
delete mode 100644 include/core/processor/is_motion.h
create mode 100644 include/core/processor/motion_provider.h
create mode 100644 prova.txt
delete mode 100644 src/processor/is_motion.cpp
create mode 100644 src/processor/motion_provider.cpp
delete mode 100644 test/dummy/processor_is_motion_dummy.h
create mode 100644 test/dummy/processor_motion_provider_dummy.h
create mode 100644 test/gtest_buffer_frame.cpp
rename test/{gtest_is_motion.cpp => gtest_motion_provider.cpp} (70%)
delete mode 100644 test/gtest_pack_KF_buffer.cpp
create mode 100644 test/yaml/solver.yaml
create mode 100644 wolf_scripts/license_header_2022.txt
create mode 100755 wolf_scripts/license_manager.sh
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 050de2e95..3696d52a0 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,18 +1,89 @@
+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 ..
- - make -j$WOLF_N_PROC
- - ctest -j$WOLF_N_PROC
+ - make -j$(nproc)
+ - ctest -j$(nproc)
# run demos
- ../bin/hello_wolf
- ../bin/hello_wolf_autoconf
- make install
-stages:
- - build_and_test
- - deploy
+############ 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
@@ -21,6 +92,7 @@ build_and_test:xenial:
script:
- *build_and_test_definition
+############ UBUNTU 18.04 TESTS ############
build_and_test:bionic:
stage: build_and_test
image: labrobotica/wolf_deps:18.04
@@ -29,6 +101,16 @@ build_and_test:bionic:
script:
- *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:
@@ -75,4 +157,10 @@ deploy_bodydynamics:
WOLF_CORE_BRANCH: $CI_COMMIT_BRANCH
trigger:
project: mobile_robotics/wolf_projects/wolf_lib/plugins/bodydynamics
- strategy: depend
\ No newline at end of file
+ strategy: depend
+
+############ WAIT FOR PLUGINS ############
+final_all:
+ stage: final
+ script:
+ - echo "ALL PLUGINS PIPELINES SUCCEED!!!"
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1789e6330..4a288d3fc 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,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}")
@@ -35,16 +34,12 @@ SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
#Set compiler according C++11 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)
@@ -85,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)
@@ -142,71 +122,13 @@ 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/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 +138,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,18 +164,37 @@ 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/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
@@ -275,54 +223,48 @@ 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/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_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,22 +274,39 @@ 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
@@ -372,10 +331,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 +360,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 +373,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 +400,6 @@ ADD_LIBRARY(${PLUGIN_NAME}
${SRCS_HARDWARE}
${SRCS_LANDMARK}
${SRCS_MAP}
- ${SRCS_MATH}
${SRCS_PROBLEM}
${SRCS_PROCESSOR}
${SRCS_SENSOR}
@@ -438,7 +408,6 @@ ADD_LIBRARY(${PLUGIN_NAME}
${SRCS_TRAJECTORY}
${SRCS_TREE_MANAGER}
${SRCS_UTILS}
- ${SRCS_WRAPPER}
${SRCS_YAML}
)
@@ -462,30 +431,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 +480,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 +492,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 +508,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 +515,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/cmake_modules/wolfcoreConfig.cmake b/cmake_modules/wolfcoreConfig.cmake
index b4df7e15b..5204cd493 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 0fd8d42cc..0625a61b5 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 386d80313..037ff094a 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 9ead1c183..f5756924b 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 10fb8fa05..03803089d 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 d3902daad..d06ace3a7 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 596278b2f..449568264 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 ea584a419..5ae7e031e 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 b924d29ee..24123e4df 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 2b2d03d43..5500b724a 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 edeb6b1f0..db934ca13 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 8b2ad4c0e..c2c11187e 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 79287af06..78aeafe9b 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 7df38e733..06469b0b1 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 f086da907..ffd8b9b9c 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 54507f9ac..7073082e3 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 70684f779..80f55753b 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 4be898bc1..c46580eff 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"
@@ -23,7 +23,12 @@ config:
verbose: 0
period: 0.2
update_immediately: false
- n_threads: 1
+ 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 04592dbd0..b5f159c73 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 48831f651..98f496f26 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 83cc7af84..14bb17428 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 b027c874d..53a37cef1 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 2fdc1f9f2..a8df23723 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 82aab6219..821ffefeb 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 9283f8411..740407a9a 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 c33c744c6..0765d8ae7 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 68d34c20b..d73f17d8e 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 e9797427f..a41e7dec7 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 c0d86ee10..a70a54b2a 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 e0a81bf2d..8bc18706d 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 054c4f935..0b6deb6a8 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 07db9e035..15b32f85e 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 0ac7b3b2f..1adb08e43 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 c9972fe78..9bca9394f 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 2504d0b10..42bf8a7a9 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 1717be546..6cf6179ff 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 fcffbbf80..51d10c497 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 691c44166..d859b6a65 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 4bf6f7de6..e638c0850 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_
@@ -44,31 +65,58 @@ struct ParamsCeres : public ParamsSolver
if (update_immediately)
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;
}
diff --git a/include/core/ceres_wrapper/sparse_utils.h b/include/core/ceres_wrapper/sparse_utils.h
index 4b7171f77..e0d70f26f 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 edd074487..d533ff0da 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 dcd7d3717..f450b60de 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 b9a4886c2..4760d1cdd 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 2643f13c8..c327ab2d0 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 97f9a50ff..7a7f9eeae 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 93daadcee..b2c0d0e4f 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 68c820c6f..0b8f9e111 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 e3dd39803..05508895c 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 58d0570dd..3b9e464a0 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 bdf76c063..503ace5dd 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 726771964..9606e23b1 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 61f6767a2..d4207b254 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 4357aac83..2d3f40ed2 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 139b87b65..04f7f7b17 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 ad35fb6c2..2daacba6d 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 3122e8097..5e93a1538 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 a51c9a7fc..4a608feb0 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 1d7e85804..2247a898b 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 afcf8daf6..a132c3c0a 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 115ea57f1..89d8cb126 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 43735ff44..5259e5c51 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 203e8f05f..8b756e29a 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 0ace09b3e..36b5b2ca1 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 0f5ecc2a6..535acdfe9 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 91016cbfc..e83d964e0 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 701967949..bd30ee739 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 ed36d9b50..c7ab25754 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_
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 0ef770b1c..59f423f42 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 21098d332..7984e6773 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 891173fc5..a7fbe5cc3 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 93663bdf7..13c870269 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 ffd8ac872..4ce5ab465 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 685eefaba..9eb2e5fd9 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 45cbdf3e1..acb4c0862 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 a0a86c6ee..f33c18fcb 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 3d0eba08b..8abc538d8 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,13 @@ 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;
+// MotionProviderPtr getMotionProvider();
+ std::map<int,MotionProviderPtr>& getMotionProviderMap();
+ const std::map<int,MotionProviderPtr>& getMotionProviderMap() const;
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
@@ -187,20 +207,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 +263,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 +328,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;
@@ -396,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
@@ -428,21 +443,21 @@ 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 MotionProviderPtr Problem::getMotionProvider()
+//{
+// if (not motion_provider_map_.empty())
+// return motion_provider_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 e36e85a6d..74f01419e 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 a17ac88e7..000000000
--- 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 548fcf6e1..4763d0bc4 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 000000000..9a6e84d9c
--- /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 bbb9d109f..b2e0f6fdb 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 7203ff3fd..e9cf00b45 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_fix_wing_model.h
index 8c2391735..97c8ad6f3 100644
--- a/include/core/processor/processor_fix_wing_model.h
+++ b/include/core/processor/processor_fix_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
*
@@ -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
*/
diff --git a/include/core/processor/processor_logging.h b/include/core/processor/processor_logging.h
index bc1f8399a..52eab893a 100644
--- a/include/core/processor/processor_logging.h
+++ b/include/core/processor/processor_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 processor_logging.h
*
diff --git a/include/core/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h
index 94af3fa16..b6e4d965b 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 207bacb8e..66f40e6e6 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 254eded5c..f3a50080d 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 95035e56a..83baf370b 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 0e0a68527..ca5cd69cd 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 855d30ee2..f03ed4ea4 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 86fc8f35a..f9c954bf2 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 23f0b89fc..bbccccf95 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 1d8bfb343..87511440e 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 fb75e84cd..3a77700e8 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 fc12b568c..0fdcac98b 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_
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 212a955b1..5d1f2bb0c 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
index 5e2a37df7..7070ff1c7 100644
--- a/include/core/sensor/sensor_model.h
+++ b/include/core/sensor/sensor_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--------
#ifndef SRC_SENSOR_MODEL_H_
#define SRC_SENSOR_MODEL_H_
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index 0859f15bc..a734043ce 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 d5f5b417e..2b94194e2 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 4df621546..e653fe458 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 3e13e598f..fc8e51070 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 f3fbeefb3..40575577f 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,6 +136,8 @@ 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;
@@ -140,17 +163,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;
@@ -210,6 +238,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 +248,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 b20b857bd..468337355 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 523e2147a..fb094beed 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 806e4d040..43e367f4f 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 78bfd843d..8c752208b 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 b89d49c8c..83c392894 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 e625515d2..3f0f689fe 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 449c8eee3..f3baf950f 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 7f5e0a0d0..393a2fb27 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 4e1028bf9..c440584c0 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 418e0909f..f1bcd4519 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 460835cb9..62833a8b4 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 8494ec3a8..435489cd3 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 b8bbb981c..476aceb79 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 c637ab6bb..70f4c3018 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 1a5fda068..3cc093665 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 5406fe0c1..8f4b0468b 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 1c675c7b7..e3569b5b4 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
*
diff --git a/include/core/state_block/state_homogeneous_3d.h b/include/core/state_block/state_homogeneous_3d.h
index ccc45424e..6ee8a85c2 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 d52b18c83..4200f81f7 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 431524af9..46cce552c 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 c9e91935f..ae4782088 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 9fd06f2f0..8aa2ac43d 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 16e58428d..46eedc0f2 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 f0e20f4b5..6df971b5b 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 035bd0aa0..947a53175 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 7a266d50b..1c53906c6 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 1765c3f90..36d67b012 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 7873b792c..fcd44023d 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
index 1f319f3f8..3a83601e8 100644
--- a/include/core/utils/eigen_predicates.h
+++ b/include/core/utils/eigen_predicates.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 eigen_predicates.h
* \brief Some utils for comparing Eigen types
diff --git a/include/core/utils/graph_search.h b/include/core/utils/graph_search.h
index cf14ff844..549c1f1de 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 7da10d043..13fe987d7 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 b59a96c31..b34ceccb1 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
*
diff --git a/include/core/utils/make_unique.h b/include/core/utils/make_unique.h
index f336eb4f7..0bfdbb837 100644
--- a/include/core/utils/make_unique.h
+++ b/include/core/utils/make_unique.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 make_unique.h
*
diff --git a/include/core/utils/params_server.h b/include/core/utils/params_server.h
index e8b473ca2..e5283ccd5 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 3595ea584..edbaa1b54 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 31564f92f..b89b9102a 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 74473f695..d5f948961 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 0542cba4d..76991419a 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 000000000..e69de29bb
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 546fafeec..8e91f7e75 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 7a0933647..f4f159345 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 b87e4fb10..3f925fd62 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 4755b6fe0..c7f394fdc 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 a692011ff..ba1ab7408 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 190fa7090..113ac5710 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 dcce03c9d..304e56273 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 1cb1425c0..c6623d4f5 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 0d7746f92..297b041be 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 5914b40a8..aaf0c0970 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"
diff --git a/src/common/node_base.cpp b/src/common/node_base.cpp
index 3db2a0798..af4db4552 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 0421efa9b..5ea454b0a 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 f3c300baa..bf7fb3d02 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 df80bf00b..46151eba6 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 c2af9127c..11db6b079 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 9ba01d1f1..62dcff2cc 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 d0b988b79..6d0260b07 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 a4d2571b9..c0d2083ca 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 cb5ea4c10..bcf4e2b48 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 71e344552..951d544ea 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 e6e5a15c3..9db6b0c37 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 11e26334c..09d317544 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 7f6db136f..f736ce180 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_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--------
// wolf
#include "core/map/map_base.h"
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 9c939355a..787619db0 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,7 +66,7 @@ Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>()),
map_ptr_(std::make_shared<MapBase>()),
- processor_is_motion_map_(),
+ motion_provider_map_(),
frame_structure_(_frame_structure),
prior_options_(std::make_shared<PriorOptions>())
{
@@ -92,35 +113,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);
- //
- // cout << "PRINTING SERVER MAP" << endl;
- // _server.print();
- // cout << "-----------------------------------" << endl;
+ int dim = _server.getParam<int> ("problem/dimension");
+ auto problem = Problem::create(frame_structure, dim);
+
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");
@@ -128,20 +151,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");
@@ -151,22 +176,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);
+
// Tree manager
std::string tree_manager_type = _server.getParam<std::string>("problem/tree_manager/type");
@@ -174,7 +202,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
if (tree_manager_type != "None" and tree_manager_type != "none")
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);
@@ -185,7 +213,6 @@ 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"));
@@ -194,9 +221,7 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
}
else
{
- WOLF_TRACE("Prior mode: ", prior_mode);
problem->setPriorOptions(prior_mode,
- _server.getParam<double>("problem/prior/time_tolerance"),
_server.getParam<VectorComposite>("problem/prior/state"));
}
@@ -221,8 +246,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)
{
@@ -421,7 +445,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();
@@ -448,8 +472,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);
@@ -496,8 +520,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);
@@ -544,8 +568,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();
@@ -607,39 +631,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());
}
@@ -675,9 +699,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
@@ -691,7 +715,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);
@@ -1009,7 +1033,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 )
{
@@ -1023,13 +1046,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")
{
@@ -1067,7 +1087,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")
@@ -1116,7 +1136,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;
@@ -1126,27 +1146,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);
}
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
deleted file mode 100644
index 57333a055..000000000
--- 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 52383b14c..f61c1b1f9 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 000000000..e2723db61
--- /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 28963ed64..adeac3db5 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 43f15c52d..db06d0078 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_fix_wing_model.cpp
index 7c12e90f8..4c75926cc 100644
--- a/src/processor/processor_fix_wing_model.cpp
+++ b/src/processor/processor_fix_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
*
@@ -29,7 +50,7 @@ void ProcessorFixWingModel::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 ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr)
{
if (_keyframe_ptr->getV()->isFixed())
return;
diff --git a/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
index 486be875a..2c8e16ce6 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 9b7f8da44..805f9bb4c 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 573406efe..98f55c0f0 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 e57ba32bd..bf24a1d4e 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 70d9c6c97..727a49d74 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 52421e863..c707c37ea 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 cda95e5ed..d55a2666c 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 ec16f76dc..bab09ac82 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 da6a88b75..1830270b1 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 0700d403f..7c9732d76 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"
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index afdc43e02..4620caea4 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
index 6eb00f021..292f1a05e 100644
--- a/src/sensor/sensor_model.cpp
+++ b/src/sensor/sensor_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--------
#include "core/sensor/sensor_model.h"
namespace wolf {
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index 574e4277e..711545f66 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 b261029d6..b44561ff4 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 c308bed71..87f2880b8 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 99dfb8729..1dc258b88 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"
@@ -185,6 +206,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 +525,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;
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index f94353d93..0f4d82837 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 ce743db05..bd800ab2e 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"
diff --git a/src/state_block/local_parametrization_base.cpp b/src/state_block/local_parametrization_base.cpp
index aee3f7041..93f906091 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 846b14a49..c6e90d7a5 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 727fa0de8..601a93c54 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 0280e2657..513d6d462 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 04658a0fb..9f3e729ea 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 4a64613a3..4bf0282fd 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 7d10c3167..5d6f1345e 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 35c20da45..b2ab0e2fd 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 06e58733a..830073ea7 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 8f445c572..f77008e1b 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 d2876701b..20dc65e0e 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 9fdf6984b..c11bb2667 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 634446679..ce4a79743 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 f07cc4d55..b4c830bd5 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 bd168ee20..a8dd748ea 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 a33b7e3c7..68a44ff61 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 edba0bd17..02e0ff3d8 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 4b9521595..102729bd9 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 46964ac57..a3da6a740 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})
@@ -89,8 +93,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 +108,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})
diff --git a/test/dummy/factor_dummy_zero_1.h b/test/dummy/factor_dummy_zero_1.h
index f2d174702..5a52d9c47 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 978592b61..707a99908 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 cf81f3025..c04043d85 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 5f6f21124..f4c53fa10 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 e765ed760..9d09f97ae 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 dca4808c9..000000000
--- 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 388f2f0dd..a0e456eec 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 000000000..c94013296
--- /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 8485d2670..ab58e3e0e 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 12705325a..d68a55e03 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 d36efd3f8..54525c868 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 58561c896..22060c82d 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 cf45c71c9..d718fc955 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
*
diff --git a/test/dummy/tree_manager_dummy.h b/test/dummy/tree_manager_dummy.h
index 1e7e70f2e..1f115ace8 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 a99f7a195..be07e9804 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 000000000..ebc17ca13
--- /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 3a0cbc455..aa3607810 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 d068e024a..9be8c838f 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
index 036bc2a4b..6dc7184ec 100644
--- a/test/gtest_eigen_predicates.cpp
+++ b/test/gtest_eigen_predicates.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/eigen_predicates.h"
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index f790bc57f..0aa294035 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 65d779ffd..e50f8c4fd 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 e67a2f740..2a98cc7a3 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 c0cbacdec..7a1a33091 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 477520c0f..2134768da 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 f861114e7..c8a04dcd1 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 c314c74ce..14724df23 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 a2e7783be..4f280a6ff 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 45315ddf2..fd8aebb18 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 d38ec5284..498df49f2 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 8bf5b82ed..544ed5127 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 b4c11c8bb..cc4030a9e 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 eb1f5eb4f..ee2bc076d 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 5771b1246..9369c090e 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 0834d9507..be2eb8584 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 96545d1be..33b358362 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 bbb978af9..1aa20b956 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 b37ce9708..26c9b3cc0 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 9693846e7..43d4b5724 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 71e271a92..e60bc8753 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 8b6ed4fc1..a709195a5 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 58480003d..d67ab498b 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 ce406cd1a..92d97edcb 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 dd3f75031..5c4b2b789 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 f47e9bd12..108147b82 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 0c8b8fa62..982bc05a3 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 3aad59381..225a49326 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 d1fce41e5..000000000
--- 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 89670c622..194671976 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 bf427f782..e8116c8ff 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 df50e8f75..bb37da59d 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 6d0e7014d..d5fa0054e 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 ec7700ae2..7e1e8624e 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 449795489..893418156 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 fc30487fe..0fcf73467 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_fix_wing_model.cpp
index e817a9c40..41eaed71f 100644
--- a/test/gtest_processor_fix_wing_model.cpp
+++ b/test/gtest_processor_fix_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--------
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
@@ -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 aec8092de..9623ecbaa 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 92bf678d4..a0d840d11 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 876a4450d..a5e0beabb 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 49593d64a..7a142212a 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 d0fe1b1d8..8017ad604 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 a8a0efd09..bdc5e696b 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 3472a400f..ac13388e2 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 e2936a8dd..a28d52df1 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 64d6b73a1..4294d2ab6 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 9506f844b..f8521e4f9 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 e5efa2dff..89bf93042 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 3cd41dde3..c0004ad39 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 2eb3f46e9..e66342ba4 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 4e772c1bb..8c21ca410 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 dbcc4b674..9bce46a48 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 f4bcea67b..74dfe643a 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 f8f70272b..9a3281350 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 a14c436dd..130a3495c 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 7c4831134..12b8d2a1a 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 b8b86469e..914b8a7c3 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 5ec1cbe4d..e4b1c92bb 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 65092ed3b..e394a7c59 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 04acb34ee..21a4f8978 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 8f00f6499..0b030b80f 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 114e865bd..d00b201f9 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 000000000..ec6a06a9e
--- /dev/null
+++ b/test/yaml/solver.yaml
@@ -0,0 +1,13 @@
+period: 1
+verbose: 2
+update_immediately: 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 000000000..0c987025f
--- /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 000000000..2e90fb0ad
--- /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 d33d26f0e..035249b8f 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 79158895d..a86b1d70c 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 2acdaa25f..9fc9750ab 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 7a0ceac5b..39b0c1d15 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}"
--
GitLab