diff --git a/.gitignore b/.gitignore index c237d0402877332e46b0dbaeb3a4a12c6283bfe7..5861b1e56adb7cc3c87bb3c10d66c1736ea8885c 100644 --- a/.gitignore +++ b/.gitignore @@ -33,5 +33,11 @@ src/examples/map_apriltag_save.yaml build_release/ IMU.found +imu.found +.ccls +.ccls-cache +.ccls-root +compile_commands.json +.vimspector.json est.csv /imu.found diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml index d176b3066b5e09dd442ac2bf1a8219f98ccc6d4a..ebfb3d2077bca5974bb4c8e84431101734ec69e3 100644 --- a/.gitlab-ci.yml +++ b/.gitlab-ci.yml @@ -1,107 +1,154 @@ -image: labrobotica/ceres:1.14 +stages: + - license + - build_and_test -before_script: - ## +############ YAML ANCHORS ############ +.preliminaries_template: &preliminaries_definition ## Install ssh-agent if not already installed, it is required by Docker. ## (change apt-get to yum if you use an RPM-based image) - ## - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )' - ## ## Run ssh-agent (inside the build environment) - ## - eval $(ssh-agent -s) - ## ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store ## We're using tr to fix line endings which makes ed25519 keys work ## without extra base64 encoding. ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556 - ## - mkdir -p ~/.ssh - chmod 700 ~/.ssh - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts - ## - ## Create the SSH directory and give it the right permissions - ## - - ls + # update apt - apt-get update - - apt-get install -y build-essential cmake -# SPDLOG -# - apt-get install -y libspdlog-dev - - if [ -d spdlog ]; then - - echo "directory exists" - - if [ "$(ls -A ./spdlog)" ]; then - - echo "directory not empty" - - cd spdlog - - git pull - - else - - echo "directory empty" - - git clone https://github.com/gabime/spdlog.git - - cd spdlog - - fi + # create 'ci_deps' folder (if not exists) + - mkdir -pv ci_deps + +.license_header_template: &license_header_definition + - cd $CI_PROJECT_DIR + + # configure git + - export CI_NEW_BRANCH=ci_processing$RANDOM + - echo creating new temporary branch... $CI_NEW_BRANCH + - git config --global user.email "${CI_EMAIL}" + - git config --global user.name "${CI_USERNAME}" + - git checkout -b $CI_NEW_BRANCH # temporary branch + + # license headers + - export CURRENT_YEAR=$( date +'%Y' ) + - echo "current year:" ${CURRENT_YEAR} + - if [ -f license_header_${CURRENT_YEAR}.txt ]; then + # add license headers to new files + - echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..." + - ./ci_deps/wolf/wolf_scripts/license_manager.sh --add --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps - else - - echo "directory inexistent" - - git clone https://github.com/gabime/spdlog.git - - cd spdlog + # update license headers of all files + - export PREV_YEAR=$(( CURRENT_YEAR-1 )) + - echo "Creating new file license_header_${CURRENT_YEAR}.txt..." + - git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt + - sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt + - ./ci_deps/wolf/wolf_scripts/license_manager.sh --update --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps - fi - - git fetch - - git checkout v0.17.0 - - mkdir -pv build - - cd build - - ls - - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF .. - - make install - - cd ../.. -# YAML -# - apt-get install -y libyaml-cpp-dev - - if [ -d yaml-cpp ]; then - - echo "directory exists" - - if [ "$(ls -A ./yaml-cpp)" ]; then - - echo "directory not empty" - - cd yaml-cpp - - git pull - - else - - echo "directory empty" - - git clone https://github.com/jbeder/yaml-cpp.git - - cd yaml-cpp - - fi + # push changes (if any) + - if git commit -a -m "[skip ci] license headers added or modified" ; then + - git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git" + - git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME} - else - - echo "directory inexistent" - - git clone https://github.com/jbeder/yaml-cpp.git - - cd yaml-cpp + - echo "No changes, nothing to commit!" + - fi + +.install_wolf_template: &install_wolf_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d wolf ]; then + - echo "directory wolf exists" + - cd wolf + - git checkout devel + - git pull + - git checkout $WOLF_CORE_BRANCH + - else + - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git + - cd wolf + - git checkout $WOLF_CORE_BRANCH - fi - mkdir -pv build - cd build - - ls - - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF .. + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF .. + - make -j$(nproc) - make install - - cd ../.. -#Wolf core - - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git - - cd wolf + +.build_and_test_template: &build_and_test_definition + - cd $CI_PROJECT_DIR - mkdir -pv build - cd build - - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=ON .. - make -j$(nproc) - ctest -j$(nproc) - make install - - cd ../.. -wolf_build_and_test: - stage: build +############ LICENSE HEADERS ############ +license_headers: + stage: license + image: labrobotica/wolf_deps:16.04 + cache: + - key: wolf-xenial + paths: + - ci_deps/wolf/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + script: + - *license_header_definition + +############ UBUNTU 16.04 TESTS ############ +build_and_test:xenial: + stage: build_and_test + image: labrobotica/wolf_deps:16.04 + cache: + - key: wolf-xenial + paths: + - ci_deps/wolf/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + script: + - *build_and_test_definition + +############ UBUNTU 18.04 TESTS ############ +build_and_test:bionic: + stage: build_and_test + image: labrobotica/wolf_deps:18.04 + cache: + - key: wolf-bionic + paths: + - ci_deps/wolf/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + script: + - *build_and_test_definition + +############ UBUNTU 20.04 TESTS ############ +build_and_test:focal: + stage: build_and_test + image: labrobotica/wolf_deps:20.04 + cache: + - key: wolf-focal + paths: + - ci_deps/wolf/ except: - master + before_script: + - *preliminaries_definition + - *install_wolf_definition script: - - mkdir -pv build - - cd build - - ls # we can check whether the directory was already full - - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_EXAMPLES=ON -DBUILD_TESTS=ON .. - - make -j$(nproc) - - ctest -j$(nproc) - - make install + - *build_and_test_definition diff --git a/CMakeLists.txt b/CMakeLists.txt index a48779bd37405b9f9b3cb4064b2bebe3eddc227c..9b9d87e3b939f577ce013850c409f09050d6459a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ # Pre-requisites about cmake itself -CMAKE_MINimuM_REQUIRED(VERSION 2.6) +CMAKE_MINimuM_REQUIRED(VERSION 2.8) if(COMMAND cmake_policy) cmake_policy(SET CMP0005 NEW) @@ -8,11 +8,11 @@ endif(COMMAND cmake_policy) # MAC OSX RPATH SET(CMAKE_MACOSX_RPATH 1) - # The project name PROJECT(imu) set(PLUGIN_NAME "wolf${PROJECT_NAME}") +MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...") SET(EXECUTABLE_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/bin) SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib) @@ -24,21 +24,17 @@ ENDIF (NOT CMAKE_BUILD_TYPE) message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.") #Set Flags -SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT") +SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -O0 -D_REENTRANT") SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT") -#Set compiler according C++11 support +#Set compiler according C++14 support include(CheckCXXCompilerFlag) -CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) -CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) -if(COMPILER_SUPPORTS_CXX11) - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") -elseif(COMPILER_SUPPORTS_CXX0X) - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) +if(COMPILER_SUPPORTS_CXX14) + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") else() - message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") endif() if(UNIX) @@ -71,35 +67,24 @@ if(BUILD_TESTS) enable_testing() endif() -MESSAGE("Starting ${PROJECT_NAME} CMakeLists ...") -CMAKE_MINimuM_REQUIRED(VERSION 2.8) - #CMAKE modules - SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake_modules") MESSAGE(STATUS ${CMAKE_MODULE_PATH}) # Some wolf compilation options - IF((CMAKE_BUILD_TYPE MATCHES DEBUG) OR (CMAKE_BUILD_TYPE MATCHES debug) OR (CMAKE_BUILD_TYPE MATCHES Debug)) set(_WOLF_DEBUG true) ENDIF() option(_WOLF_TRACE "Enable wolf tracing macro" ON) -# Does this has any other interest -# but for the examples ? -# yes, for the tests ! -IF(BUILD_EXAMPLES OR BUILD_TESTS) - string(TOUPPER ${PROJECT_NAME} UPPER_NAME) - set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR}) -ENDIF(BUILD_EXAMPLES OR BUILD_TESTS) - - -#find dependencies. -# ============EXAMPLE================== +# ============ DEPENDENCIES ============ FIND_PACKAGE(wolfcore REQUIRED) +# ============ config.h ============ +string(TOUPPER ${PROJECT_NAME} UPPER_NAME) +set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR}) + # Define the directory where will be the configured config.h SET(WOLF_CONFIG_DIR ${PROJECT_BINARY_DIR}/conf/${PROJECT_NAME}/internal) @@ -117,20 +102,11 @@ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/internal/config.h.in "${WOLF_CONFIG_D message("CONFIG DIRECTORY ${PROJECT_BINARY_DIR}") include_directories("${PROJECT_BINARY_DIR}/conf") -#INCLUDES SECTION -# ============EXAMPLE================== +# ============ INCLUDES SECTION ============ INCLUDE_DIRECTORIES(${wolfcore_INCLUDE_DIRS}) INCLUDE_DIRECTORIES(BEFORE "include") -#HEADERS - -SET(HDRS_COMMON - ) -SET(HDRS_MATH - include/imu/math/imu_tools.h - ) -SET(HDRS_UTILS - ) +# ============ HEADERS ============ SET(HDRS_CAPTURE include/imu/capture/capture_compass.h include/imu/capture/capture_imu.h @@ -139,108 +115,67 @@ SET(HDRS_FACTOR include/imu/factor/factor_compass_3d.h include/imu/factor/factor_imu.h include/imu/factor/factor_imu_bias_static.h + include/imu/factor/factor_imu2d.h ) SET(HDRS_FEATURE include/imu/feature/feature_imu.h + include/imu/feature/feature_imu2d.h ) -SET(HDRS_LANDMARK +SET(HDRS_MATH + include/imu/math/imu_tools.h ) SET(HDRS_PROCESSOR include/imu/processor/processor_compass.h include/imu/processor/processor_imu.h + include/imu/processor/processor_imu2d.h ) SET(HDRS_SENSOR include/imu/sensor/sensor_compass.h include/imu/sensor/sensor_imu.h - ) -SET(HDRS_SOLVER + include/imu/sensor/sensor_imu2d.h ) SET(HDRS_TREE_MANAGER include/imu/tree_manager/tree_manager_sliding_window_static_bias.h ) -SET(HDRS_DTASSC - ) - -#SOURCES - -SET(SRCS_COMMON - ) -SET(SRCS_MATH - include/imu/math/imu_tools.h - ) -SET(SRCS_UTILS - ) +# ============ SOURCES ============ SET(SRCS_CAPTURE src/capture/capture_imu.cpp ) -SET(SRCS_FACTOR - ) SET(SRCS_FEATURE src/feature/feature_imu.cpp - ) -SET(SRCS_LANDMARK + src/feature/feature_imu2d.cpp ) SET(SRCS_PROCESSOR src/processor/processor_compass.cpp src/processor/processor_imu.cpp + src/processor/processor_imu2d.cpp test/processor_imu_UnitTester.cpp + test/processor_imu2d_UnitTester.cpp ) SET(SRCS_SENSOR src/sensor/sensor_compass.cpp src/sensor/sensor_imu.cpp - ) -SET(SRCS_DTASSC - ) -SET(SRCS_SOLVER + src/sensor/sensor_imu2d.cpp ) SET(SRCS_TREE_MANAGER src/tree_manager/tree_manager_sliding_window_static_bias.cpp ) SET(SRCS_YAML src/yaml/processor_imu_yaml.cpp + src/yaml/processor_imu2d_yaml.cpp src/yaml/sensor_imu_yaml.cpp + src/yaml/sensor_imu2d_yaml.cpp ) -#OPTIONALS -#optional HDRS and SRCS -# ==================EXAMPLE=============== -# IF (Ceres_FOUND) -# SET(HDRS_WRAPPER -# include/base/solver_suitesparse/sparse_utils.h -# include/base/solver/solver_manager.h -# include/base/ceres_wrapper/ceres_manager.h -# include/base/ceres_wrapper/cost_function_wrapper.h -# include/base/ceres_wrapper/create_numeric_diff_cost_function.h -# include/base/ceres_wrapper/local_parametrization_wrapper.h -# ) -# SET(SRCS_WRAPPER -# src/solver/solver_manager.cpp -# src/ceres_wrapper/ceres_manager.cpp -# src/ceres_wrapper/local_parametrization_wrapper.cpp -# ) -# ELSE(Ceres_FOUND) -# SET(HDRS_WRAPPER) -# SET(SRCS_WRAPPER) -# ENDIF(Ceres_FOUND) - # create the shared library ADD_LIBRARY(${PLUGIN_NAME} SHARED - ${SRCS_BASE} ${SRCS_CAPTURE} - ${SRCS_COMMON} - ${SRCS_DTASSC} - ${SRCS_FACTOR} ${SRCS_FEATURE} - ${SRCS_LANDMARK} - ${SRCS_MATH} ${SRCS_PROCESSOR} ${SRCS_SENSOR} - ${SRCS_SOLVER} ${SRCS_TREE_MANAGER} - ${SRCS_UTILS} - ${SRCS_WRAPPER} ${SRCS_YAML} ) @@ -256,73 +191,46 @@ elseif (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") # using GCC endif() - #Link the created libraries -#===============EXAMPLE========================= -# IF (Ceres_FOUND) -# TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${CERES_LIBRARIES}) -# ENDIF(Ceres_FOUND) TARGET_LINK_LIBRARIES(${PLUGIN_NAME} ${wolfcore_LIBRARIES}) - -#Build tests -#===============EXAMPLE========================= +#===============Build tests========================= IF(BUILD_TESTS) MESSAGE("Building tests.") add_subdirectory(test) ENDIF(BUILD_TESTS) #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_DTASSC} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/association) -INSTALL(FILES ${HDRS_MATH} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math) -INSTALL(FILES ${HDRS_COMMON} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/common) -INSTALL(FILES ${HDRS_UTILS} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/utils) INSTALL(FILES ${HDRS_CAPTURE} DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/capture) INSTALL(FILES ${HDRS_FACTOR} DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/factor) INSTALL(FILES ${HDRS_FEATURE} DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/feature) -INSTALL(FILES ${HDRS_SENSOR} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor) +INSTALL(FILES ${HDRS_MATH} + DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/math) INSTALL(FILES ${HDRS_PROCESSOR} DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/processor) -INSTALL(FILES ${HDRS_LANDMARK} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/landmark) -INSTALL(FILES ${HDRS_WRAPPER} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/ceres_wrapper) -#INSTALL(FILES ${HDRS_SOLVER_SUITESPARSE} -# DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver_suitesparse) -INSTALL(FILES ${HDRS_SOLVER} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/solver) -INSTALL(FILES ${HDRS_SERIALIZATION} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/serialization) +INSTALL(FILES ${HDRS_SENSOR} + DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/sensor) INSTALL(FILES ${HDRS_TREE_MANAGER} DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/tree_manager) -INSTALL(FILES ${HDRS_YAML} - DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/yaml) +FILE(WRITE imu.found "") INSTALL(FILES ${PROJECT_NAME}.found DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}) -FILE(WRITE imu.found "") - INSTALL(FILES "${WOLF_CONFIG_DIR}/config.h" DESTINATION include/iri-algorithms/wolf/plugin_${PROJECT_NAME}/${PROJECT_NAME}/internal) INSTALL(FILES "${CMAKE_SOURCE_DIR}/cmake_modules/${PLUGIN_NAME}Config.cmake" DESTINATION "lib/cmake/${PLUGIN_NAME}") + INSTALL(DIRECTORY ${SPDLOG_INCLUDE_DIRS} DESTINATION "include/iri-algorithms/") export(PACKAGE ${PLUGIN_NAME}) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000000000000000000000000000000000000..ea713a415e0a702b7e0098a2889919b53643d8ad --- /dev/null +++ b/LICENSE @@ -0,0 +1,620 @@ + + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/> + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. diff --git a/README.md b/README.md index dd79d5e177365518822ce675cf4fce278e65e45f..2345eb53e24a8f4179ea43625f727662fba7df26 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ WOLF - Windowed Localization Frames | IMU Plugin =================================== -For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf_doc/). \ No newline at end of file +For installation guide and code documentation, please visit the [documentation website](http://mobile_robotics.pages.iri.upc-csic.es/wolf_projects/wolf_lib/wolf-doc-sphinx/). diff --git a/cmake_modules/wolfimuConfig.cmake b/cmake_modules/wolfimuConfig.cmake index b157cd3870468e1fbfad10b206b4ba55e310244b..97662cc09eb78aaf9b2b80e5b32cad9c59579bfc 100644 --- a/cmake_modules/wolfimuConfig.cmake +++ b/cmake_modules/wolfimuConfig.cmake @@ -12,7 +12,7 @@ ENDIF(wolfimu_INCLUDE_DIRS) FIND_LIBRARY( wolfimu_LIBRARIES NAMES libwolfimu.so libwolfimu.dylib - PATHS /usr/local/lib/iri-algorithms) + PATHS /usr/local/lib) IF(wolfimu_LIBRARIES) MESSAGE("Found wolf imu lib: ${wolfimu_LIBRARIES}") ELSE(wolfimu_LIBRARIES) @@ -77,3 +77,8 @@ if(NOT wolf_FOUND) list(APPEND wolfimu_LIBRARIES ${wolfcore_LIBRARIES}) list(REVERSE wolfimu_LIBRARIES) endif() + +# provide both INCLUDE_DIR and INCLUDE_DIRS +SET(wolfimu_INCLUDE_DIR ${wolfimu_INCLUDE_DIRS}) +# provide both LIBRARY and LIBRARIES +SET(wolfimu_LIBRARY ${wolfimu_LIBRARIES}) diff --git a/demos/demo_factor_imu.cpp b/demos/demo_factor_imu.cpp index 581d7429331d13a0bf67f847f7d9924c0814358e..9b0e262c6c41715f619ea5b25b4be67b9672d3c5 100644 --- a/demos/demo_factor_imu.cpp +++ b/demos/demo_factor_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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/ceres_wrapper/solver_ceres.h> @@ -46,7 +67,7 @@ int main(int argc, char** argv) // Set the origin Eigen::VectorXd x0(16); x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,.001, 0,0,.002; // Try some non-zero biases - wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(x0, t); //this also creates a keyframe at origin + wolf_problem_ptr_->getMotionProvider()->setOrigin(x0, t); //this also creates a keyframe at origin wolf_problem_ptr_->getTrajectory()->getFrameList().front()->fix(); //fix the keyframe at origin TimeStamp ts(0); @@ -78,14 +99,14 @@ int main(int argc, char** argv) /// ******************************************************************************************** /// /// factor creation //create FrameIMU - ts = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - state_vec = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState(); + ts = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_; + state_vec = wolf_problem_ptr_->getMotionProvider()->getCurrentState(); FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature - delta_preint_cov = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); - delta_preint = wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; + delta_preint_cov = wolf_problem_ptr_->getMotionProvider()->getCurrentDeltaPreintCov(); + delta_preint = wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_; std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); feat_imu->setCapture(imu_ptr); @@ -114,7 +135,7 @@ int main(int argc, char** argv) std::cout << "residuals : " << residu.transpose() << std::endl; //reset origin of motion to new frame - wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(last_frame); + wolf_problem_ptr_->getMotionProvider()->setOrigin(last_frame); imu_ptr->setFrame(last_frame); } /// ******************************************************************************************** /// @@ -139,14 +160,14 @@ int main(int argc, char** argv) /// ******************************************************************************************** /// /// factor creation //create FrameIMU - ts = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - state_vec = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState(); + ts = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_; + state_vec = wolf_problem_ptr_->getMotionProvider()->getCurrentState(); FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature - delta_preint_cov = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); - delta_preint = wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; + delta_preint_cov = wolf_problem_ptr_->getMotionProvider()->getCurrentDeltaPreintCov(); + delta_preint = wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_; std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); feat_imu->setCapture(imu_ptr); @@ -175,7 +196,7 @@ int main(int argc, char** argv) std::cout << "residuals : " << residu.transpose() << std::endl; //reset origin of motion to new frame - wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(last_frame); + wolf_problem_ptr_->getMotionProvider()->setOrigin(last_frame); imu_ptr->setFrame(last_frame); } @@ -197,14 +218,14 @@ int main(int argc, char** argv) //create the factor //create FrameIMU - ts = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - state_vec = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState(); + ts = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_; + state_vec = wolf_problem_ptr_->getMotionProvider()->getCurrentState(); FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY, ts, state_vec); wolf_problem_ptr_->getTrajectory()->addFrame(last_frame); //create a feature - delta_preint_cov = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); - delta_preint = wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; + delta_preint_cov = wolf_problem_ptr_->getMotionProvider()->getCurrentDeltaPreintCov(); + delta_preint = wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_; std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov); feat_imu->setCapture(imu_ptr); @@ -239,13 +260,13 @@ int main(int argc, char** argv) ///having a look at covariances Eigen::MatrixXd predelta_cov; predelta_cov.resize(9,9); - predelta_cov = wolf_problem_ptr_->getProcessorIsMotion()->getCurrentDeltaPreintCov(); + predelta_cov = wolf_problem_ptr_->getMotionProvider()->getCurrentDeltaPreintCov(); //std::cout << "predelta_cov : \n" << predelta_cov << std::endl; ///Optimization // PRIOR //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectory()->getFrameList().front(); - wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(wolf_problem_ptr_->getTrajectory()->getFrameList().front()); + wolf_problem_ptr_->getMotionProvider()->setOrigin(wolf_problem_ptr_->getTrajectory()->getFrameList().front()); //SensorBasePtr sensorbase = std::make_shared<SensorBase>("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0); //CapturePosePtr initial_covariance = std::make_shared<CaptureFix>(TimeStamp(0), sensorbase, first_frame->getState().head(7), Eigen::Matrix6d::Identity() * 0.01); //first_frame->addCapture(initial_covariance); diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp index e218a88cbf8f48b7537954f52d02ce7c7ee957f1..b9d4db8a650c799465f8f176156a55d065e884f9 100644 --- a/demos/demo_imuDock.cpp +++ b/demos/demo_imuDock.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imuDock.cpp * diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp index d5bd9aa8bc357f6be1c5135d8c1ad7730c4858da..6b8fee70fa055ecfa00bc9317d4d427fdea71236 100644 --- a/demos/demo_imuDock_autoKFs.cpp +++ b/demos/demo_imuDock_autoKFs.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imuDock_autoKFs.cpp * diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp index 628c4b65ed2e8b88a3cdfbedc8f07ef80f906001..7fd1d9488c9ff3b9625aa8b2e6c9fb6938f1f850 100644 --- a/demos/demo_imuPlateform_Offline.cpp +++ b/demos/demo_imuPlateform_Offline.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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/ceres_wrapper/solver_ceres.h> @@ -145,9 +166,9 @@ int main(int argc, char** argv) } TimeStamp t0, tf; - t0 = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().front().ts_; - tf = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - int N = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().size(); + t0 = wolf_problem_ptr_->getMotionProvider()->getBuffer().front().ts_; + tf = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_; + int N = wolf_problem_ptr_->getMotionProvider()->getBuffer().size(); //Finally, process the only one odom input mot_ptr->setTimeStamp(ts); @@ -168,20 +189,20 @@ int main(int argc, char** argv) std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8) << x_origin.head(16).transpose() << std::endl; std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8) - << wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; + << wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl; std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8) - << wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState().head(16).transpose() << std::endl; + << wolf_problem_ptr_->getMotionProvider()->getCurrentState().head(16).transpose() << std::endl; std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) - << (wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; + << (wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; // Print statistics std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl; std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl; /*TimeStamp t0, tf; - t0 = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().front().ts_; - tf = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - int N = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().size();*/ + t0 = wolf_problem_ptr_->getMotionProvider()->getBuffer().front().ts_; + tf = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_; + int N = wolf_problem_ptr_->getMotionProvider()->getBuffer().size();*/ std::cout << "t0 : " << t0.get() << " s" << std::endl; std::cout << "tf : " << tf.get() << " s" << std::endl; std::cout << "duration : " << tf-t0 << " s" << std::endl; diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp index 66db5c1c71e81035aa6ea70e000a8a92d29b4af6..3837895eb78d85b15dd9a8818b4b459ca425aee9 100644 --- a/demos/demo_imu_constrained0.cpp +++ b/demos/demo_imu_constrained0.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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/ceres_wrapper/solver_ceres.h> @@ -223,20 +244,20 @@ int main(int argc, char** argv) std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8) << x_origin.head(16).transpose() << std::endl; std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8) - << wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; + << wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl; std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8) - << wolf_problem_ptr_->getProcessorIsMotion()->getCurrentState().head(16).transpose() << std::endl; + << wolf_problem_ptr_->getMotionProvider()->getCurrentState().head(16).transpose() << std::endl; std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) - << (wolf_problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl; + << (wolf_problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl; // Print statistics std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl; std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl; TimeStamp t0, tf; - t0 = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().front().ts_; - tf = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - int N = wolf_problem_ptr_->getProcessorIsMotion()->getBuffer().size(); + t0 = wolf_problem_ptr_->getMotionProvider()->getBuffer().front().ts_; + tf = wolf_problem_ptr_->getMotionProvider()->getBuffer().back().ts_; + int N = wolf_problem_ptr_->getMotionProvider()->getBuffer().size(); std::cout << "t0 : " << t0.get() << " s" << std::endl; std::cout << "tf : " << tf.get() << " s" << std::endl; std::cout << "duration : " << tf-t0 << " s" << std::endl; diff --git a/demos/demo_processor_imu.cpp b/demos/demo_processor_imu.cpp index 5439f2624415c6115ccedb3bb438596fc1d0bf38..ad49116d06753947d87d141e9eb89908242caba6 100644 --- a/demos/demo_processor_imu.cpp +++ b/demos/demo_processor_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_processor_imu.cpp * @@ -93,7 +114,7 @@ int main(int argc, char** argv) // Set the origin Eigen::VectorXd x0(16); x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases - problem_ptr_->getProcessorIsMotion()->setOrigin(x0, t); + problem_ptr_->getMotionProvider()->setOrigin(x0, t); // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.) CaptureIMUPtr imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero()); @@ -132,18 +153,18 @@ int main(int argc, char** argv) << data.transpose() << std::endl; std::cout << "Current delta: " << std::fixed << std::setprecision(3) << std::setw(8) << std::right - << problem_ptr_->getProcessorIsMotion()->getMotion().delta_.transpose() << std::endl; + << problem_ptr_->getMotionProvider()->getMotion().delta_.transpose() << std::endl; std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8) - << problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; + << problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl; - Eigen::VectorXd x = problem_ptr_->getProcessorIsMotion()->getCurrentState(); + Eigen::VectorXd x = problem_ptr_->getMotionProvider()->getCurrentState(); std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8) << x.head(10).transpose() << std::endl; std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) - << (problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; + << (problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; std::cout << std::endl; @@ -155,10 +176,10 @@ int main(int argc, char** argv) Eigen::VectorXd x_debug; TimeStamp ts; - delta_debug = problem_ptr_->getProcessorIsMotion()->getMotion().delta_; - delta_integr_debug = problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_; - x_debug = problem_ptr_->getProcessorIsMotion()->getCurrentState(); - ts = problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; + delta_debug = problem_ptr_->getMotionProvider()->getMotion().delta_; + delta_integr_debug = problem_ptr_->getMotionProvider()->getMotion().delta_integr_; + x_debug = problem_ptr_->getMotionProvider()->getCurrentState(); + ts = problem_ptr_->getMotionProvider()->getBuffer().back().ts_; if(debug_results) debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t" @@ -178,11 +199,11 @@ int main(int argc, char** argv) std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8) << x0.head(16).transpose() << std::endl; std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8) - << problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_.transpose() << std::endl; + << problem_ptr_->getMotionProvider()->getMotion().delta_integr_.transpose() << std::endl; std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8) - << problem_ptr_->getProcessorIsMotion()->getCurrentState().head(16).transpose() << std::endl; + << problem_ptr_->getMotionProvider()->getCurrentState().head(16).transpose() << std::endl; // std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8) -// << (problem_ptr_->getProcessorIsMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; +// << (problem_ptr_->getMotionProvider()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl; // Print statistics std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl; @@ -194,9 +215,9 @@ int main(int argc, char** argv) #endif TimeStamp t0, tf; - t0 = problem_ptr_->getProcessorIsMotion()->getBuffer().front().ts_; - tf = problem_ptr_->getProcessorIsMotion()->getBuffer().back().ts_; - int N = problem_ptr_->getProcessorIsMotion()->getBuffer().size(); + t0 = problem_ptr_->getMotionProvider()->getBuffer().front().ts_; + tf = problem_ptr_->getMotionProvider()->getBuffer().back().ts_; + int N = problem_ptr_->getMotionProvider()->getBuffer().size(); std::cout << "t0 : " << t0.get() << " s" << std::endl; std::cout << "tf : " << tf.get() << " s" << std::endl; std::cout << "duration : " << tf-t0 << " s" << std::endl; diff --git a/demos/demo_processor_imu_jacobians.cpp b/demos/demo_processor_imu_jacobians.cpp index 6c0714c9c9968db72b580c2004f7a1810925e4c0..2cbc0518a97beb6f178603e7a0f7538356899cfe 100644 --- a/demos/demo_processor_imu_jacobians.cpp +++ b/demos/demo_processor_imu_jacobians.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_processor_imu_jacobians.cpp * @@ -49,7 +70,7 @@ int main(int argc, char** argv) Eigen::VectorXd x0(16); x0 << 0,1,0, 0,0,0,1, 1,0,0, 0,0,.000, 0,0,.000; // P Q V B B - //wolf_problem_ptr_->getProcessorIsMotion()->setOrigin(x0, t); + //wolf_problem_ptr_->getMotionProvider()->setOrigin(x0, t); //CaptureIMU* imu_ptr; diff --git a/demos/processor_imu2d.yaml b/demos/processor_imu2d.yaml new file mode 100644 index 0000000000000000000000000000000000000000..6d8d756c415d5d9db4b32ab80cf1ea06f72736fd --- /dev/null +++ b/demos/processor_imu2d.yaml @@ -0,0 +1,12 @@ +type: "ProcessorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +time_tolerance: 0.0025 # Time tolerance for joining KFs +unmeasured_perturbation_std: 0.00001 + +keyframe_vote: + voting_active: false + voting_aux_active: false + max_time_span: 2.0 # seconds + max_buff_length: 20000 # motion deltas + dist_traveled: 2.0 # meters + angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg) diff --git a/demos/sensor_imu2d.yaml b/demos/sensor_imu2d.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ce810d4f5f397a7bcb8647e086c026b125fbc936 --- /dev/null +++ b/demos/sensor_imu2d.yaml @@ -0,0 +1,9 @@ +type: "SensorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +motion_variances: + a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2 + w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec + ab_initial_stdev: 0.800 # m/s2 - initial bias + wb_initial_stdev: 0.350 # rad/sec - initial bias + ab_rate_stdev: 0.1 # m/s2/sqrt(s) + wb_rate_stdev: 0.0400 # rad/s/sqrt(s) diff --git a/include/imu/capture/capture_compass.h b/include/imu/capture/capture_compass.h index 1a77f7ce5282fbaf6b615c8cea1950eaa7cbe274..dc1fef50b1e6cc25acf4154634da98af698f6e65 100644 --- a/include/imu/capture/capture_compass.h +++ b/include/imu/capture/capture_compass.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_COMPASS_H_ #define CAPTURE_COMPASS_H_ diff --git a/include/imu/capture/capture_imu.h b/include/imu/capture/capture_imu.h index 861af516e10a8c48afc1be766800cf8a7721cbc6..4b4e05a1caf32f4627a7bc78d8fffdc413a1acf8 100644 --- a/include/imu/capture/capture_imu.h +++ b/include/imu/capture/capture_imu.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU_H #define CAPTURE_IMU_H @@ -23,7 +44,7 @@ class CaptureImu : public CaptureMotion SensorBasePtr _sensor_ptr, const Eigen::Vector6d& _data, const Eigen::MatrixXd& _data_cov, - const Vector6d& _bias, + const VectorXd& _bias, CaptureBasePtr _capture_origin_ptr = nullptr); ~CaptureImu() override; diff --git a/include/imu/factor/factor_compass_3d.h b/include/imu/factor/factor_compass_3d.h index 6bb877776b886236d74f69abb1ec8062dad0bec5..3772950880281c9b6e6261192d9ac3edb0f563cd 100644 --- a/include/imu/factor/factor_compass_3d.h +++ b/include/imu/factor/factor_compass_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_COMPASS_3D_H_ #define FACTOR_COMPASS_3D_H_ diff --git a/include/imu/factor/factor_fix_bias.h b/include/imu/factor/factor_fix_bias.h index e989b9b8861fa83d7f0568f2ea545dd50bed647e..2de0cf321f0cf93aefb50808c44afe3301f67fb1 100644 --- a/include/imu/factor/factor_fix_bias.h +++ b/include/imu/factor/factor_fix_bias.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_FIX_BIAS_H_ #define FACTOR_FIX_BIAS_H_ diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h index 17c80316246bdd977ed59b4edc0aea6f34dca4f7..7bbae22c3f2f0598b60c19f1bad5d45e177ab0c0 100644 --- a/include/imu/factor/factor_imu.h +++ b/include/imu/factor/factor_imu.h @@ -1,9 +1,31 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU_THETA_H_ #define FACTOR_IMU_THETA_H_ //Wolf includes #include "imu/feature/feature_imu.h" #include "imu/sensor/sensor_imu.h" +#include "imu/processor/processor_imu.h" #include "core/factor/factor_autodiff.h" #include "core/math/rotations.h" @@ -235,9 +257,9 @@ Eigen::Vector9d FactorImu::error() Map<const Vector3d > acc_bias(bias.data()); Map<const Vector3d > gyro_bias(bias.data() + 3); - Eigen::Vector9d delta_exp = expectation(); + Eigen::Vector10d delta_exp = expectation(); - Eigen::Vector9d delta_preint = getMeasurement(); + Eigen::Vector10d delta_preint = getMeasurement(); Eigen::Vector9d delta_step; @@ -247,9 +269,9 @@ Eigen::Vector9d FactorImu::error() Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step); - Eigen::Vector9d res = imu::diff(delta_exp, delta_corr); + Eigen::Vector9d err = imu::diff(delta_exp, delta_corr); - return res; + return err; } template<typename D1, typename D2, typename D3> diff --git a/include/imu/factor/factor_imu2d.h b/include/imu/factor/factor_imu2d.h new file mode 100644 index 0000000000000000000000000000000000000000..71129e3ead4e1840cbd6d11bad14231a7e35cefe --- /dev/null +++ b/include/imu/factor/factor_imu2d.h @@ -0,0 +1,205 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU2D_THETA_H_ +#define FACTOR_IMU2D_THETA_H_ + +//Wolf includes +#include "imu/feature/feature_imu2d.h" +#include "imu/processor/processor_imu2d.h" +#include "imu/sensor/sensor_imu2d.h" +#include "core/factor/factor_autodiff.h" +#include "core/math/rotations.h" +#include "imu/math/imu2d_tools.h" + +//Eigen include + +namespace wolf { + +WOLF_PTR_TYPEDEFS(FactorImu2d); + +//class +class FactorImu2d : public FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3> +{ + public: + FactorImu2d(const FeatureImu2dPtr& _ftr_ptr, + const CaptureImuPtr& _capture_origin_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status = FAC_ACTIVE); + + ~FactorImu2d() override = default; + + /** \brief : compute the residual from the state blocks being iterated by the solver. + -> computes the expected measurement + -> corrects actual measurement with new bias + -> compares the corrected measurement with the expected one + -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;) + */ + template<typename T> + bool operator ()(const T* const _p1, + const T* const _o1, + const T* const _v1, + const T* const _b1, + const T* const _p2, + const T* const _o2, + const T* const _v2, + const T* const _b2, + T* _res) const; + Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureImu2dPtr& _ftr_ptr) + { + Eigen::Matrix3d res = Eigen::Matrix3d::Identity(); + if(_ftr_ptr->getCapture()->getSensor()){ + res *= 1./(std::static_pointer_cast<SensorImu2d>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev() * dt_); + res(2,2) = 1./(std::static_pointer_cast<SensorImu2d>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev() * dt_); + } + WOLF_WARN_COND(!_ftr_ptr->getCapture()->getSensor(), "Null Sensor Pointer"); + return res; + } + + private: + // Imu processor + ProcessorImu2dPtr processor_imu2d_; + + /// Preintegrated delta + Eigen::Vector5d delta_preint_; + + // Biases used during preintegration + Eigen::Vector3d bias_preint_; + + // Jacobians of preintegrated deltas wrt biases + Eigen::Matrix<double, 5, 3> jacobian_bias_; + + /// Metrics + const double dt_; ///< delta-time and delta-time-squared between keyframes + + /** bias covariance and bias residuals + * + * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2) + * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error + * + * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix + * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r + * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r) + * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse() + * + * same logic for gyroscope bias + */ + const Eigen::Matrix5d sqrt_delta_preint_inv_; + const Eigen::Matrix3d sqrt_bias_drift_dt_inv_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; +}; + +///////////////////// IMPLEMENTAITON //////////////////////////// + +inline FactorImu2d::FactorImu2d(const FeatureImu2dPtr& _ftr_ptr, + const CaptureImuPtr& _cap_origin_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status) : + FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3>( // ... + "FactorImu2d", + TOP_MOTION, + _ftr_ptr, + _cap_origin_ptr->getFrame(), + _cap_origin_ptr, + nullptr, + nullptr, + _processor_ptr, + _apply_loss_function, + _status, + _cap_origin_ptr->getFrame()->getP(), + _cap_origin_ptr->getFrame()->getO(), + _cap_origin_ptr->getFrame()->getV(), + _cap_origin_ptr->getSensorIntrinsic(), + _ftr_ptr->getFrame()->getP(), + _ftr_ptr->getFrame()->getO(), + _ftr_ptr->getFrame()->getV(), + _ftr_ptr->getCapture()->getSensorIntrinsic()), + processor_imu2d_(std::static_pointer_cast<ProcessorImu2d>(_processor_ptr)), + delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time + bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time + jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases + dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()), + sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper()), + sqrt_bias_drift_dt_inv_(getBiasDriftSquareRootInformationUpper(_ftr_ptr)) +{ + // +} + +template<typename T> +inline bool FactorImu2d::operator ()(const T* const _p1, + const T* const _th1, + const T* const _v1, + const T* const _b1, + const T* const _p2, + const T* const _th2, + const T* const _v2, + const T* const _b2, + T* _res) const +{ + using namespace Eigen; + + // MAPS + Map<const Matrix<T,2,1> > p1(_p1); + const T& th1 = *_th1; + Map<const Matrix<T,2,1> > v1(_v1); + Map<const Matrix<T,3,1> > b1(_b1); + + Map<const Matrix<T,2,1> > p2(_p2); + const T& th2 = *_th2; + Map<const Matrix<T,2,1> > v2(_v2); + Map<const Matrix<T,3,1> > b2(_b2); + + Map<Matrix<T,8,1> > res(_res); + + //residual + /* + * MATH: + * res_delta = (Covariancia delta)^(T/2) * ((delta_preint (+) Jacob^delta_bias*(b1-b1_preint))- (x2 (-) x1)) + */ + Matrix<T, 5, 1> delta_step = jacobian_bias_*(b1 - bias_preint_); + Matrix<T, 5, 1> delta_preint = delta_preint_.cast<T>(); + Matrix<T, 5, 1> delta_corr = imu2d::plus(delta_preint, delta_step); + + Matrix<T, 5, 1> delta_predicted; + Map<Matrix<T,2,1> > dp_predicted( &delta_predicted(0) + 0); + T& dth_predicted = delta_predicted(2); + Map<Matrix<T,2,1> > dv_predicted(&delta_predicted(0) + 3); + imu2d::between(p1, th1, v1, p2, th2, v2, dt_, dp_predicted, dth_predicted, dv_predicted); + + Matrix<T, 5, 1> delta_error = delta_corr - delta_predicted; + delta_error(2) = pi2pi(delta_error(2)); + + res.template head<5>() = sqrt_delta_preint_inv_*delta_error; + + + //bias drift + res.template tail<3>() = sqrt_bias_drift_dt_inv_*(b2 -b1); + + return true; +} + +} // namespace wolf + +#endif diff --git a/include/imu/feature/feature_imu.h b/include/imu/feature/feature_imu.h index 9248608f14b2c6a0d339af5776ae2459d96ad8ab..6aa5f45116edbe33ac4459868cf34b9819af4e3c 100644 --- a/include/imu/feature/feature_imu.h +++ b/include/imu/feature/feature_imu.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU_H_ #define FEATURE_IMU_H_ diff --git a/include/imu/feature/feature_imu2d.h b/include/imu/feature/feature_imu2d.h new file mode 100644 index 0000000000000000000000000000000000000000..58f64b31afcd6610783359458a1261394d2726fa --- /dev/null +++ b/include/imu/feature/feature_imu2d.h @@ -0,0 +1,89 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- +#ifndef FEATURE_IMU2D_H_ +#define FEATURE_IMU2D_H_ + +//Wolf includes +#include "imu/capture/capture_imu.h" +#include "core/feature/feature_base.h" +#include "core/common/wolf.h" + +//std includes + +namespace wolf { + +WOLF_PTR_TYPEDEFS(FeatureImu2d); + +class FeatureImu2d : public FeatureBase +{ + public: + + /** \brief Constructor from and measures + * + * \param _measurement the measurement + * \param _meas_covariance the noise of the measurement + * \param _dD_db_jacobians Jacobians of preintegrated delta wrt Imu2d biases + * \param acc_bias accelerometer bias of origin frame + * \param gyro_bias gyroscope bias of origin frame + * \param _cap_imu_ptr pointer to parent CaptureMotion + */ + FeatureImu2d(const Eigen::VectorXd& _delta_preintegrated, + const Eigen::MatrixXd& _delta_preintegrated_covariance, + const Eigen::Vector3d& _bias, + const Eigen::Matrix<double,5,3>& _dD_db_jacobians, + CaptureMotionPtr _cap_imu_ptr = nullptr); + + /** \brief Constructor from capture pointer + * + * \param _cap_imu_ptr pointer to parent CaptureMotion + */ + FeatureImu2d(CaptureMotionPtr _cap_imu_ptr); + + ~FeatureImu2d() override; + + const Eigen::Vector3d& getBiasPreint() const; + const Eigen::Matrix<double, 5, 3>& getJacobianBias() const; + + private: + + // Used biases + Eigen::Vector3d bias_preint_; + + Eigen::Matrix<double, 5, 3> jacobian_bias_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; +}; + +inline const Eigen::Vector3d& FeatureImu2d::getBiasPreint() const +{ + return bias_preint_; +} + +inline const Eigen::Matrix<double, 5, 3>& FeatureImu2d::getJacobianBias() const +{ + return jacobian_bias_; +} + +} // namespace wolf + +#endif diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h new file mode 100644 index 0000000000000000000000000000000000000000..018c34f27e4b7910ab777c842098dec503ff7cf0 --- /dev/null +++ b/include/imu/math/imu2d_tools.h @@ -0,0 +1,864 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- +/* + * imu2d_tools.h + * + * Created on: Nov 16, 2020 + * Author: igeer + */ + +#ifndef IMU2D_TOOLS_H_ +#define IMU2D_TOOLS_H_ + +#include "core/common/wolf.h" +#include "core/math/rotations.h" +#include "core/math/SE2.h" +#include "core/state_block/state_composite.h" + +#include <cstdio> + +/* + * Most functions in this file are the 2d versions of the functions in imu_tools.h + * They relate manipulations of Delta motion magnitudes used for Imu pre-integration. + * + * The Delta is defined as + * Delta = [Dp, Dth, Dv] + * with + * Dp : position delta + * Dth : angle delta + * Dv : velocity delta + * + * They are listed below: + * + * - identity: I = Delta at the origin, with Dp = [0,0]; Dth = [0], Dv = [0,0] + * - inverse: so that D (+) D.inv = I + * - compose: Dc = D1 (+) D2 + * - between: Db = D2 (-) D1, so that D2 = D1 (+) Db + * - composeOverState: x2 = x1 (+) D + * - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D + * - log: go from Delta manifold to tangent space (equivalent to log() in rotations) + * - exp_Imu: go from tangent space to delta manifold (equivalent to exp() in rotations) + * - plus: D2 = D1 (+) exp_Imu(d) + * - diff: d = log_Imu( D2 (-) D1 ) + * - body2delta: construct a delta from body magnitudes of linAcc and angVel + */ + +namespace wolf +{ +namespace imu2d { +using namespace Eigen; +using namespace SE2; + +template<typename D1, typename D2, typename D3> +inline void identity(MatrixBase<D1>& p, double& th, MatrixBase<D3>& v) +{ + p = MatrixBase<D1>::Zero(2,1); + th = 0; + v = MatrixBase<D3>::Zero(2,1); +} + +template<typename D1, typename D2, typename D3> +inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& th, MatrixBase<D3>& v) +{ + typedef typename D1::Scalar T1; + typedef typename D2::Scalar T2; + typedef typename D3::Scalar T3; + p << T1(0), T1(0); + th << T2(0); + v << T3(0), T3(0); +} + +template<typename T = double> +inline Matrix<T, 5, 1> identity() +{ + Matrix<T, 5, 1> ret; + ret<< T(0), T(0), + T(0), + T(0), T(0); + return ret; +} + +inline VectorComposite identityComposite() +{ + VectorComposite D; + D.emplace('P', Vector2d::Zero()); + D.emplace('O', Vector1d::Zero()); + D.emplace('V', Vector2d::Zero()); + return D; +} + +template<typename D1, typename D2, typename D3, typename D4, class T1, class T2, class T3> +inline void inverse(const MatrixBase<D1>& dp, const T1& dth, const MatrixBase<D2>& dv, + const T2& dt, + MatrixBase<D3>& idp, T3& idth, MatrixBase<D4>& idv ) +{ + MatrixSizeCheck<2, 1>::check(dp); + MatrixSizeCheck<2, 1>::check(dv); + MatrixSizeCheck<2, 1>::check(idp); + MatrixSizeCheck<2, 1>::check(idv); + const auto& dR1T = Eigen::Rotation2D<T1>(-dth).matrix(); + + idp = - dR1T*(dp - dv*dt ); + idth = - dth; + idv = - dR1T*dv; +} + +template<typename D1, typename D2, class T> +inline void inverse(const MatrixBase<D1>& d, + T dt, + MatrixBase<D2>& id) +{ + MatrixSizeCheck<5, 1>::check(d); + MatrixSizeCheck<5, 1>::check(id); + + Map<const Matrix<typename D1::Scalar, 2, 1> > dp ( & d( 0 ) ); + Map<const Matrix<typename D1::Scalar, 2, 1> > dv ( & d( 3 ) ); + Map<Matrix<typename D2::Scalar, 2, 1> > idp ( & id( 0 ) ); + Map<Matrix<typename D2::Scalar, 2, 1> > idv ( & id( 3 ) ); + + inverse(dp, d(2), dv, dt, idp, id(2), idv); +} + +template<typename D, class T> +inline Matrix<typename D::Scalar, 5, 1> inverse(const MatrixBase<D>& d, + T& dt) +{ + Matrix<typename D::Scalar, 5, 1> id; + inverse(d, dt, id); + return id; +} + +template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T1, class T2, class T3, class T4> +inline void compose(const MatrixBase<D1>& dp1, const T1& dth1, const MatrixBase<D2>& dv1, + const MatrixBase<D3>& dp2, const T2& dth2, const MatrixBase<D4>& dv2, + const T3& dt, + MatrixBase<D5>& sum_p, T4& sum_th, MatrixBase<D6>& sum_v ) +{ + MatrixSizeCheck<2, 1>::check(dp1); + MatrixSizeCheck<2, 1>::check(dv1); + MatrixSizeCheck<2, 1>::check(dp2); + MatrixSizeCheck<2, 1>::check(dv2); + MatrixSizeCheck<2, 1>::check(sum_p); + MatrixSizeCheck<2, 1>::check(sum_v); + const auto& dR1 = Eigen::Rotation2D<T1>(dth1).matrix(); + + sum_p = dp1 + dv1*dt + dR1*dp2; //Rotation matrix here and below because we are rotating a vector + sum_v = dv1 + dR1*dv2; + sum_th = pi2pi(dth1+dth2); //Sum here because angles compose by sum +} + +template<typename D1, typename D2, typename D3, class T> +inline void compose(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + T dt, + MatrixBase<D3>& sum) +{ + MatrixSizeCheck<5, 1>::check(d1); + MatrixSizeCheck<5, 1>::check(d2); + MatrixSizeCheck<5, 1>::check(sum); + + Map<const Matrix<typename D1::Scalar, 2, 1> > dp1 ( & d1( 0 ) ); + Map<const Matrix<typename D1::Scalar, 2, 1> > dv1 ( & d1( 3 ) ); + Map<const Matrix<typename D2::Scalar, 2, 1> > dp2 ( & d2( 0 ) ); + Map<const Matrix<typename D2::Scalar, 2, 1> > dv2 ( & d2( 3 ) ); + Map<Matrix<typename D3::Scalar, 2, 1> > sum_p ( & sum( 0 ) ); + sum(2) = d1(2) + d2(2); + Map<Matrix<typename D3::Scalar, 2, 1> > sum_v ( & sum( 3 ) ); + + compose(dp1, d1(2), dv1, dp2, d2(2), dv2, dt, sum_p, sum(2), sum_v); +} + +inline void compose(const VectorComposite& d1, const VectorComposite& d2, double dt, VectorComposite& dc) +{ + compose(d1.at('P'), d1.at('O')(0), d1.at('V'), d2.at('P'), d2.at('O')(0), d2.at('V'), dt, dc['P'], dc['O'](0), dc['V']); +} + +inline VectorComposite compose(const VectorComposite& d1, const VectorComposite& d2, double dt) +{ + VectorComposite dc("POV", {2,1,2}); + compose(d1.at('P'), d1.at('O')(0), d1.at('V'), d2.at('P'), d2.at('O')(0), d2.at('V'), dt, dc['P'], dc['O'](0), dc['V']); + return dc; +} + +template<typename D1, typename D2, class T> +inline Matrix<typename D1::Scalar, 5, 1> compose(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + T dt) +{ + Matrix<typename D1::Scalar, 5, 1> ret; + compose(d1, d2, dt, ret); + return ret; +} + +template<typename D1, typename D2, typename D3, typename D4, typename D5, class T> +inline void compose(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + T dt, + MatrixBase<D3>& sum, + MatrixBase<D4>& J_sum_d1, + MatrixBase<D5>& J_sum_d2) +{ + MatrixSizeCheck<5, 1>::check(d1); + MatrixSizeCheck<5, 1>::check(d2); + MatrixSizeCheck<5, 1>::check(sum); + MatrixSizeCheck< 5, 5>::check(J_sum_d1); + MatrixSizeCheck< 5, 5>::check(J_sum_d2); + + //Useful auxiliaries + const auto& dR1 = Rotation2D<typename D1::Scalar>(d1(2)).matrix(); + + // Jac wrt first delta + J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I + J_sum_d1.block(0,2,2,1).noalias() = dR1 * skewed(d2.head(2)) ; // dDp'/dDo + J_sum_d1.block(0,3,2,2).noalias() = Matrix2d::Identity() * dt; // dDp'/dDv = I*dt + J_sum_d1.block(3,2,2,1).noalias() = dR1 * skewed(d2.tail(2)) ; // dDv'/dDo + // J_sum_d1.block(2,2,1,1) = 1; // dDo'/dDo = 1 + + // Jac wrt second delta + J_sum_d2.setIdentity(); // + J_sum_d2.block(0,0,2,2) = dR1; // dDp'/ddp + J_sum_d2.block(3,3,2,2) = dR1; // dDv'/ddv + // J_sum_d2.block(2,2,1,1) = 1; // dDo'/ddo = 1 + + // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable + compose(d1, d2, dt, sum); +} + +//inline void compose(const VectorComposite& d1, +// const VectorComposite& d2, +// double dt, +// VectorComposite& sum, +// MatrixComposite& J_sum_d1, +// MatrixComposite& J_sum_d2) +//{ +// +// // Some useful temporaries +// Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR +// Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR +// +// // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!! +// J_sum_d1.clear(); +// J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I +// J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo +// J_sum_d1.emplace('P','V', Matrix3d::Identity() * dt); // dDp'/dDv = I*dt +// J_sum_d1.emplace('O','P', Matrix3d::Zero()); +// J_sum_d1.emplace('O','O', dR2.transpose()); // dDo'/dDo +// J_sum_d1.emplace('O','V', Matrix3d::Zero()); +// J_sum_d1.emplace('V','P', Matrix3d::Zero()); +// J_sum_d1.emplace('V','O', - dR1 * skew(d2.at('V'))) ; // dDv'/dDo +// J_sum_d1.emplace('V','V', Matrix3d::Identity()); // dDv'/dDv = I +// +// +// // Jac wrt second delta +// J_sum_d2.clear(); +// J_sum_d2.emplace('P','P', dR1); // dDp'/ddp +// J_sum_d2.emplace('P','O', Matrix3d::Zero()) ; // dDp'/ddo +// J_sum_d2.emplace('P','V', Matrix3d::Zero()); // dDp'/ddv +// J_sum_d2.emplace('O','P', Matrix3d::Zero()); +// J_sum_d2.emplace('O','O', Matrix3d::Identity());// dDo'/ddo +// J_sum_d2.emplace('O','V', Matrix3d::Zero()); +// J_sum_d2.emplace('V','P', Matrix3d::Zero()); +// J_sum_d2.emplace('V','O', Matrix3d::Zero()) ; // dDv'/ddo +// J_sum_d2.emplace('V','V', dR1); // dDv'/ddv +// +// // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable +// compose(d1, d2, dt, sum); +//} +// +template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T1, class T2, class T3, class T4> +inline void between(const MatrixBase<D1>& dp1, const T1& dth1, const MatrixBase<D2>& dv1, + const MatrixBase<D3>& dp2, const T2& dth2, const MatrixBase<D4>& dv2, + const T3 dt, + MatrixBase<D5>& diff_p, T4& diff_th, MatrixBase<D6>& diff_v ) +{ + MatrixSizeCheck<2, 1>::check(dp1); + MatrixSizeCheck<2, 1>::check(dv1); + MatrixSizeCheck<2, 1>::check(dp2); + MatrixSizeCheck<2, 1>::check(dv2); + MatrixSizeCheck<2, 1>::check(diff_p); + MatrixSizeCheck<2, 1>::check(diff_v); + + const auto& dR1_tr = Rotation2D<T1>(-dth1).matrix(); + diff_p = dR1_tr * ( dp2 - dp1 - dv1*dt ); + diff_v = dR1_tr * ( dv2 - dv1 ); + diff_th = pi2pi(dth2 - dth1); +} + +template<typename D1, typename D2, typename D3, class T> +inline void between(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + T dt, + MatrixBase<D3>& d2_minus_d1) +{ + MatrixSizeCheck<5, 1>::check(d1); + MatrixSizeCheck<5, 1>::check(d2); + MatrixSizeCheck<5, 1>::check(d2_minus_d1); + + Map<const Matrix<typename D1::Scalar, 2, 1> > dp1 ( & d1(0) ); + Map<const Matrix<typename D1::Scalar, 2, 1> > dv1 ( & d1(3) ); + Map<const Matrix<typename D2::Scalar, 2, 1> > dp2 ( & d2(0) ); + Map<const Matrix<typename D2::Scalar, 2, 1> > dv2 ( & d2(3) ); + Map<Matrix<typename D3::Scalar, 2, 1> > diff_p ( & d2_minus_d1(0) ); + Map<Matrix<typename D3::Scalar, 2, 1> > diff_v ( & d2_minus_d1(3) ); + + between(dp1, d1(2), dv1, dp2, d2(2), dv2, dt, diff_p, d2_minus_d1(2), diff_v); +} + +template<typename D1, typename D2, class T> +inline Matrix<typename D1::Scalar, 5, 1> between(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + T dt) +{ + Matrix<typename D1::Scalar, 5, 1> diff; + between(d1, d2, dt, diff); + return diff; +} + +//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T> +//inline void composeOverState(const MatrixBase<D1>& p, const QuaternionBase<D2>& q, const MatrixBase<D3>& v, +// const MatrixBase<D4>& dp,const QuaternionBase<D5>& dq,const MatrixBase<D6>& dv, +// T dt, +// MatrixBase<D7>& p_plus_dp, QuaternionBase<D8>& q_plus_dq, MatrixBase<D9>& v_plus_dv) +//{ +// MatrixSizeCheck<3, 1>::check(p); +// MatrixSizeCheck<3, 1>::check(v); +// MatrixSizeCheck<3, 1>::check(dp); +// MatrixSizeCheck<3, 1>::check(dv); +// MatrixSizeCheck<3, 1>::check(p_plus_dp); +// MatrixSizeCheck<3, 1>::check(v_plus_dv); +// +// p_plus_dp = p + v*dt + 0.5*gravity()*dt*dt + q*dp; +// v_plus_dv = v + gravity()*dt + q*dv; +// q_plus_dq = q*dq; // dq here to avoid possible aliasing between x and x_plus_d +//} +// +//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T> +//inline void composeOverState(const MatrixBase<D1>& p , const MatrixBase<D2>& q , const MatrixBase<D3>& v, +// const MatrixBase<D4>& dp, const MatrixBase<D5>& dq, const MatrixBase<D6>& dv, +// T dt, +// MatrixBase<D7>& p_plus_dp, MatrixBase<D8>& q_plus_dq, MatrixBase<D9>& v_plus_dv) +//{ +// +// MatrixSizeCheck<3, 1>::check(p); +// MatrixSizeCheck<4, 1>::check(q); +// MatrixSizeCheck<3, 1>::check(v); +// MatrixSizeCheck<3, 1>::check(dp); +// MatrixSizeCheck<4, 1>::check(dq); +// MatrixSizeCheck<3, 1>::check(dv); +// MatrixSizeCheck<3, 1>::check(p_plus_dp); +// MatrixSizeCheck<4, 1>::check(q_plus_dq); +// MatrixSizeCheck<3, 1>::check(v_plus_dv); +// +// Map<const Quaternion<typename D2::Scalar> > qq ( & q (0) ); +// Map<const Quaternion<typename D5::Scalar> > dqq ( & dq (0) ); +// Map< Quaternion<typename D8::Scalar> > qq_plus_dqq ( & q_plus_dq (0) ); +// +// composeOverState(p, qq, v, +// dp, dqq, dv, +// dt, +// p_plus_dp, qq_plus_dqq, v_plus_dv); +//} +// +//template<typename D1, typename D2, typename D3, class T> +//inline void composeOverState(const MatrixBase<D1>& x, +// const MatrixBase<D2>& d, +// T dt, +// MatrixBase<D3>& x_plus_d) +//{ +// MatrixSizeCheck<10, 1>::check(x); +// MatrixSizeCheck<10, 1>::check(d); +// MatrixSizeCheck<10, 1>::check(x_plus_d); +// +// Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & x( 0 ) ); +// Map<const Quaternion<typename D1::Scalar> > q ( & x( 3 ) ); +// Map<const Matrix<typename D1::Scalar, 3, 1> > v ( & x( 7 ) ); +// Map<const Matrix<typename D2::Scalar, 3, 1> > dp ( & d( 0 ) ); +// Map<const Quaternion<typename D2::Scalar> > dq ( & d( 3 ) ); +// Map<const Matrix<typename D2::Scalar, 3, 1> > dv ( & d( 7 ) ); +// Map<Matrix<typename D3::Scalar, 3, 1> > p_plus_d ( & x_plus_d( 0 ) ); +// Map<Quaternion<typename D3::Scalar> > q_plus_d ( & x_plus_d( 3 ) ); +// Map<Matrix<typename D3::Scalar, 3, 1> > v_plus_d ( & x_plus_d( 7 ) ); +// +// composeOverState( p, q, v, +// dp, dq, dv, +// dt, +// p_plus_d, q_plus_d, v_plus_d); +//} +// +//template<typename D1, typename D2, class T> +//inline Matrix<typename D1::Scalar, 10, 1> composeOverState(const MatrixBase<D1>& x, +// const MatrixBase<D2>& d, +// T dt) +//{ +// Matrix<typename D1::Scalar, 10, 1> ret; +// composeOverState(x, d, dt, ret); +// return ret; +//} +// +//template<class T> +//inline void composeOverState(const VectorComposite& x, +// const VectorComposite& d, +// T dt, +// VectorComposite& x_plus_d) +//{ +// assert(x_plus_d.count('P') && "provided reference does not have key 'P'"); +// assert(x_plus_d.count('O') && "provided reference does not have key 'O'"); +// assert(x_plus_d.count('V') && "provided reference does not have key 'V'"); +// +// composeOverState(x.at('P'), x.at('O'), x.at('V'), +// d.at('P'), d.at('O'), d.at('V'), +// dt, +// x_plus_d['P'], x_plus_d['O'], x_plus_d['V']); +//} +// +//template<class T> +//inline VectorComposite composeOverState(const VectorComposite& x, +// const VectorComposite& d, +// T dt) +//{ +// VectorComposite ret("POV", {3,4,3}); +// +// composeOverState(x, d, dt, ret); +// return ret; +//} +// +//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T> +//inline void betweenStates(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, const MatrixBase<D3>& v1, +// const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, const MatrixBase<D6>& v2, +// const T dt, +// MatrixBase<D7>& dp, QuaternionBase<D8>& dq, MatrixBase<D9>& dv ) +//{ +// MatrixSizeCheck<3, 1>::check(p1); +// MatrixSizeCheck<3, 1>::check(v1); +// MatrixSizeCheck<3, 1>::check(p2); +// MatrixSizeCheck<3, 1>::check(v2); +// MatrixSizeCheck<3, 1>::check(dp); +// MatrixSizeCheck<3, 1>::check(dv); +// +// dp = q1.conjugate() * ( p2 - p1 - v1*dt - (T)0.5*gravity().cast<T>()*(T)dt*(T)dt ); +// dv = q1.conjugate() * ( v2 - v1 - gravity().cast<T>()*(T)dt ); +// dq = q1.conjugate() * q2; +//} +// +//template<typename D1, typename D2, typename D3, class T> +//inline void betweenStates(const MatrixBase<D1>& x1, +// const MatrixBase<D2>& x2, +// T dt, +// MatrixBase<D3>& x2_minus_x1) +//{ +// MatrixSizeCheck<10, 1>::check(x1); +// MatrixSizeCheck<10, 1>::check(x2); +// MatrixSizeCheck<10, 1>::check(x2_minus_x1); +// +// Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & x1(0) ); +// Map<const Quaternion<typename D1::Scalar> > q1 ( & x1(3) ); +// Map<const Matrix<typename D1::Scalar, 3, 1> > v1 ( & x1(7) ); +// Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & x2(0) ); +// Map<const Quaternion<typename D2::Scalar> > q2 ( & x2(3) ); +// Map<const Matrix<typename D2::Scalar, 3, 1> > v2 ( & x2(7) ); +// Map<Matrix<typename D3::Scalar, 3, 1> > dp ( & x2_minus_x1(0) ); +// Map<Quaternion<typename D3::Scalar> > dq ( & x2_minus_x1(3) ); +// Map<Matrix<typename D3::Scalar, 3, 1> > dv ( & x2_minus_x1(7) ); +// +// betweenStates(p1, q1, v1, p2, q2, v2, dt, dp, dq, dv); +//} +// +//template<typename D1, typename D2, class T> +//inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1, +// const MatrixBase<D2>& x2, +// T dt) +//{ +// Matrix<typename D1::Scalar, 10, 1> ret; +// betweenStates(x1, x2, dt, ret); +// return ret; +//} +// +//template<typename Derived> +//Matrix<typename Derived::Scalar, 9, 1> log_Imu(const MatrixBase<Derived>& delta_in) +//{ +// MatrixSizeCheck<10, 1>::check(delta_in); +// +// Matrix<typename Derived::Scalar, 9, 1> ret; +// +// Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) ); +// Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) ); +// Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & delta_in(7) ); +// Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) ); +// Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) ); +// Map<Matrix<typename Derived::Scalar, 3, 1> > dv_ret ( & ret(6) ); +// +// dp_ret = dp_in; +// dv_ret = dv_in; +// do_ret = log_q(dq_in); +// +// return ret; +//} +// +//template<typename Derived> +//Matrix<typename Derived::Scalar, 10, 1> exp_Imu(const MatrixBase<Derived>& d_in) +//{ +// MatrixSizeCheck<9, 1>::check(d_in); +// +// Matrix<typename Derived::Scalar, 10, 1> ret; +// +// Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) ); +// Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) ); +// Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & d_in(6) ); +// Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) ); +// Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) ); +// Map<Matrix<typename Derived::Scalar, 3, 1> > dv ( & ret(7) ); +// +// dp = dp_in; +// dv = dv_in; +// dq = exp_q(do_in); +// +// return ret; +//} +// +template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T1, class T2, class T3> +inline void plus(const MatrixBase<D1>& dp1, const T1& dth1, const MatrixBase<D2>& dv1, + const MatrixBase<D3>& dp2, const T2& dth2, const MatrixBase<D4>& dv2, + MatrixBase<D5>& plus_p, T3& plus_th, MatrixBase<D6>& plus_v ) +{ + plus_p = dp1 + dp2; + plus_v = dv1 + dv2; + plus_th = dth1 + dth2; +} + +template<typename D1, typename D2, typename D3> +inline void plus(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + MatrixBase<D3>& d_pert) +{ + Map<const Matrix<typename D1::Scalar, 2, 1> > dp1 ( & d1(0) ); + Map<const Matrix<typename D1::Scalar, 2, 1> > dv1 ( & d1(3) ); + Map<const Matrix<typename D2::Scalar, 2, 1> > dp2 ( & d2(0) ); + Map<const Matrix<typename D2::Scalar, 2, 1> > dv2 ( & d2(3) ); + Map<Matrix<typename D3::Scalar, 2, 1> > dp_p ( & d_pert(0) ); + Map<Matrix<typename D3::Scalar, 2, 1> > dv_p ( & d_pert(3) ); + + plus(dp1, d1(2), dv1, dp2, d2(2), dv2, dp_p, d_pert(2), dv_p); +} + +template<typename D1, typename D2> +inline Matrix<typename D1::Scalar, 5, 1> plus(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2) +{ + Matrix<typename D1::Scalar, 5, 1> ret; + plus(d1, d2, ret); + return ret; +} +// +//inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res) +//{ +// plus(x.at('P'), x.at('O'), x.at('V'), tau.at('P'), tau.at('O'), tau.at('V'), res.at('P'), res.at('O'), res.at('V')); +//} +// +//inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau) +//{ +// VectorComposite res("POV", {3,4,3}); +// +// plus(x, tau, res); +// +// return res; +//} + + +template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6> +inline void diff(const MatrixBase<D1>& dp1, const typename D1::Scalar& do1, const MatrixBase<D2>& dv1, + const MatrixBase<D3>& dp2, const typename D1::Scalar& do2, const MatrixBase<D4>& dv2, + MatrixBase<D5>& diff_p, typename D1::Scalar& diff_o, MatrixBase<D6>& diff_v ) +{ + diff_p = dp2 - dp1; + diff_v = dv2 - dv1; + diff_o = do2 - do1; +} + +//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11> +//inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1, +// const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2, +// MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v , +// MatrixBase<D10>& J_do_dq1, MatrixBase<D11>& J_do_dq2) +//{ +// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v); +// +// J_do_dq1 = - jac_SO3_left_inv(diff_o); +// J_do_dq2 = jac_SO3_right_inv(diff_o); +//} + +template<typename D1, typename D2, typename D3> +inline void diff(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2, + MatrixBase<D3>& err) +{ + Map<const Matrix<typename D1::Scalar, 2, 1> > dp1 ( & d1(0) ); + Map<const Matrix<typename D1::Scalar, 2, 1> > dv1 ( & d1(3) ); + Map<const Matrix<typename D2::Scalar, 2, 1> > dp2 ( & d2(0) ); + Map<const Matrix<typename D2::Scalar, 2, 1> > dv2 ( & d2(3) ); + Map<Matrix<typename D3::Scalar, 2, 1> > diff_p ( & err(0) ); + Map<Matrix<typename D3::Scalar, 2, 1> > diff_v ( & err(3) ); + + diff(dp1, d1(2), dv1, dp2, d2(2), dv2, diff_p, err(2), diff_v); +} + +//template<typename D1, typename D2, typename D3, typename D4, typename D5> +//inline void diff(const MatrixBase<D1>& d1, +// const MatrixBase<D2>& d2, +// MatrixBase<D3>& dif, +// MatrixBase<D4>& J_diff_d1, +// MatrixBase<D5>& J_diff_d2) +//{ +// Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) ); +// Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) ); +// Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) ); +// Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) ); +// Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) ); +// Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) ); +// Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) ); +// Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) ); +// Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) ); +// +// Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2; +// +// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2); +// +// /* d = diff(d1, d2) is +// * dp = dp2 - dp1 +// * do = Log(dq1.conj * dq2) +// * dv = dv2 - dv1 +// * +// * With trivial Jacobians for dp and dv, and: +// * J_do_dq1 = - J_l_inv(theta) +// * J_do_dq2 = J_r_inv(theta) +// */ +// +// J_diff_d1 = - Matrix<typename D4::Scalar, 9, 9>::Identity();// d(p2 - p1) / d(p1) = - Identity +// J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta) +// +// J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity +// J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta) +//} + +template<typename D1, typename D2> +inline Matrix<typename D1::Scalar, 5, 1> diff(const MatrixBase<D1>& d1, + const MatrixBase<D2>& d2) +{ + Matrix<typename D1::Scalar, 5, 1> ret; + diff(d1, d2, ret); + return ret; +} + +template<typename D1, typename D2, typename D3> +inline void exp(const MatrixBase<D1>& a, + const typename D1::Scalar& w, + const typename D1::Scalar& dt, + MatrixBase<D2>& dp, + typename D1::Scalar& dth, + MatrixBase<D3>& dv) +{ + MatrixSizeCheck<2,1>::check(a); + MatrixSizeCheck<2,1>::check(dp); + MatrixSizeCheck<2,1>::check(dv); + + typedef typename D1::Scalar T; + dth = w*dt; + if(dth > Constants::EPS){ + T s = std::sin(dth); + T c = std::cos(dth); + T aux1 = s/dth; + T aux2 = (1-c)/dth; + T aux3 = aux2/dth; + T aux4 = (dth-s)/(dth*dth); + Matrix<T,2,2> skw = SE2::skew((T) 1.0); + + Matrix<T, 2, 2> P = Matrix<T, 2, 2>::Identity() * aux3 + skw*aux4; + Matrix<T, 2, 2> Q = Matrix<T, 2, 2>::Identity() * aux1 + skw*aux2; + + dp = P*a*dt*dt; + dv = Q*a*dt; + //dth = dth; + } + else{ + dp = (T)0.5*a*dt*dt; + dv = a*dt; + //dth = dth; + } +} + + +template<typename D1, typename D2, typename D3> +inline void body2delta(const MatrixBase<D1>& a, + const typename D1::Scalar& w, + const typename D1::Scalar& dt, + MatrixBase<D2>& dp, + typename D1::Scalar& dth, + MatrixBase<D3>& dv) +{ + MatrixSizeCheck<2,1>::check(a); + MatrixSizeCheck<2,1>::check(dp); + MatrixSizeCheck<2,1>::check(dv); + + //dp = 0.5 * a * dt * dt; + //dv = a * dt; + //dth = w * dt; + imu2d::exp(a, w, dt, dp, dth, dv); + +} + +//template<typename D1, typename D2, typename D3, typename D4, typename D5> +//inline void body2delta(const MatrixBase<D1>& a, +// const MatrixBase<D2>& w, +// const typename D1::Scalar& dt, +// MatrixBase<D3>& dp, +// MatrixBase<D4>& dq, +// MatrixBase<D5>& dv) +//{ +// MatrixSizeCheck<3,1>::check(a); +// MatrixSizeCheck<3,1>::check(w); +// MatrixSizeCheck<3,1>::check(dp); +// MatrixSizeCheck<4,1>::check(dq); +// MatrixSizeCheck<3,1>::check(dv); +// +// Map<Quaternion<typename D4::Scalar>> mdq ( & dq(0) ); +// +// body2delta(a, w, dt, dp, mdq, dv); +//} + +template<typename D1> +inline Matrix<typename D1::Scalar, 5, 1> body2delta(const MatrixBase<D1>& body, + const typename D1::Scalar& dt) +{ + MatrixSizeCheck<3,1>::check(body); + + typedef typename D1::Scalar T; + + Matrix<T, 5, 1> delta; + + Map< Matrix<T, 2, 1>> dp ( & delta(0) ); + Map< Matrix<T, 2, 1>> dv ( & delta(3) ); + + body2delta(body.block(0,0,2,1), body(2,0), dt, dp, delta(2), dv); + + return delta; +} + +//template<typename D1> +//inline void body2delta(const MatrixBase<D1>& body, +// const typename D1::Scalar& dt, +// VectorComposite& _delta) +//{ +// MatrixSizeCheck<6,1>::check(body); +// +// body2delta(body.block(0,0,3,1), +// body.block(3,0,3,1), +// dt, +// _delta['P'], +// _delta['O'], +// _delta['V']); +//} + +template<typename D1, typename D2, typename D3> +inline void body2delta(const MatrixBase<D1>& body, + const typename D1::Scalar& dt, + MatrixBase<D2>& delta, + MatrixBase<D3>& jac_body) +{ + MatrixSizeCheck<3,1>::check(body); + MatrixSizeCheck<5,3>::check(jac_body); + + typedef typename D1::Scalar T; + + delta = body2delta(body, dt); + + //Jacobians are not exact, we don't know the exact expression for the imu2d::exp() function + jac_body.setZero(); + jac_body.block(0,0,2,2) = 0.5 * dt * dt * Matrix<T, 2, 2>::Identity(); + jac_body(2,2) = dt; + jac_body.block(3,0,2,2) = dt * Matrix<T, 2, 2>::Identity(); +} + +//template<typename D1> +//inline void body2delta(const MatrixBase<D1>& body, +// const typename D1::Scalar& dt, +// VectorComposite& delta, +// MatrixComposite& jac_body) +//{ +// MatrixSizeCheck<6,1>::check(body); +// +// typedef typename D1::Scalar T; +// +// body2delta(body, dt, delta); +// +// Matrix<T, 3, 1> w = body.block(3,0,3,1); +// +// jac_body.emplace('P','a', 0.5 * dt * dt * Matrix3d::Identity()); // 0,0 +// jac_body.emplace('P','w', Matrix3d::Zero()); // 0,3 +// jac_body.emplace('O','a', Matrix3d::Zero()); // 3,0 +// jac_body.emplace('O','w', dt * jac_SO3_right(w * dt)); // 3,3 +// jac_body.emplace('V','a', dt * Matrix3d::Identity()); // 6,0 +// jac_body.emplace('V','w', Matrix3d::Zero()); // 6,6 +//} +//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7> +//void motion2data(const MatrixBase<D1>& a, const MatrixBase<D2>& w, const QuaternionBase<D3>& q, const MatrixBase<D4>& a_b, const MatrixBase<D5>& w_b, MatrixBase<D6>& a_m, MatrixBase<D7>& w_m) +//{ +// MatrixSizeCheck<3,1>::check(a); +// MatrixSizeCheck<3,1>::check(w); +// MatrixSizeCheck<3,1>::check(a_b); +// MatrixSizeCheck<3,1>::check(w_b); +// MatrixSizeCheck<3,1>::check(a_m); +// MatrixSizeCheck<3,1>::check(w_m); +// +// // Note: data = (a_m , w_m) +// a_m = a + a_b - q.conjugate()*gravity(); +// w_m = w + w_b; +//} +// +///* Create Imu data from body motion +// * Input: +// * motion : [ax, ay, az, wx, wy, wz] the motion in body frame +// * q : the current orientation wrt horizontal +// * bias : the bias of the Imu +// * Output: +// * return : the data vector as created by the Imu (with motion, gravity, and bias) +// */ +//template<typename D1, typename D2, typename D3> +//Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, const QuaternionBase<D2>& q, const MatrixBase<D3>& bias) +//{ +// Matrix<typename D1::Scalar, 6, 1> data; +// Map<Matrix<typename D1::Scalar, 3, 1>> a_m (data.data() ); +// Map<Matrix<typename D1::Scalar, 3, 1>> w_m (data.data() + 3); +// +// motion2data(motion.block(0,0,3,1), +// motion.block(3,0,3,1), +// q, +// bias.block(0,0,3,1), +// bias.block(3,0,3,1), +// a_m, +// w_m ); +// +// return data; +//} + +} // namespace imu2d +} // namespace wolf + +#endif /* Imu_TOOLS_H_ */ diff --git a/include/imu/math/imu_tools.h b/include/imu/math/imu_tools.h index d9b0b58454132c8abaa11d50a5847d18b829337f..e696966cde5f21034b3e04d464c9c3305f5e4ec7 100644 --- a/include/imu/math/imu_tools.h +++ b/include/imu/math/imu_tools.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- /* * imu_tools.h * @@ -566,7 +587,7 @@ inline void plus(const MatrixBase<D1>& dp1, const MatrixBase<D2>& dq1, const Mat template<typename D1, typename D2, typename D3> inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, - MatrixBase<D3>& d_pert) + MatrixBase<D3>& d) { Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) ); Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) ); @@ -574,11 +595,11 @@ inline void plus(const MatrixBase<D1>& d1, Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) ); Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) ); Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(6) ); - Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_pert(0) ); - Map<Quaternion<typename D3::Scalar> > dq_p ( & d_pert(3) ); - Map<Matrix<typename D3::Scalar, 3, 1> > dv_p ( & d_pert(7) ); + Map<Matrix<typename D3::Scalar, 3, 1> > dp ( & d (0) ); + Map<Quaternion<typename D3::Scalar> > dq ( & d (3) ); + Map<Matrix<typename D3::Scalar, 3, 1> > dv ( & d (7) ); - plus(dp1, dq1, dv1, dp2, do2, dv2, dp_p, dq_p, dv_p); + plus(dp1, dq1, dv1, dp2, do2, dv2, dp, dq, dv); } template<typename D1, typename D2> diff --git a/include/imu/processor/processor_compass.h b/include/imu/processor/processor_compass.h index 690755716de4029d257ae0807bf947bc2b511acb..71f572e6317be94c99a22d1431ef73be8d7a08cd 100644 --- a/include/imu/processor/processor_compass.h +++ b/include/imu/processor/processor_compass.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU_PROCESSOR_PROCESSORCOMPASS_H_ #define INCLUDE_IMU_PROCESSOR_PROCESSORCOMPASS_H_ @@ -38,11 +59,11 @@ class ProcessorCompass : public ProcessorBase protected: void processCapture(CaptureBasePtr) override; - void processKeyFrame(FrameBasePtr, const double&) override; + void processKeyFrame(FrameBasePtr _frm) override; void processMatch(FrameBasePtr, CaptureBasePtr); 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/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h index d82a0afe492f938558eb28c53da70a65fb515b3f..12676c83d8536bc8f2ccbcc78eb288dfe74804cd 100644 --- a/include/imu/processor/processor_imu.h +++ b/include/imu/processor/processor_imu.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU_H #define PROCESSOR_IMU_H @@ -19,7 +40,7 @@ struct ParamsProcessorImu : public ParamsProcessorMotion } std::string print() const override { - return "\n" + ParamsProcessorMotion::print(); + return ParamsProcessorMotion::print(); } }; @@ -77,8 +98,6 @@ class ProcessorImu : public ProcessorMotion{ protected: ParamsProcessorImuPtr params_motion_Imu_; - Eigen::Matrix<double, 9, 9> unmeasured_perturbation_cov_; - }; } diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h new file mode 100644 index 0000000000000000000000000000000000000000..433ccf6b92724169420813bdea8692cf5bc6777e --- /dev/null +++ b/include/imu/processor/processor_imu2d.h @@ -0,0 +1,118 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU2D_H +#define PROCESSOR_IMU2D_H + +// Wolf +#include "imu/capture/capture_imu.h" +#include "imu/feature/feature_imu.h" +#include "core/processor/processor_motion.h" + +namespace wolf { +WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorImu2d); + +struct ParamsProcessorImu2d : public ParamsProcessorMotion +{ + ParamsProcessorImu2d() = default; + ParamsProcessorImu2d(std::string _unique_name, const ParamsServer& _server): + ParamsProcessorMotion(_unique_name, _server) + { + // + } + std::string print() const override + { + return ParamsProcessorMotion::print(); + } +}; + +WOLF_PTR_TYPEDEFS(ProcessorImu2d); + +//class +class ProcessorImu2d : public ProcessorMotion{ + public: + ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_Imu); + ~ProcessorImu2d() override; + void configure(SensorBasePtr _sensor) override { }; + + WOLF_PROCESSOR_CREATE(ProcessorImu2d, ParamsProcessorImu2d); + void preProcess() override; + + protected: + void computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jacobian_calib) const override; + void deltaPlusDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta, + const double _dt, + Eigen::VectorXd& _delta_preint_plus_delta) const override; + void deltaPlusDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta, + const double _dt, + Eigen::VectorXd& _delta_preint_plus_delta, + Eigen::MatrixXd& _jacobian_delta_preint, + Eigen::MatrixXd& _jacobian_delta) const override; + void statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _Dt, + VectorComposite& _x_plus_delta) const override; + Eigen::VectorXd deltaZero() const override; + Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, + const Eigen::VectorXd& delta_step) const override; + VectorXd getCalibration (const CaptureBasePtr _capture) const override; + void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; + bool voteForKeyFrame() const override; + CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin) override; + FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override; + FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, + CaptureBasePtr _capture_origin) override; + + protected: + ParamsProcessorImu2dPtr params_motion_Imu_; +}; + +} + +///////////////////////////////////////////////////////// +// IMPLEMENTATION. Put your implementation includes here +///////////////////////////////////////////////////////// + +namespace wolf{ + +inline Eigen::VectorXd ProcessorImu2d::deltaZero() const +{ + return (Eigen::VectorXd(5) << 0,0, 0, 0,0 ).finished(); // p, q, v +} + +} // namespace wolf + +#endif // PROCESSOR_Imu_H diff --git a/include/imu/sensor/sensor_compass.h b/include/imu/sensor/sensor_compass.h index e21bb028d1daf45e662698e4e21b2055fa5c69df..a446b8acabf40ace81f7280d931d14eb8faedb32 100644 --- a/include/imu/sensor/sensor_compass.h +++ b/include/imu/sensor/sensor_compass.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_SENSOR_COMPASS_H_ #define SENSOR_SENSOR_COMPASS_H_ diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h index 783f1f51055b8e9a8123027f44b2ec2f5c5cc46c..e5d0052d893c9996963d65b54b93a7823b02f26e 100644 --- a/include/imu/sensor/sensor_imu.h +++ b/include/imu/sensor/sensor_imu.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU_H #define SENSOR_IMU_H diff --git a/include/imu/sensor/sensor_imu2d.h b/include/imu/sensor/sensor_imu2d.h new file mode 100644 index 0000000000000000000000000000000000000000..700d612ba2752ca3d115bd355658b7e30940ef73 --- /dev/null +++ b/include/imu/sensor/sensor_imu2d.h @@ -0,0 +1,140 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU2D_H +#define SENSOR_IMU2D_H + +//wolf includes +#include "core/sensor/sensor_base.h" +#include "core/utils/params_server.h" +#include <iostream> + +namespace wolf { + +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorImu2d); + + +struct ParamsSensorImu2d : public ParamsSensorBase +{ + //noise std dev + double w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s) + double a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s) + + //Initial biases std dev + double ab_initial_stdev = 0.01; //accelerometer micro_g/sec + double wb_initial_stdev = 0.01; //gyroscope rad/sec + + // bias rate of change std dev + double ab_rate_stdev = 0.00001; + double wb_rate_stdev = 0.00001; + + ~ParamsSensorImu2d() override = default; + ParamsSensorImu2d() + { + //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. + } + ParamsSensorImu2d(std::string _unique_name, const ParamsServer& _server): + ParamsSensorBase(_unique_name, _server) + { + w_noise = _server.getParam<double>(prefix + _unique_name + "/w_noise"); + a_noise = _server.getParam<double>(prefix + _unique_name + "/a_noise"); + ab_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_initial_stdev"); + wb_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_initial_stdev"); + ab_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_rate_stdev"); + wb_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_rate_stdev"); + } + std::string print() const override + { + return ParamsSensorBase::print() + "\n" + + "w_noise: " + std::to_string(w_noise) + "\n" + + "a_noise: " + std::to_string(a_noise) + "\n" + + "ab_initial_stdev: " + std::to_string(ab_initial_stdev) + "\n" + + "wb_initial_stdev: " + std::to_string(wb_initial_stdev) + "\n" + + "ab_rate_stdev: " + std::to_string(ab_rate_stdev) + "\n" + + "wb_rate_stdev: " + std::to_string(wb_rate_stdev) + "\n"; + } +}; + +WOLF_PTR_TYPEDEFS(SensorImu2d); + +class SensorImu2d : public SensorBase +{ + + protected: + double a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz) + double w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz) + + //This is a trial to factor how much can the bias change in 1 sec at most + double ab_initial_stdev; //accelerometer m/sec^s + double wb_initial_stdev; //gyroscope rad/sec + double ab_rate_stdev; //accelerometer m/sec^2 / sqrt(sec) + double wb_rate_stdev; //gyroscope rad/sec / sqrt(sec) + + public: + + SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params); + SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPtr _params); + WOLF_SENSOR_CREATE(SensorImu2d, ParamsSensorImu2d, 3); + + double getGyroNoise() const; + double getAccelNoise() const; + double getWbInitialStdev() const; + double getAbInitialStdev() const; + double getWbRateStdev() const; + double getAbRateStdev() const; + + ~SensorImu2d() override; + +}; + +inline double SensorImu2d::getGyroNoise() const +{ + return w_noise; +} + +inline double SensorImu2d::getAccelNoise() const +{ + return a_noise; +} + +inline double SensorImu2d::getWbInitialStdev() const +{ + return wb_initial_stdev; +} + +inline double SensorImu2d::getAbInitialStdev() const +{ + return ab_initial_stdev; +} + +inline double SensorImu2d::getWbRateStdev() const +{ + return wb_rate_stdev; +} + +inline double SensorImu2d::getAbRateStdev() const +{ + return ab_rate_stdev; +} + +} // namespace wolf + +#endif // SENSOR_Imu2D_H diff --git a/license_header_2022.txt b/license_header_2022.txt new file mode 100644 index 0000000000000000000000000000000000000000..0c987025f9dba3e7af993051b9bdf4b5ff400e0d --- /dev/null +++ b/license_header_2022.txt @@ -0,0 +1,17 @@ +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp index 7f3457c5aff220581e6bd6f791a4ab18bd35b135..cf9b8bbe1eafb3f58b11cbe03bdc80bf2f4e3a6e 100644 --- a/src/capture/capture_imu.cpp +++ b/src/capture/capture_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 "imu/capture/capture_imu.h" #include "imu/sensor/sensor_imu.h" #include "core/state_block/state_quaternion.h" @@ -17,7 +38,7 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts, _capture_origin_ptr, nullptr, nullptr, - _sensor_ptr->isStateBlockDynamic('I') ? std::make_shared<StateBlock>(6, false) : nullptr) + _sensor_ptr->isStateBlockDynamic('I') ? std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false) : nullptr) { // } @@ -26,7 +47,7 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts, SensorBasePtr _sensor_ptr, const Eigen::Vector6d& _acc_gyro_data, const Eigen::MatrixXd& _data_cov, - const Vector6d& _bias, + const VectorXd& _bias, CaptureBasePtr _capture_origin_ptr) : CaptureMotion("CaptureImu", _init_ts, @@ -38,7 +59,7 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts, nullptr, _sensor_ptr->isStateBlockDynamic('I') ? std::make_shared<StateBlock>(_bias, false) : nullptr) { - // + assert((_bias.size() == 3) or (_bias.size() == 6)); } CaptureImu::~CaptureImu() diff --git a/src/feature/feature_imu.cpp b/src/feature/feature_imu.cpp index d3547a52c2c6794ed2d795af780ca2063ef14e36..3dfb28f79cff470aa22a1a468c4ab4c48dc09c5e 100644 --- a/src/feature/feature_imu.cpp +++ b/src/feature/feature_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 "imu/feature/feature_imu.h" namespace wolf { diff --git a/src/feature/feature_imu2d.cpp b/src/feature/feature_imu2d.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9fee0928794000dc527029cdf13cc1e51bef0875 --- /dev/null +++ b/src/feature/feature_imu2d.cpp @@ -0,0 +1,49 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 "imu/feature/feature_imu2d.h" + +namespace wolf { + +FeatureImu2d::FeatureImu2d(const Eigen::VectorXd& _delta_preintegrated, + const Eigen::MatrixXd& _delta_preintegrated_covariance, + const Eigen::Vector3d& _bias, + const Eigen::Matrix<double,5,3>& _dD_db_jacobians, + CaptureMotionPtr _cap_imu_ptr) : + FeatureBase("FeatureImu2d", _delta_preintegrated, _delta_preintegrated_covariance), + bias_preint_(_bias), + jacobian_bias_(_dD_db_jacobians) +{ +} + +FeatureImu2d::FeatureImu2d(CaptureMotionPtr _cap_imu_ptr): + FeatureBase("FeatureImu2d", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()), + bias_preint_ (_cap_imu_ptr->getCalibrationPreint()), + jacobian_bias_(_cap_imu_ptr->getJacobianCalib()) +{ +} + +FeatureImu2d::~FeatureImu2d() +{ + // +} + +} // namespace wolf diff --git a/src/processor/processor_compass.cpp b/src/processor/processor_compass.cpp index a52cbf199fb0ace2318490cb3cc25f2fe1123ddf..bf30eb57164306cc11aefa69642dab4b8fe30c18 100644 --- a/src/processor/processor_compass.cpp +++ b/src/processor/processor_compass.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 "imu/processor/processor_compass.h" #include "imu/capture/capture_compass.h" #include "imu/factor/factor_compass_3d.h" @@ -18,24 +39,24 @@ ProcessorCompass::~ProcessorCompass() void ProcessorCompass::processCapture(CaptureBasePtr _capture) { // Search for any stored frame within time tolerance of capture - auto frame_pack = buffer_pack_kf_.select(_capture->getTimeStamp(), params_->time_tolerance); - if (frame_pack) + auto keyframe = buffer_frame_.select(_capture->getTimeStamp(), params_->time_tolerance); + if (keyframe) { - processMatch(frame_pack->key_frame, _capture); + processMatch(keyframe, _capture); // remove the frame and older frames - buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp()); + buffer_frame_.removeUpTo(keyframe->getTimeStamp()); } // Otherwise: store capture // Note that more than one processor can be emplacing frames, so an older frame can arrive later than this one. else - buffer_capture_.add(_capture->getTimeStamp(), _capture); + buffer_capture_.emplace(_capture->getTimeStamp(), _capture); } -void ProcessorCompass::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance) +void ProcessorCompass::processKeyFrame(FrameBasePtr _frame) { // Search for any stored capture within time tolerance of frame - auto capture = buffer_capture_.select(_frame->getTimeStamp(), _time_tolerance); + auto capture = buffer_capture_.select(_frame->getTimeStamp(), getTimeTolerance()); if (capture) { processMatch(_frame, capture); @@ -44,10 +65,10 @@ void ProcessorCompass::processKeyFrame(FrameBasePtr _frame, const double& _time_ buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10); } // Otherwise: If frame is more recent than any capture in buffer -> store frame to be processed later in processCapture - else if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), _time_tolerance) == nullptr) + else if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), getTimeTolerance()) == nullptr) { // store frame - buffer_pack_kf_.add(_frame, _time_tolerance); + buffer_frame_.emplace(_frame->getTimeStamp(), _frame); } // Otherwise: There are captures more recent than the frame but none that match with it -> discard frame } diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp index f1b7ffbf2e5185526be6b0056c503681b7afa8f4..8ea0483860864d7e865cdfbc17e1745484374668 100644 --- a/src/processor/processor_imu.cpp +++ b/src/processor/processor_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- // imu #include "imu/processor/processor_imu.h" #include "imu/factor/factor_imu.h" @@ -13,11 +34,7 @@ ProcessorImu::ProcessorImu(ParamsProcessorImuPtr _params_motion_imu) : ProcessorMotion("ProcessorImu", "POV", 3, 10, 10, 9, 6, 6, _params_motion_imu), params_motion_Imu_(std::make_shared<ParamsProcessorImu>(*_params_motion_imu)) { - // Set constant parts of Jacobians - jacobian_delta_preint_.setIdentity(9,9); // dDp'/dDp, dDv'/dDv, all zeros - jacobian_delta_.setIdentity(9,9); // - jacobian_calib_.setZero(9,6); - unmeasured_perturbation_cov_ = pow(params_motion_Imu_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<double, 9, 9>::Identity(); + // } ProcessorImu::~ProcessorImu() diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a1efbcf5861a232703aed6ab4a63afdf1dbd9c99 --- /dev/null +++ b/src/processor/processor_imu2d.cpp @@ -0,0 +1,262 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- +// imu +#include "imu/processor/processor_imu2d.h" +#include "imu/factor/factor_imu2d.h" +#include "imu/math/imu2d_tools.h" + +// wolf +#include <core/state_block/state_composite.h> + +namespace wolf { + + ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_imu) : + ProcessorMotion("ProcessorImu2d", "POV", 2, 5, 5, 5, 6, 3, _params_motion_imu), + params_motion_Imu_(std::make_shared<ParamsProcessorImu2d>(*_params_motion_imu)) + { + // + } + + ProcessorImu2d::~ProcessorImu2d() + { + // + } + + void ProcessorImu2d::preProcess() + { + auto cap_ptr = std::dynamic_pointer_cast<CaptureImu>(incoming_ptr_); + assert(cap_ptr != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImu").c_str()); + } + + bool ProcessorImu2d::voteForKeyFrame() const + { + // time span + if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span) + { + WOLF_DEBUG( "PM: vote: time span" ); + return true; + } + // buffer length + if (getBuffer().size() > params_motion_Imu_->max_buff_length) + { + WOLF_DEBUG( "PM: vote: buffer length" ); + return true; + } + // angle turned + double angle = std::abs(delta_integrated_(2)); + if (angle > params_motion_Imu_->angle_turned) + { + WOLF_DEBUG( "PM: vote: angle turned" ); + return true; + } + //WOLF_DEBUG( "PM: do not vote" ); + return false; + } + + + CaptureMotionPtr ProcessorImu2d::emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin) + { + auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureImu>(_frame_own, + _ts, + _sensor, + _data, + _data_cov, + _capture_origin)); + setCalibration(cap_motion, _calib); + cap_motion->setCalibrationPreint(_calib_preint); + + return cap_motion; + } + + FeatureBasePtr ProcessorImu2d::emplaceFeature(CaptureMotionPtr _capture_motion) + { + auto feature = FeatureBase::emplace<FeatureImu2d>(_capture_motion, + _capture_motion->getBuffer().back().delta_integr_, + _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_, + _capture_motion->getCalibrationPreint(), + _capture_motion->getBuffer().back().jacobian_calib_); + return feature; + } + + VectorXd ProcessorImu2d::getCalibration (const CaptureBasePtr _capture) const + { + if (_capture) + return _capture->getStateBlock('I')->getState(); + else + return getSensor()->getStateBlockDynamic('I')->getState(); + } + + void ProcessorImu2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration) + { + _capture->getSensorIntrinsic()->setState(_calibration); + } + + FactorBasePtr ProcessorImu2d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) + { + CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin); + FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(_feature_motion); + + auto fac_imu = FactorBase::emplace<FactorImu2d>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function); + + return fac_imu; + } + + void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jac_delta_calib) const + { + using namespace Eigen; + assert(_data.size() == data_size_ && "Wrong data size!"); + Vector3d data_2d; + data_2d << _data(0), _data(1), _data(5); + Matrix3d data_cov_2d; + data_cov_2d << _data_cov(0,0), _data_cov(0,1), _data_cov(0,5), + _data_cov(1,0), _data_cov(1,1), _data_cov(1,5), + _data_cov(5,0), _data_cov(5,1), _data_cov(5,5); + + + + Matrix<double, 5, 3> jac_delta_data; + /* + * We have the following computing pipeline: + * Input: data, calib, dt + * Output: delta, delta_cov, jac_calib + * + * We do: + * body = data - calib (measurement - bias) + * delta = body2delta(body, dt) --> jac_body + * jac_data = jac_body + * jac_calib = - jac_body + * delta_cov <-- propagate data_cov using jac_data + * + * where body2delta(.) produces a delta from body=(a,w) as follows: + * dp = P * a * dt^2 + * dth = exp(w * dt) = w * dt + * dv = Q * a * dt + * where P and Q are defined in imu2d::exp(), and also a jacobian J^delta_data + */ + + //create delta + imu2d::body2delta(data_2d - _calib, _dt, _delta, jac_delta_data); + + // compute delta_cov + _delta_cov = jac_delta_data * data_cov_2d * jac_delta_data.transpose(); + + // compute jacobian_calib + _jac_delta_calib = - jac_delta_data; + } + + void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta, + const double _dt, + Eigen::VectorXd& _delta_preint_plus_delta) const + { + /* MATHS: + * Simple composition, + * (D o d)p = Dp + Dth*dp + Dv*dt + * (D o d)th = Dth+dth + * (D o d)v = Dv + Dth*dv + * where Dth*dp and Dth*dv are rotations and not scalar products. The version with jacobians is not used here. + */ + _delta_preint_plus_delta = imu2d::compose(_delta_preint, _delta, _dt); + } + + void ProcessorImu2d::statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _dt, + VectorComposite& _x_plus_delta) const + { + assert(_x.includesStructure("POV") && "Any key missing in _x"); + assert(_delta.size() == 5 && "Wrong _delta vector size"); + assert(_dt >= 0 && "Time interval _dt is negative!"); + + const auto& delta = VectorComposite(_delta, "POV", {2,1,2}); + /* + * MATHS: + * + * In the absence of gravity (2D case) it's the same as deltaPlusDelta(), so the same compose() function is used + */ + _x_plus_delta = imu2d::compose(_x, delta, _dt); + + } + + void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta, + const double _dt, + Eigen::VectorXd& _delta_preint_plus_delta, + Eigen::MatrixXd& _jacobian_delta_preint, + Eigen::MatrixXd& _jacobian_delta) const + { + /* + * Expression of the delta integration step, D' = D (+) d: + * + * Dp' = Dp + Dv*dt + Dq*dp + * Dv' = Dv + Dq*dv + * Dq' = Dq * dq + * + * Jacobians for covariance propagation. + * + * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as: + * + * D'_D = [ I DR*skew(1)*dp I*dt + * 0 1 0 + * 0 DR*skew(1)*dv I ] + * + * b. With respect to delta, gives _jacobian_delta = D'_d as: + * + * D'_d = [ DR 0 0 + * 0 1 0 + * 0 0 DR ] + * + * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion. + */ + imu2d::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu2d_tools + } + + Eigen::VectorXd ProcessorImu2d::correctDelta (const Eigen::VectorXd& delta_preint, + const Eigen::VectorXd& delta_step) const + { + return imu2d::plus(delta_preint, delta_step); + } + + +} // namespace wolf + +// Register in the FactorySensor +#include "core/processor/factory_processor.h" + +namespace wolf +{ + WOLF_REGISTER_PROCESSOR(ProcessorImu2d) + WOLF_REGISTER_PROCESSOR_AUTO(ProcessorImu2d) +} diff --git a/src/sensor/sensor_compass.cpp b/src/sensor/sensor_compass.cpp index d81f52926e3633cc31d248a88e8f119bc6f2012b..43171641af0110385be84e72f40251bc5797b5cc 100644 --- a/src/sensor/sensor_compass.cpp +++ b/src/sensor/sensor_compass.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 "imu/sensor/sensor_compass.h" #include "core/state_block/state_block.h" #include "core/state_block/state_quaternion.h" diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp index bd5c035cba9999c8dc8c93bdd2a6b317746d4a97..586cc0c2aee61875ef30c715538a8546f0fb1c20 100644 --- a/src/sensor/sensor_imu.cpp +++ b/src/sensor/sensor_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 "imu/sensor/sensor_imu.h" #include "core/state_block/state_block.h" #include "core/state_block/state_quaternion.h" @@ -28,7 +49,7 @@ SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu& ab_rate_stdev(_params.ab_rate_stdev), wb_rate_stdev(_params.wb_rate_stdev) { - assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2d."); + assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 3d."); if (prior_bias) { diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e307e6ec245d12e82ec6bb8b5b5bc7d7f94708fa --- /dev/null +++ b/src/sensor/sensor_imu2d.cpp @@ -0,0 +1,58 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 <imu/sensor/sensor_imu2d.h> +#include <core/state_block/state_block.h> +#include <core/state_block/state_angle.h> + +namespace wolf { + +SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPtr _params) : + SensorImu2d(_extrinsics, *_params) +{ + // +} + +SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) : + SensorBase("SensorImu2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), std::make_shared<StateBlock>(3, false, nullptr), (Eigen::Vector3d()<<_params.a_noise,_params.a_noise,_params.w_noise).finished(), false, false, true), + a_noise(_params.a_noise), + w_noise(_params.w_noise), + ab_initial_stdev(_params.ab_initial_stdev), + wb_initial_stdev(_params.wb_initial_stdev), + ab_rate_stdev(_params.ab_rate_stdev), + wb_rate_stdev(_params.wb_rate_stdev) +{ + assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d."); +} + +SensorImu2d::~SensorImu2d() +{ + // +} + +} // namespace wolf + +// Register in the FactorySensor +#include "core/sensor/factory_sensor.h" +namespace wolf { +WOLF_REGISTER_SENSOR(SensorImu2d) +WOLF_REGISTER_SENSOR_AUTO(SensorImu2d); +} // namespace wolf diff --git a/src/yaml/processor_imu2d_yaml.cpp b/src/yaml/processor_imu2d_yaml.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b1d1e282e4991d7ad991d5e6f5a70bb31984c187 --- /dev/null +++ b/src/yaml/processor_imu2d_yaml.cpp @@ -0,0 +1,74 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu2d_yaml.cpp + * + * Created on: Nov 24, 2020 + * \author: igeer + */ + +// wolf yaml +#include "imu/processor/processor_imu2d.h" +#include "core/yaml/yaml_conversion.h" + +// wolf +#include "core/common/factory.h" + +// yaml-cpp library +#include <yaml-cpp/yaml.h> + +namespace wolf +{ + +namespace +{ +static ParamsProcessorBasePtr createProcessorImu2dParams(const std::string & _filename_dot_yaml) +{ + YAML::Node config = YAML::LoadFile(_filename_dot_yaml); + std::cout << _filename_dot_yaml << '\n'; + + if (config["type"].as<std::string>() == "ProcessorImu2d") + { + YAML::Node kf_vote = config["keyframe_vote"]; + + ParamsProcessorImu2dPtr params = std::make_shared<ParamsProcessorImu2d>(); + params->time_tolerance = config["time_tolerance"] .as<double>(); + params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<double>(); + + params->max_time_span = kf_vote["max_time_span"] .as<double>(); + params->max_buff_length = kf_vote["max_buff_length"] .as<SizeEigen>(); + params->dist_traveled = kf_vote["dist_traveled"] .as<double>(); + params->angle_turned = kf_vote["angle_turned"] .as<double>(); + params->voting_active = kf_vote["voting_active"] .as<bool>(); + return params; + } + + std::cout << "Bad configuration file. No processor type found." << std::endl; + return nullptr; +} + +// Register in the FactorySensor +const bool WOLF_UNUSED registered_prc_imu2d = FactoryParamsProcessor::registerCreator("ProcessorImu2d", createProcessorImu2dParams); + +} // namespace [unnamed] + +} // namespace wolf diff --git a/src/yaml/processor_imu_yaml.cpp b/src/yaml/processor_imu_yaml.cpp index 4d75aa8f13069a7a22404de29fe90462196b394e..30221d8e0350f1bcf840acd0fddcba2f2f845e8e 100644 --- a/src/yaml/processor_imu_yaml.cpp +++ b/src/yaml/processor_imu_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_imu_yaml.cpp * diff --git a/src/yaml/sensor_imu2d_yaml.cpp b/src/yaml/sensor_imu2d_yaml.cpp new file mode 100644 index 0000000000000000000000000000000000000000..23aee8a13778464ad2ef877494e376e43f4b814e --- /dev/null +++ b/src/yaml/sensor_imu2d_yaml.cpp @@ -0,0 +1,75 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu2d_yaml.cpp + * + * Created on: Nov 24, 2020 + * \author: igeer + */ + +// wolf yaml +#include "imu/sensor/sensor_imu2d.h" +#include "core/yaml/yaml_conversion.h" + +// wolf +#include "core/common/factory.h" + +// yaml-cpp library +#include <yaml-cpp/yaml.h> + +namespace wolf +{ + +namespace +{ + +static ParamsSensorBasePtr createParamsSensorImu2d(const std::string & _filename_dot_yaml) +{ + YAML::Node config = YAML::LoadFile(_filename_dot_yaml); + + if (config["type"].as<std::string>() == "SensorImu2d") + { + YAML::Node variances = config["motion_variances"]; + YAML::Node kf_vote = config["keyframe_vote"]; + + ParamsSensorImu2dPtr params = std::make_shared<ParamsSensorImu2d>(); + + params->a_noise = variances["a_noise"] .as<double>(); + params->w_noise = variances["w_noise"] .as<double>(); + params->ab_initial_stdev = variances["ab_initial_stdev"] .as<double>(); + params->wb_initial_stdev = variances["wb_initial_stdev"] .as<double>(); + params->ab_rate_stdev = variances["ab_rate_stdev"] .as<double>(); + params->wb_rate_stdev = variances["wb_rate_stdev"] .as<double>(); + + return params; + } + + std::cout << "Bad configuration file. No sensor type found." << std::endl; + return nullptr; +} + +// Register in the FactorySensor +const bool WOLF_UNUSED registered_imu2d_intr = FactoryParamsSensor::registerCreator("SensorImu2d", createParamsSensorImu2d); + +} // namespace [unnamed] + +} // namespace wolf diff --git a/src/yaml/sensor_imu_yaml.cpp b/src/yaml/sensor_imu_yaml.cpp index 87201a4699e047e1873fe12de0ce121bcd96d1db..fbee701af0d92988fd0833da9aac2dbbee5927c4 100644 --- a/src/yaml/sensor_imu_yaml.cpp +++ b/src/yaml/sensor_imu_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_imu_yaml.cpp * @@ -28,7 +49,6 @@ static ParamsSensorBasePtr createParamsSensorImu(const std::string & _filename_d if (config["type"].as<std::string>() == "SensorImu") { YAML::Node variances = config["motion_variances"]; - YAML::Node kf_vote = config["keyframe_vote"]; ParamsSensorImuPtr params = std::make_shared<ParamsSensorImu>(); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 42bce1a6d1364c4ea6af8943a55efcbc88345aad..893aaadb8400712867b4f3df2e223b2c3fb0e4a9 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -14,33 +14,48 @@ target_link_libraries(gtest_example ${PLUGIN_NAME}) # ########################################################### wolf_add_gtest(gtest_processor_imu gtest_processor_imu.cpp) -target_link_libraries(gtest_processor_imu ${PLUGIN_NAME} ${wolf_LIBRARY}) +target_link_libraries(gtest_processor_imu ${PLUGIN_NAME}) + +wolf_add_gtest(gtest_processor_imu2d gtest_processor_imu2d.cpp) +target_link_libraries(gtest_processor_imu2d ${PLUGIN_NAME}) wolf_add_gtest(gtest_imu gtest_imu.cpp) -target_link_libraries(gtest_imu ${PLUGIN_NAME} ${wolf_LIBRARY}) +target_link_libraries(gtest_imu ${PLUGIN_NAME}) wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp) -target_link_libraries(gtest_imu_tools ${PLUGIN_NAME} ${wolf_LIBRARY}) +target_link_libraries(gtest_imu_tools ${PLUGIN_NAME}) + +wolf_add_gtest(gtest_imu2d_tools gtest_imu2d_tools.cpp) +target_link_libraries(gtest_imu2d_tools ${PLUGIN_NAME}) wolf_add_gtest(gtest_processor_imu_jacobians gtest_processor_imu_jacobians.cpp) -target_link_libraries(gtest_processor_imu_jacobians ${PLUGIN_NAME} ${wolf_LIBRARY}) +target_link_libraries(gtest_processor_imu_jacobians ${PLUGIN_NAME}) wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp) -target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY}) +target_link_libraries(gtest_feature_imu ${PLUGIN_NAME}) + +wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp) +target_link_libraries(gtest_factor_imu2d ${PLUGIN_NAME}) + +wolf_add_gtest(gtest_imu_static_init gtest_imu_static_init.cpp) +target_link_libraries(gtest_imu_static_init ${PLUGIN_NAME}) + +wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp) +target_link_libraries(gtest_imu2d_static_init ${PLUGIN_NAME}) wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp) -target_link_libraries(gtest_imu_mocap_fusion ${PLUGIN_NAME} ${wolf_LIBRARY}) +target_link_libraries(gtest_imu_mocap_fusion ${PLUGIN_NAME}) wolf_add_gtest(gtest_factor_compass_3d gtest_factor_compass_3d.cpp) -target_link_libraries(gtest_factor_compass_3d ${PLUGIN_NAME} ${wolf_LIBRARY}) +target_link_libraries(gtest_factor_compass_3d ${PLUGIN_NAME}) # Has been excluded from tests for god knows how long, so thing is broken. # Maybe call an archeologist to fix this thing? # wolf_add_gtest(gtest_factor_imu gtest_factor_imu.cpp) -# target_link_libraries(gtest_factor_imu ${PLUGIN_NAME} ${wolf_LIBRARY}) +# target_link_libraries(gtest_factor_imu ${PLUGIN_NAME}) wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_intrinsics_update.cpp) -target_link_libraries(gtest_processor_motion_intrinsics_update ${PLUGIN_NAME} ${wolf_LIBRARY}) +target_link_libraries(gtest_processor_motion_intrinsics_update ${PLUGIN_NAME}) wolf_add_gtest(gtest_sensor_compass gtest_sensor_compass.cpp) -target_link_libraries(gtest_sensor_compass ${PLUGIN_NAME} ${wolf_LIBRARY}) \ No newline at end of file +target_link_libraries(gtest_sensor_compass ${PLUGIN_NAME}) diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp index 140792d538ab5ae568ba7743fbc49bce0bb6d9f2..c73292612597de2fb22c677ffd5bedf92c62daa4 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_compass_3d.cpp b/test/gtest_factor_compass_3d.cpp index f438aa03fbc5c18ed3d6f0d87c26ed568c535a6d..c368b7691e2012691ab2ec5c35dbe6bdb251b640 100644 --- a/test/gtest_factor_compass_3d.cpp +++ b/test/gtest_factor_compass_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 "imu/internal/config.h" diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp index c22ac6eafd00cb4c0cf480f70ffa64c0c8a88676..b6fe81776f2fb7db6ef7aaba0e4f130d7a6c2637 100644 --- a/test/gtest_factor_imu.cpp +++ b/test/gtest_factor_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu.cpp * @@ -89,7 +110,7 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test expected_final_state = x_origin; //null bias + static //set origin of the problem - KF0 = problem->setPrior(x_origin, P_origin, t, 0.005); + KF0 = problem->setPrior(x_origin, P_origin, t); // KF0 = processor_imu->setOrigin(x_origin, t); //===================================================== END{INITIALIZATION} @@ -1220,7 +1241,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test Eigen::Quaterniond current_quatState(Eigen::Quaterniond::Identity()); //set origin of the problem - origin_KF = problem->setPrior(x_origin, P_origin, t, 0.005); + origin_KF = problem->setPrior(x_origin, P_origin, t); // origin_KF = processor_imu->setOrigin(x_origin, t); // processor_odom3d->setOrigin(origin_KF); diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp new file mode 100644 index 0000000000000000000000000000000000000000..7dd71060608bd87f8b3188f9827f213d181322e6 --- /dev/null +++ b/test/gtest_factor_imu2d.cpp @@ -0,0 +1,446 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- +#include <core/ceres_wrapper/solver_ceres.h> +#include <core/utils/utils_gtest.h> + +#include "imu/factor/factor_imu2d.h" +#include "imu/math/imu2d_tools.h" +#include "imu/sensor/sensor_imu2d.h" + +using namespace Eigen; +using namespace wolf; + +// Input odometry data and covariance +Matrix6d data_cov = 0.1*Matrix6d::Identity(); +Matrix5d delta_cov = 0.1*Matrix5d::Identity(); + +// Jacobian of the bias +MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0, + 0, 1, 0, + 0, 0, 1, + 1, 0, 0, + 0, 1, 0 ).finished()); +//preintegration bias +Vector3d b0_preint = Vector3d::Zero(); + +// Problem and solver +ProblemPtr problem_ptr = Problem::create("POV", 2); +SolverCeres solver(problem_ptr); + +// Two frames +FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero()); +FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero()); + +// Imu2d sensor +SensorBasePtr sensor = std::make_shared<SensorImu2d>( Vector3d::Zero(), ParamsSensorImu2d() ); // default params: see sensor_imu2d.h + +//capture from frm1 to frm0 +auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr); +auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0); + +TEST(FactorImu2d, check_tree) +{ + ASSERT_TRUE(problem_ptr->check(0)); +} + +TEST(FactorImu2d, bias_zero_solve_f1) +{ + for(int i = 0; i < 50; i++){ + // Random delta + Vector5d delta = Vector5d::Random() * 10; //delta *= 0; + delta(2) = pi2pi(delta(2)); + + // Random initial pose + Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0; + x0(2) = pi2pi(x0(2)); + + // x1 groundtruth + Vector5d x1; + x1 = imu2d::compose(x0, delta, 1); + + // Set states + frm0->setState(x0); + frm1->setState(x1); + + // feature & factor with delta measurement + auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1); + FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false); + + // Fix frm0 and biases, perturb frm1 + frm0->fix(); + cap0->fix(); + frm1->unfix(); + cap1->fix(); + frm1->perturb(0.01); + + // solve + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + + ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); + //WOLF_INFO(frm1->getStateVector()); + + // remove feature (and factor) for the next loop + fea1->remove(); +} +} + +TEST(FactorImu2d, bias_zero_solve_f0) +{ + for(int i = 0; i < 50; i++){ + // Random delta + Vector5d delta = Vector5d::Random() * 10; //delta *= 0; + delta(2) = pi2pi(delta(2)); + + // Random initial pose + Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0; + x0(2) = pi2pi(x0(2)); + + // x1 groundtruth + Vector5d x1; + x1 = imu2d::compose(x0, delta, 1); + + // Set states + frm0->setState(x0); + frm1->setState(x1); + + // feature & factor with delta measurement + auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1); + FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false); + + // Fix frm0 and biases, perturb frm1 + frm0->unfix(); + cap0->fix(); + frm1->fix(); + cap1->fix(); + frm0->perturb(0.01); + + // solve + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + + ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); + //WOLF_INFO(frm1->getStateVector()); + + // remove feature (and factor) for the next loop + fea1->remove(); +} +} + +TEST(FactorImu2d, bias_zero_solve_b1) +{ + for(int i = 0; i < 50; i++){ + // Random delta + Vector5d delta = Vector5d::Random() * 10; //delta *= 0; + delta(2) = pi2pi(delta(2)); + + // Random initial pose + Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0; + x0(2) = pi2pi(x0(2)); + + // x1 groundtruth + Vector5d x1; + x1 = imu2d::compose(x0, delta, 1); + + //0 Initial bias + Vector3d b0 = Vector3d::Zero(); + + // Set states + frm0->setState(x0); + frm1->setState(x1); + cap0->getStateBlock('I')->setState(b0); + cap1->getStateBlock('I')->setState(b0); + + // feature & factor with delta measurement + auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1); + FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false); + + // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1 + frm0->fix(); + cap0->fix(); + frm1->fix(); + cap1->unfix(); + cap1->perturb(0.01); + + // solve to estimate bias at cap1 + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + + ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6); + //WOLF_INFO(cap1->getStateVector()); + + // remove feature (and factor) for the next loop + fea1->remove(); +} +} + +TEST(FactorImu2d, solve_b0) +{ + for(int i = 0; i < 50; i++){ + // Random delta + Vector5d delta_biased = Vector5d::Random() * 10; + delta_biased(2) = pi2pi(delta_biased(2)); + + // Random initial pose + Vector5d x0 = Vector5d::Random() * 10; + x0(2) = pi2pi(x0(2)); + + //Random Initial bias + Vector3d b0 = Vector3d::Random(); + + //Corrected delta + Vector5d delta_step = jacobian_bias*(b0-b0_preint); + Vector5d delta = imu2d::plus(delta_biased, delta_step); + + // x1 groundtruth + Vector5d x1; + x1 = imu2d::compose(x0, delta, 1); + + // Set states + frm0->setState(x0); + frm1->setState(x1); + cap0->getStateBlock('I')->setState(b0); + cap1->getStateBlock('I')->setState(b0); + + // feature & factor with delta measurement + auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias); + FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false); + + // Fix frm1, perturb frm0 + frm0->fix(); + cap0->unfix(); + frm1->fix(); + cap1->fix(); + cap0->perturb(0.1); + + // solve + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + + ASSERT_POSE2d_APPROX(cap0->getStateVector(), b0, 1e-6); + //WOLF_INFO(cap0->getStateVector()); + + // remove feature (and factor) for the next loop + fea1->remove(); +} +} + +TEST(FactorImu2d, solve_b1) +{ + for(int i = 0; i < 50; i++){ + // Random delta + Vector5d delta = Vector5d::Random() * 10; //delta *= 0; + delta(2) = pi2pi(delta(2)); + + // Random initial pose + Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0; + x0(2) = pi2pi(x0(2)); + + // x1 groundtruth + Vector5d x1; + x1 = imu2d::compose(x0, delta, 1); + + //0 Initial bias + Vector3d b0 = Vector3d::Random(); + + // Set states + frm0->setState(x0); + frm1->setState(x1); + cap0->getStateBlock('I')->setState(b0); + cap1->getStateBlock('I')->setState(b0); + + // feature & factor with delta measurement + auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1); + FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false); + + // Fix frm0 and frm1, unfix bias, perturb cap1 + frm0->fix(); + cap0->fix(); + frm1->fix(); + cap1->unfix(); + cap1->perturb(0.01); + + // solve to estimate bias at cap1 + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + + ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6); + //WOLF_INFO(cap1->getStateVector()); + + // remove feature (and factor) for the next loop + fea1->remove(); +} +} + +TEST(FactorImu2d, solve_f0) +{ + for(int i = 0; i < 50; i++){ + // Random delta + Vector5d delta_biased = Vector5d::Random() * 10; + delta_biased(2) = pi2pi(delta_biased(2)); + + // Random initial pose + Vector5d x0 = Vector5d::Random() * 10; + x0(2) = pi2pi(x0(2)); + + //Random Initial bias + Vector3d b0 = Vector3d::Random(); + + //Corrected delta + Vector5d delta_step = jacobian_bias*(b0-b0_preint); + Vector5d delta = imu2d::plus(delta_biased, delta_step); + + // x1 groundtruth + Vector5d x1; + x1 = imu2d::compose(x0, delta, 1); + + // Set states + frm0->setState(x0); + frm1->setState(x1); + cap0->getStateBlock('I')->setState(b0); + cap1->getStateBlock('I')->setState(b0); + + // feature & factor with delta measurement + auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias); + FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false); + + // Fix frm1, perturb frm0 + frm0->unfix(); + cap0->fix(); + frm1->fix(); + cap1->fix(); + frm0->perturb(0.1); + + // solve + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + + ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6); + //WOLF_INFO(frm0->getStateVector()); + + // remove feature (and factor) for the next loop + fea1->remove(); +} +} + +TEST(FactorImu2d, solve_f1) +{ + for(int i = 0; i < 50; i++){ + // Random delta + Vector5d delta_biased = Vector5d::Random() * 10; + delta_biased(2) = pi2pi(delta_biased(2)); + + // Random initial pose + Vector5d x0 = Vector5d::Random() * 10; + x0(2) = pi2pi(x0(2)); + + //Random Initial bias + Vector3d b0 = Vector3d::Random(); + + //Corrected delta + Vector5d delta_step = jacobian_bias*(b0-b0_preint); + Vector5d delta = imu2d::plus(delta_biased, delta_step); + + // x1 groundtruth + Vector5d x1; + x1 = imu2d::compose(x0, delta, 1); + + // Set states + frm0->setState(x0); + frm1->setState(x1); + cap0->getStateBlock('I')->setState(b0); + cap1->getStateBlock('I')->setState(b0); + + // feature & factor with delta measurement + auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias); + FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false); + + // Fix frm1, perturb frm0 + frm0->fix(); + cap0->fix(); + frm1->unfix(); + cap1->fix(); + frm1->perturb(0.1); + + // solve + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + + ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); + //WOLF_INFO(frm0->getStateVector()); + + // remove feature (and factor) for the next loop + fea1->remove(); +} +} + +TEST(FactorImu2d, solve_f1_b1) +{ + for(int i = 0; i < 50; i++){ + // Random delta + Vector5d delta_biased = Vector5d::Random() * 10; + delta_biased(2) = pi2pi(delta_biased(2)); + + // Random initial pose + Vector5d x0 = Vector5d::Random() * 10; + x0(2) = pi2pi(x0(2)); + + //Random Initial bias + Vector3d b0 = Vector3d::Random(); + + //Corrected delta + Vector5d delta_step = jacobian_bias*(b0-b0_preint); + Vector5d delta = imu2d::plus(delta_biased, delta_step); + + // x1 groundtruth + Vector5d x1; + x1 = imu2d::compose(x0, delta, 1); + + // Set states + frm0->setState(x0); + frm1->setState(x1); + cap0->getStateBlock('I')->setState(b0); + cap1->getStateBlock('I')->setState(b0); + + // feature & factor with delta measurement + auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias); + FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false); + + // Fix frm1, perturb frm0 + frm0->fix(); + cap0->fix(); + frm1->unfix(); + cap1->unfix(); + frm1->perturb(0.1); + cap1->perturb(0.1); + + // solve + std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF); + + ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6); + ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6); + //WOLF_INFO(frm0->getStateVector()); + + // remove feature (and factor) for the next loop + fea1->remove(); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + // ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_feature_imu.cpp b/test/gtest_feature_imu.cpp index ef63025f2b0db475bae9ceba850f960755570e08..d634807aec2b5b6b4f4243ea106c6b5363ffa10f 100644 --- a/test/gtest_feature_imu.cpp +++ b/test/gtest_feature_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 "imu/capture/capture_imu.h" #include "imu/processor/processor_imu.h" @@ -57,7 +78,7 @@ class FeatureImu_test : public testing::Test // Set the origin VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - origin_frame = problem->setPriorFactor(x0, s0, t, 0.01); + origin_frame = problem->setPriorFactor(x0, s0, t); processor_motion_ptr_->setOrigin(origin_frame); // Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.) @@ -68,7 +89,7 @@ class FeatureImu_test : public testing::Test sensor_ptr, data_, Eigen::Matrix6d::Identity(), - Eigen::Vector6d::Zero()) ); + Eigen::Vector6d::Zero().eval()) ); //process data data_ << 2, 0, 9.8, 0, 0, 0; diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp index 161157b8e079a5dd5f3dd4c873e754e021003c6f..6e63ec0bba182b5b0e163ed3c7ff094896f4a296 100644 --- a/test/gtest_imu.cpp +++ b/test/gtest_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_Imu.cpp * @@ -328,7 +349,7 @@ class Process_Factor_Imu : public testing::Test // wolf objects WOLF_INFO("x0c: ", x0c); WOLF_INFO("s0c: ", s0c); - KF_0 = problem->setPriorFactor(x0c, s0c, t0, dt/2); + KF_0 = problem->setPriorFactor(x0c, s0c, t0); processor_imu->setOrigin(KF_0); WOLF_INFO("prior set"); @@ -475,28 +496,10 @@ class Process_Factor_Imu : public testing::Test { // This perturbs states to estimate around the exact value, then assigns to the keyframe // Perturbations are applied only if the state block is unfixed - - VectorXd x_pert(10); - - // KF 0 - x_pert = x0; - if (!p0_fixed) - x_pert.head(3) += Vector3d::Random() * 0.01; - if (!q0_fixed) - x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized(); - if (!v0_fixed) - x_pert.tail(3) += Vector3d::Random() * 0.01; - KF_0->setState(x_pert); - - // KF 1 - x_pert = x1_exact; - if (!p1_fixed) - x_pert.head(3) += Vector3d::Random() * 0.01; - if (!q1_fixed) - x_pert.segment(3,4) = (Quaterniond(x_pert.data() + 3) * exp_q(Vector3d::Random() * 0.01)).coeffs().normalized(); - if (!v1_fixed) - x_pert.tail(3) += Vector3d::Random() * 0.01; - KF_1->setState(x_pert); + KF_0->setState(x0); + KF_0->perturb(); + KF_1->setState(x1_exact); + KF_1->perturb(); } virtual void buildProblem() @@ -505,7 +508,7 @@ class Process_Factor_Imu : public testing::Test FrameBasePtr KF = problem->emplaceFrame(t, x1_exact); // ===================================== Imu CALLBACK - problem->keyFrameCallback(KF, nullptr, dt/2); + problem->keyFrameCallback(KF, nullptr); // Process Imu for the callback to take effect data = Vector6d::Zero(); diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ddf1624c4a9c7c9d44c6ce43c35d8f975c8d2494 --- /dev/null +++ b/test/gtest_imu2d_static_init.cpp @@ -0,0 +1,555 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu2d_static_init.cpp + * + * Created on: Sept 2021 + * Author: igeer + */ + +#include <fstream> +#include <core/ceres_wrapper/solver_ceres.h> +#include <core/utils/utils_gtest.h> +#include "imu/internal/config.h" + +#include "imu/factor/factor_imu2d.h" +#include "imu/math/imu2d_tools.h" +#include "imu/sensor/sensor_imu2d.h" +#include "core/capture/capture_void.h" +#include <core/factor/factor_relative_pose_2d.h> + +using namespace Eigen; +using namespace wolf; + +class ProcessorImu2dStaticInitTest : public testing::Test +{ + + public: //These can be accessed in fixtures + wolf::ProblemPtr problem_ptr_; + wolf::SensorBasePtr sensor_ptr_; + wolf::ProcessorMotionPtr processor_motion_; + wolf::TimeStamp t; + wolf::TimeStamp t0; + double dt; + Vector6d data; + Matrix6d data_cov; + FrameBasePtr KF0_; + FrameBasePtr first_frame_; + FrameBasePtr last_frame_; + SolverCeresPtr solver_; + + //a new of this will be created for each new test + void SetUp() override + { + WOLF_INFO("Doing setup..."); + using namespace Eigen; + using std::shared_ptr; + using std::make_shared; + using std::static_pointer_cast; + using namespace wolf::Constants; + + std::string wolf_root = _WOLF_IMU_ROOT_DIR; + + // Wolf problem + problem_ptr_ = Problem::create("POV", 2); + Vector3d extrinsics = (Vector3d() << 0,0,0).finished(); + sensor_ptr_ = problem_ptr_->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/test/yaml/sensor_imu2d_static_init.yaml"); + ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu2d_static_init.yaml"); + processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr); + + // Time and data variables + dt = 0.1; + t0.set(0); + t = t0; + data = Vector6d::Random(); + data_cov = Matrix6d::Identity(); + last_frame_ = nullptr; + first_frame_ = nullptr; + + // Create the solver + solver_ = make_shared<SolverCeres>(problem_ptr_); + + // Set the origin + VectorComposite x_origin("POV", {Vector2d::Zero(), Vector1d::Zero(), Vector2d::Zero()}); + VectorComposite std_origin("POV", {0.001*Vector2d::Ones(), 0.001*Vector1d::Ones(), 0.001*Vector2d::Ones()}); + //KF0_ = problem_ptr_->setPriorFix(x_origin, t0); + KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0); + + } + + void TestStatic(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol) + { + //Data + data.head(2) = body_magnitudes.head(2); + data(5) = body_magnitudes(2); + data.head(2) += bias_groundtruth.head(2); + data(5) += bias_groundtruth(2); + + //Set bias initial guess + sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess); + processor_motion_->setOrigin(KF0_); + +#if WRITE_CSV_FILE + std::fstream file_est; + file_est.open("./est2d-"+test_name+".csv", std::fstream::out); + // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n"; + std::string header_est; + if(testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n"; + if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n"; + //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n"; + if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n"; + file_est << header_est; +#endif + + + int n_frames = 0; + for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){ + WOLF_INFO("\n------------------------------------------------------------------------"); + + //Create and process capture + auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front()); + C->process(); + + auto state = problem_ptr_->getState(); + auto bias_est = sensor_ptr_->getIntrinsic()->getState(); + auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + +#if WRITE_CSV_FILE + // pre-solve print to CSV + if(testing_var == 3){ + file_est << std::fixed << t << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'] << ";" + << bias_est.head(2).norm() << ";" + << bias_est(2) << ";" + << bias_preint.head(2).norm() << ";" + << bias_preint(2) << "\n"; + } + else + { + file_est << std::fixed << t << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'] << ";" + << bias_est(testing_var) << ";" + << bias_est(2) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(2) << "\n"; + + } +#endif + + // new frame + if (last_frame_ != processor_motion_->getOrigin()->getFrame()) + { + n_frames++; + last_frame_ = processor_motion_->getOrigin()->getFrame(); + + // impose static + last_frame_->getP()->setState(KF0_->getP()->getState()); + last_frame_->getO()->setState(KF0_->getO()->getState()); + last_frame_->getV()->setZero(); + + //Fix frame + last_frame_->fix(); + + //Solve + if(n_frames > 0) + { + std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO("Solver Report: ", report); + + state = problem_ptr_->getState(); + bias_est = sensor_ptr_->getIntrinsic()->getState(); + bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + + WOLF_INFO("The current problem is:"); + problem_ptr_->print(4); + +#if WRITE_CSV_FILE + // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result + if(testing_var == 3){ + file_est << std::fixed << t+dt/2 << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'] << ";" + << bias_est.head(2).norm() << ";" + << bias_est(2) << ";" + << bias_preint.head(2).norm() << ";" + << bias_preint(2) << "\n"; + } + else + { + file_est << std::fixed << t+dt/2 << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'] << ";" + << bias_est(testing_var) << ";" + << bias_est(2) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(2) << "\n"; + + } +#endif + } + + } + + + WOLF_INFO("Number of frames is ", n_frames); + WOLF_INFO("The state is: ", state); + WOLF_INFO("The estimated bias is: ", bias_est.transpose()); + WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose()); + if(small_tol) + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6); + } + } + else + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3); + } + } + } + +#if WRITE_CSV_FILE + file_est.close(); +#endif + + } + + void TestStaticZeroOdom(const Vector3d& body_magnitudes, const Vector3d& bias_groundtruth, const Vector3d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol) + { + //Data + data.head(2) = body_magnitudes.head(2); + data(5) = body_magnitudes(2); + data.head(2) += bias_groundtruth.head(2); + data(5) += bias_groundtruth(2); + + //Set bias initial guess + sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess); + processor_motion_->setOrigin(KF0_); + +#if WRITE_CSV_FILE + std::fstream file_est; + file_est.open("./est2dzeroodom-"+test_name+".csv", std::fstream::out); + // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n"; + std::string header_est; + if(testing_var == 0) header_est = "t;px;vx;o;bax_est;bg_est;bax_preint;bg_preint\n"; + if(testing_var == 1) header_est = "t;py;vy;o;bay_est;bg_est;bay_preint;bg_preint\n"; + //if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n"; + if(testing_var == 3) header_est = "t;pnorm;vnorm;o;banorm_est;bg_est;banorm_preint;bg_preint\n"; + file_est << header_est; +#endif + + int n_frames = 0; + for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){ + WOLF_INFO("\n------------------------------------------------------------------------"); + + //Create and process capture + auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front()); + C->process(); + + auto state = problem_ptr_->getState(); + auto bias_est = sensor_ptr_->getIntrinsic()->getState(); + auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + +#if WRITE_CSV_FILE + // pre-solve print to CSV + if(testing_var == 3){ + file_est << std::fixed << t << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'] << ";" + << bias_est.head(2).norm() << ";" + << bias_est(2) << ";" + << bias_preint.head(2).norm() << ";" + << bias_preint(2) << "\n"; + } + else + { + file_est << std::fixed << t << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'] << ";" + << bias_est(testing_var) << ";" + << bias_est(2) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(2) << "\n"; + + } +#endif + + // new frame + if (last_frame_ != processor_motion_->getOrigin()->getFrame()) + { + n_frames++; + last_frame_ = processor_motion_->getOrigin()->getFrame(); + + // impose zero odometry + processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure())); + + // add zero displacement and rotation capture & feature & factor with all previous frames + assert(sensor_ptr_->getProblem()); + for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin(); + frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end(); + frm_pair++) + { + if (frm_pair->second == last_frame_) + break; + auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr); + + auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero, + "FeatureZeroOdom", + Vector3d::Zero(), + Eigen::MatrixXd::Identity(3,3) * 0.01); + + FactorBase::emplace<FactorRelativePose2d>(feature_zero, + feature_zero, + frm_pair->second, + nullptr, + false, + TOP_MOTION); + + } + + // impose static + last_frame_->getV()->setZero(); + + //Fix frame + last_frame_->getV()->fix(); + + //Solve + if(n_frames > 0) + { + std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO("Solver Report: ", report); + + state = problem_ptr_->getState(); + bias_est = sensor_ptr_->getIntrinsic()->getState(); + bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + + WOLF_INFO("The current problem is:"); + problem_ptr_->print(4); + +#if WRITE_CSV_FILE + // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result + if(testing_var == 3){ + file_est << std::fixed << t+dt/2 << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'] << ";" + << bias_est.head(2).norm() << ";" + << bias_est(2) << ";" + << bias_preint.head(2).norm() << ";" + << bias_preint(2) << "\n"; + } + else + { + file_est << std::fixed << t+dt/2 << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'] << ";" + << bias_est(testing_var) << ";" + << bias_est(2) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(2) << "\n"; + + } +#endif + } + + } + + + + WOLF_INFO("Number of frames is ", n_frames); + WOLF_INFO("The state is: ", state); + WOLF_INFO("The estimated bias is: ", bias_est.transpose()); + WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose()); + if(small_tol) + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6); + } + } + else + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3); + } + } + } + +#if WRITE_CSV_FILE + file_est.close(); +#endif + + } +}; + + + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << 0.42, 0, 0).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = (Vector3d() << 0.42, 0, 0).finished(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << 0, 0, 0.01).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = (Vector3d() << 0, 0, 0.01).finished(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); +} + + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_a_random) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = Vector3d::Random()*100; + bias_initial_guess(2) = 0; + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true); +} + +TEST_F(ProcessorImu2dStaticInitTest, static_bias_zero_initial_guess_random) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = Vector3d::Zero(); + Vector3d bias_initial_guess = Vector3d::Random()*0.01; + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); +} + +TEST_F(ProcessorImu2dStaticInitTest, realistic_test) +{ + Vector3d body_magnitudes = Vector3d::Zero(); + Vector3d bias_groundtruth = (Vector3d() << -0.529550648247, + 0.278316717683, + -0.00122491355676).finished(); + Vector3d bias_initial_guess = Vector3d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + // ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_imu2d_tools.cpp b/test/gtest_imu2d_tools.cpp new file mode 100644 index 0000000000000000000000000000000000000000..2386b7c945a9fba5782dea5cd6fc09da7d89468c --- /dev/null +++ b/test/gtest_imu2d_tools.cpp @@ -0,0 +1,669 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu2d_tools.cpp + * + * Created on: Nov 17, 2020 + * Author: igeer + */ + +#include "imu/math/imu2d_tools.h" +#include <core/utils/utils_gtest.h> + +using namespace Eigen; +using namespace wolf; +using namespace imu2d; + +TEST(Imu2d_tools, identity) +{ + VectorXd id_true; + id_true.setZero(5); + + VectorXd id = identity<double>(); + ASSERT_MATRIX_APPROX(id, id_true, 1e-10); +} + +//TEST(Imu2d_tools, identity_split) +//{ +// VectorXd p(3), qv(4), v(3); +// Quaterniond q; +// +// identity(p,q,v); +// ASSERT_MATRIX_APPROX(p, Vector3d::Zero(), 1e-10); +// ASSERT_QUATERNION_APPROX(q, Quaterniond::Identity(), 1e-10); +// ASSERT_MATRIX_APPROX(v, Vector3d::Zero(), 1e-10); +// +// identity(p,qv,v); +// ASSERT_MATRIX_APPROX(p , Vector3d::Zero(), 1e-10); +// ASSERT_MATRIX_APPROX(qv, (Vector4d()<<0,0,0,1).finished(), 1e-10); +// ASSERT_MATRIX_APPROX(v , Vector3d::Zero(), 1e-10); +//} + +TEST(Imu2d_tools, inverse) +{ + VectorXd d(5), id(5), iid(5), iiid(5), I(5); + double dt = 0.1; + + d << 0, 1, 2, 3, 4; + + id = inverse(d, dt); + + compose(id, d, dt, I); + ASSERT_MATRIX_APPROX(I, imu2d::identity(), 1e-10); + compose(d, id, -dt, I); // Observe -dt is reversed !! + ASSERT_MATRIX_APPROX(I, imu2d::identity(), 1e-10); + + inverse(id, -dt, iid); // Observe -dt is reversed !! + ASSERT_MATRIX_APPROX( d, iid, 1e-10); + iiid = inverse(iid, dt); + ASSERT_MATRIX_APPROX(id, iiid, 1e-10); +} + +TEST(Imu2d_tools, compose){ + //Compose 2 vectors + VectorXd dx1(5), dx2(5), dx3(5); + Matrix<double, 5, 1> d1, d2, d3; + double dt = 0.001; + dx1 << 1, 2, 0.78539816339, 3, 4; + d1 = dx1; + dx2 << 5, 6, 0.39269908169, 7, 8; + d2 = dx2; + + //base function + Rotation2D<double> dR1(dx1(2)); + Vector2d dp2_rot = dR1*dx2.head(2); + Vector2d dv2_rot = dR1*dx2.tail(2); + Vector2d cdp = dx1.head(2) + dp2_rot + dx1.tail(2)*dt; + Vector2d cdv = dx1.tail(2) + dv2_rot; + dx3 << cdp, + dx1(2) + dx2(2), + cdv; + VectorXd composedmatrix = compose(dx1, dx2, dt); + ASSERT_MATRIX_APPROX(dx3, composedmatrix, 1e-10); + + //// combinations of templates for sum() + compose(dx1, dx2, dt, dx3); + compose(d1, d2, dt, d3); + ASSERT_MATRIX_APPROX(d3, dx3, 1e-10); + + compose(d1, dx2, dt, dx3); + d3 = compose(dx1, d2, dt); + ASSERT_MATRIX_APPROX(d3, dx3, 1e-10); + +} + +TEST(Imu2d_tools, compose_between) +{ + VectorXd dx1(5), dx2(5), dx3(5); + Matrix<double, 5, 1> d1, d2, d3; + double dt = 0.1; + + dx1 << 0, 1, pi2pi(2), 3, 4; + d1 = dx1; + dx2 << 5, 6, pi2pi(7), 8, 9; + d2 = dx2; + compose(dx1, dx2, dt, dx3); + compose(d1, d2, dt, d3); + + // minus(d1, d3) should recover delta_2 + VectorXd diffx(5); + Matrix<double,5,1> diff; + + between(dx1, dx3, dt, diffx); + ASSERT_MATRIX_APPROX(diffx, dx2, 1e-10); + between(d1, d3, dt, diff); + ASSERT_MATRIX_APPROX(diff, d2, 1e-10); + + // minus(d3, d1, -dt) should recover inverse(d2, dt) + diff = between(d3, d1, -dt); + ASSERT_MATRIX_APPROX(diff, inverse(d2, dt), 1e-10); + diffx = between(dx3, dx1, -dt); + ASSERT_MATRIX_APPROX(diffx, inverse(dx2, dt), 1e-10); +} + +//TEST(Imu2d_tools, compose_between_with_state) +//{ +// VectorXd x(10), d(10), x2(10), x3(10), d2(10), d3(10); +// Vector4d qv; +// double dt = 0.1; +// +// qv = (Vector4d() << 3, 4, 5, 6).finished().normalized(); +// x << 0, 1, 2, qv, 7, 8, 9; +// qv = (Vector4d() << 6, 5, 4, 3).finished().normalized(); +// d << 9, 8, 7, qv, 2, 1, 0; +// +// composeOverState(x, d, dt, x2); +// x3 = composeOverState(x, d, dt); +// ASSERT_MATRIX_APPROX(x3, x2, 1e-10); +// +// // betweenStates(x, x2) should recover d +// betweenStates(x, x2, dt, d2); +// d3 = betweenStates(x, x2, dt); +// ASSERT_MATRIX_APPROX(d2, d, 1e-10); +// ASSERT_MATRIX_APPROX(d3, d, 1e-10); +// ASSERT_MATRIX_APPROX(betweenStates(x, x2, dt), d, 1e-10); +// +// // x + (x2 - x) = x2 +// ASSERT_MATRIX_APPROX(composeOverState(x, betweenStates(x, x2, dt), dt), x2, 1e-10); +// +// // (x + d) - x = d +// ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d, dt), dt), d, 1e-10); +//} +// +//TEST(Imu2d_tools, lift_retract) +//{ +// VectorXd d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi +// +// // transform to delta +// VectorXd delta = exp_Imu(d_min); +// +// // expected delta +// Vector3d dp = d_min.head(3); +// Quaterniond dq = v2q(d_min.segment(3,3)); +// Vector3d dv = d_min.tail(3); +// VectorXd delta_true(10); delta_true << dp, dq.coeffs(), dv; +// ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10); +// +// // transform to a new tangent -- should be the original tangent +// VectorXd d_from_delta = log_Imu(delta); +// ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10); +// +// // transform to a new delta -- should be the previous delta +// VectorXd delta_from_d = exp_Imu(d_from_delta); +// ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10); +//} +// +TEST(Imu2d_tools, plus) +{ + VectorXd d1(5), d2(5), d3(5); + VectorXd err(5); + d1 << 0, 1, 2, 3, 4; + err << 0.01, 0.02, 0.03, 0.04, 0.05; + + WOLF_INFO("doing sums"); + d3.head(2) = d1.head(2) + err.head(2); + d3(2) = d1(2) + err(2); + d3.tail(2) = d1.tail(2) + err.tail(2); + + WOLF_INFO("doing plus()"); + imu2d::plus(d1, err, d2); + ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXd::Zero(5), 1e-10); +} + +TEST(Imu2d_tools, diff) +{ + VectorXd d1(5), d2(5); + d1 << 0, 1, 7, 8, 9; + d2 = d1; + + VectorXd err(5); + diff(d1, d2, err); + ASSERT_MATRIX_APPROX(err, VectorXd::Zero(5), 1e-10); + ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXd::Zero(5), 1e-10); + + VectorXd d3(5); + d3.setRandom(); + err.head(2) = d3.head(2) - d1.head(2); + err(2) = d3(2) - d1(2); + err.tail(2) = d3.tail(2) - d1.tail(2); + + ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10); + +} + +TEST(Imu2d_tools, compose_jacobians) +{ + VectorXd d1(5), d2(5), d3(5), d1_pert(5), d2_pert(5), d3_pert(5); // deltas + VectorXd t1(5), t2(5); // tangents + Matrix<double, 5, 5> J1_a, J2_a, J1_n, J2_n; + double dt = 0.1; + double dx = 1e-6; + d1 << 0, 1, pi2pi(2), 3, 4; + d2 << 5, 6, pi2pi(7), 8, 9; + + // analytical jacobians + compose(d1, d2, dt, d3, J1_a, J2_a); + + // numerical jacobians + for (unsigned int i = 0; i<5; i++) + { + t1 . setZero(); + t1(i) = dx; + + // Jac wrt first delta + d1_pert = imu2d::plus(d1, t1); // (d1 + t1) + d3_pert = compose(d1_pert, d2, dt); // (d1 + t1) + d2 + t2 = diff(d3, d3_pert); // { (d2 + t1) + d2 } - { d1 + d2 } + J1_n.col(i) = t2/dx; // [ { (d2 + t1) + d2 } - { d1 + d2 } ] / dx + + // Jac wrt second delta + d2_pert = imu2d::plus(d2, t1); // (d2 + t1) + d3_pert = compose(d1, d2_pert, dt); // d1 + (d2 + t1) + t2 = diff(d3, d3_pert); // { d1 + (d2 + t1) } - { d1 + d2 } + J2_n.col(i) = t2/dx; // [ { d1 + (d2 + t1) } - { d1 + d2 } ] / dx + } + + // check that numerical and analytical match + ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4); + ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4); +} + +//TEST(Imu2d_tools, diff_jacobians) +//{ +// //diff with jacobians is not implemented and doesn't seem necessary +// VectorXd d1(5), d2(5), d3(5), d1_pert(5), d2_pert(5), d3_pert(5); // deltas +// VectorXd t1(5), t2(5); // tangents +// Matrix<double, 5, 5> J1_a, J2_a, J1_n, J2_n; +// double dx = 1e-6; +// d1 << 0, 1, pi2pi(2), 3, 4; +// d2 << 5, 6, pi2pi(7), 8, 9; +// +// // analytical jacobians +// diff(d1, d2, d3, J1_a, J2_a); +// +// // numerical jacobians +// for (unsigned int i = 0; i<9; i++) +// { +// t1 . setZero(); +// t1(i) = dx; +// +// // Jac wrt first delta +// d1_pert = plus(d1, t1); // (d1 + t1) +// d3_pert = diff(d1_pert, d2); // d2 - (d1 + t1) +// t2 = d3_pert - d3; // { d2 - (d1 + t1) } - { d2 - d1 } +// J1_n.col(i) = t2/dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx +// +// // Jac wrt second delta +// d2_pert = plus(d2, t1); // (d2 + t1) +// d3_pert = diff(d1, d2_pert); // (d2 + t1) - d1 +// t2 = d3_pert - d3; // { (d2 + t1) - d1 } - { d2 - d1 } +// J2_n.col(i) = t2/dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx +// } +// +// // check that numerical and analytical match +// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4); +// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4); +//} + +TEST(Imu2d_tools, body2delta_jacobians) +{ + VectorXd delta(5), delta_pert(5); // deltas + VectorXd body(3), pert(3); + VectorXd tang(5); // tangents + Matrix<double, 5, 3> J_a, J_n; // analytic and numerical jacs + double dt = 0.01; + double dx = 1e-6; + body << -1, 1, 0; // jacobians not exact if w != 0 + + // analytical jacobians + body2delta(body, dt, delta, J_a); + + // numerical jacobians + for (unsigned int i = 0; i<3; i++) + { + pert . setZero(); + pert(i) = dx; + + // Jac wrt first delta + delta_pert = body2delta(body + pert, dt); // delta(body + pert) + tang = diff(delta, delta_pert); // delta(body + pert) -- delta(body) + J_n.col(i) = tang/dx; // ( delta(body + pert) -- delta(body) ) / dx + } + + // check that numerical and analytical match + ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4); +} + +//motion2data is not (yet) implemented + +//TEST(motion2data, zero) +//{ +// Vector6d motion; +// Vector6d bias; +// Quaterniond q; +// +// motion .setZero(); +// bias .setZero(); +// q .setIdentity(); +// +// Vector6d data = imu2d::motion2data(motion, q, bias); +// +// Vector6d data_true; data_true << -gravity(), Vector3d::Zero(); +// +// ASSERT_MATRIX_APPROX(data, data_true, 1e-12); +//} +// +//TEST(motion2data, motion) +//{ +// Vector6d motion, g_extended; +// Vector6d bias; +// Quaterniond q; +// +// g_extended << gravity() , Vector3d::Zero(); +// +// motion << 1,2,3, 4,5,6; +// bias .setZero(); +// q .setIdentity(); +// +// Vector6d data = imu2d::motion2data(motion, q, bias); +// +// Vector6d data_true; data_true = motion - g_extended; +// +// ASSERT_MATRIX_APPROX(data, data_true, 1e-12); +//} +// +//TEST(motion2data, bias) +//{ +// Vector6d motion, g_extended; +// Vector6d bias; +// Quaterniond q; +// +// g_extended << gravity() , Vector3d::Zero(); +// +// motion .setZero(); +// bias << 1,2,3, 4,5,6; +// q .setIdentity(); +// +// Vector6d data = imu2d::motion2data(motion, q, bias); +// +// Vector6d data_true; data_true = bias - g_extended; +// +// ASSERT_MATRIX_APPROX(data, data_true, 1e-12); +//} +// +//TEST(motion2data, orientation) +//{ +// Vector6d motion, g_extended; +// Vector6d bias; +// Quaterniond q; +// +// g_extended << gravity() , Vector3d::Zero(); +// +// motion .setZero(); +// bias .setZero(); +// q = v2q(Vector3d(M_PI/2, 0, 0)); // turn 90 deg in X axis +// +// Vector6d data = imu2d::motion2data(motion, q, bias); +// +// Vector6d data_true; data_true << 0,-gravity()(2),0, 0,0,0; +// +// ASSERT_MATRIX_APPROX(data, data_true, 1e-12); +//} +// +//TEST(motion2data, AllRandom) +//{ +// Vector6d motion, g_extended; +// Vector6d bias; +// Quaterniond q; +// +// motion .setRandom(); +// bias .setRandom(); +// q.coeffs() .setRandom().normalize(); +// +// g_extended << q.conjugate()*gravity() , Vector3d::Zero(); +// +// Vector6d data = imu2d::motion2data(motion, q, bias); +// +// Vector6d data_true; data_true = motion + bias - g_extended; +// +// ASSERT_MATRIX_APPROX(data, data_true, 1e-12); +//} +// +///* Integrate acc and angVel motion, obtain Delta_preintegrated +// * Input: +// * N: number of steps +// * q0: initial orientaiton +// * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame +// * bias_real: the real bias of the Imu +// * bias_preint: the bias used for Delta pre-integration +// * Output: +// * return: the preintegrated delta +// */ +//VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt) +//{ +// VectorXd data(6); +// VectorXd body(6); +// VectorXd delta(10); +// VectorXd Delta(10), Delta_plus(10); +// Delta << 0,0,0, 0,0,0,1, 0,0,0; +// Quaterniond q(q0); +// for (int n = 0; n<N; n++) +// { +// data = motion2data(motion, q, bias_real); +// q = q*exp_q(motion.tail(3)*dt); +// body = data - bias_preint; +// delta = body2delta(body, dt); +// Delta_plus = compose(Delta, delta, dt); +// Delta = Delta_plus; +// } +// return Delta; +//} +// +///* Integrate acc and angVel motion, obtain Delta_preintegrated +// * Input: +// * N: number of steps +// * q0: initial orientaiton +// * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame +// * bias_real: the real bias of the Imu +// * bias_preint: the bias used for Delta pre-integration +// * Output: +// * J_D_b: the Jacobian of the preintegrated delta wrt the bias +// * return: the preintegrated delta +// */ +//VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b) +//{ +// VectorXd data(6); +// VectorXd body(6); +// VectorXd delta(10); +// Matrix<double, 9, 6> J_d_d, J_d_b; +// Matrix<double, 9, 9> J_D_D, J_D_d; +// VectorXd Delta(10), Delta_plus(10); +// Quaterniond q; +// +// Delta << 0,0,0, 0,0,0,1, 0,0,0; +// J_D_b.setZero(); +// q = q0; +// for (int n = 0; n<N; n++) +// { +// // Simulate data +// data = motion2data(motion, q, bias_real); +// q = q*exp_q(motion.tail(3)*dt); +// // Motion::integrateOneStep() +// { // Imu::computeCurrentDelta +// body = data - bias_preint; +// body2delta(body, dt, delta, J_d_d); +// J_d_b = - J_d_d; +// } +// { // Imu::deltaPlusDelta +// compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d); +// } +// // Motion:: jac calib +// J_D_b = J_D_D*J_D_b + J_D_d*J_d_b; +// // Motion:: buffer +// Delta = Delta_plus; +// } +// return Delta; +//} +// +//TEST(Imu2d_tools, integral_jacobian_bias) +//{ +// VectorXd Delta(10), Delta_pert(10); // deltas +// VectorXd bias_real(6), pert(6), bias_pert(6), bias_preint(6); +// VectorXd body(6), data(6), motion(6); +// VectorXd tang(9); // tangents +// Matrix<double, 9, 6> J_a, J_n; // analytic and numerical jacs +// double dt = 0.001; +// double dx = 1e-4; +// Quaterniond q0(3, 4, 5, 6); q0.normalize(); +// motion << .1, .2, .3, .3, .2, .1; +// bias_real << .002, .004, .001, .003, .002, .001; +// bias_preint << .004, .005, .006, .001, .002, .003; +// +// int N = 500; // # steps of integration +// +// // Analytical Jacobians +// Delta = integrateDelta(N, q0, motion, bias_real, bias_preint, dt, J_a); +// +// // numerical jacobians +// for (unsigned int i = 0; i<6; i++) +// { +// pert . setZero(); +// pert(i) = dx; +// +// bias_pert = bias_preint + pert; +// Delta_pert = integrateDelta(N, q0, motion, bias_real, bias_pert, dt); +// tang = diff(Delta, Delta_pert); // Delta(bias + pert) -- Delta(bias) +// J_n.col(i) = tang/dx; // ( Delta(bias + pert) -- Delta(bias) ) / dx +// } +// +// // check that numerical and analytical match +// ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4); +//} +// +//TEST(Imu2d_tools, delta_correction) +//{ +// VectorXd Delta(10), Delta_preint(10), Delta_corr; // deltas +// VectorXd bias(6), pert(6), bias_preint(6); +// VectorXd body(6), motion(6); +// VectorXd tang(9); // tangents +// Matrix<double, 9, 6> J_b; // analytic and numerical jacs +// Vector4d qv;; +// double dt = 0.001; +// Quaterniond q0(3, 4, 5, 6); q0.normalize(); +// motion << 1, 2, 3, 3, 2, 1; motion *= .1; +// +// int N = 1000; // # steps of integration +// +// // preintegration with correct bias +// bias << .004, .005, .006, .001, .002, .003; +// Delta = integrateDelta(N, q0, motion, bias, bias, dt); +// +// // preintegration with wrong bias +// pert << .002, -.001, .003, -.0003, .0002, -.0001; +// bias_preint = bias + pert; +// Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b); +// +// // correct perturbated +// Vector9d step = J_b*(bias-bias_preint); +// Delta_corr = plus(Delta_preint, step); +// +// // Corrected delta should match real delta +// ASSERT_MATRIX_APPROX(Delta, Delta_corr, 1e-5); +// +// // diff between real and corrected should be zero +// ASSERT_MATRIX_APPROX(diff(Delta, Delta_corr), Vector9d::Zero(), 1e-5); +// +// // diff between preint and corrected deltas should be the jacobian-computed step +// ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5); +//} +// +//TEST(Imu2d_tools, full_delta_residual) +//{ +// VectorXd x1(10), x2(10); +// VectorXd Delta(10), Delta_preint(10), Delta_corr(10); +// VectorXd Delta_real(9), Delta_err(9), Delta_diff(10), Delta_exp(10); +// VectorXd bias(6), pert(6), bias_preint(6), bias_null(6); +// VectorXd body(6), motion(6); +// VectorXd tang(9); // tangents +// Matrix<double, 9, 6> J_b; // analytic and numerical jacs +// double dt = 0.001; +// Quaterniond q0; q0.setIdentity(); +// +// x1 << 0,0,0, 0,0,0,1, 0,0,0; +// motion << .3, .2, .1, .1, .2, .3; // acc and gyro +//// motion << .3, .2, .1, .0, .0, .0; // only acc +//// motion << .0, .0, .0, .1, .2, .3; // only gyro +// bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01; +// bias_null << 0, 0, 0, 0, 0, 0; +//// bias_preint << 0.003, 0.002, 0.001, 0.001, 0.002, 0.003; +// bias_preint = bias_null; +// +// int N = 1000; // # steps of integration +// +// // preintegration with no bias +// Delta_real = integrateDelta(N, q0, motion, bias_null, bias_null, dt); +// +// // final state +// x2 = composeOverState(x1, Delta_real, 1000*dt); +// +// // preintegration with the correct bias +// Delta = integrateDelta(N, q0, motion, bias, bias, dt); +// +// ASSERT_MATRIX_APPROX(Delta, Delta_real, 1e-4); +// +// // preintegration with wrong bias +// Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b); +// +// // compute correction step +// Vector9d step = J_b*(bias-bias_preint); +// +// // correct preintegrated delta +// Delta_corr = plus(Delta_preint, step); +// +// // Corrected delta should match real delta +// ASSERT_MATRIX_APPROX(Delta_real, Delta_corr, 1e-3); +// +// // diff between real and corrected should be zero +// ASSERT_MATRIX_APPROX(diff(Delta_real, Delta_corr), Vector9d::Zero(), 1e-3); +// +// // expected delta +// Delta_exp = betweenStates(x1, x2, N*dt); +// +// ASSERT_MATRIX_APPROX(Delta_exp, Delta_corr, 1e-3); +// +// // compute diff between expected and corrected +//// Matrix<double, 9, 9> J_err_exp, J_err_corr; +// diff(Delta_exp, Delta_corr, Delta_err); //, J_err_exp, J_err_corr); +// +// ASSERT_MATRIX_APPROX(Delta_err, Vector9d::Zero(), 1e-3); +// +// // compute error between expected and preintegrated +// Delta_err = diff(Delta_preint, Delta_exp); +// +// // diff between preint and corrected deltas should be the jacobian-computed step +// ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-3); +// /* This implies: +// * - Since D_corr is expected to be similar to D_exp +// * step ~ diff(Delta_preint, Delta_exp) +// * - the residual can be computed with a regular '-' : +// * residual = diff(D_preint, D_exp) - J_bias * (bias - bias_preint) +// */ +// +//// WOLF_TRACE("Delta real: ", Delta_real.transpose()); +//// WOLF_TRACE("x2: ", x2.transpose()); +//// WOLF_TRACE("Delta: ", Delta.transpose()); +//// WOLF_TRACE("Delta pre: ", Delta_preint.transpose()); +//// WOLF_TRACE("Jac bias: \n", J_b); +//// WOLF_TRACE("Delta step: ", step.transpose()); +//// WOLF_TRACE("Delta cor: ", Delta_corr.transpose()); +//// WOLF_TRACE("Delta exp: ", Delta_exp.transpose()); +//// WOLF_TRACE("Delta err: ", Delta_err.transpose()); +//// WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose()); +//} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); +// ::testing::GTEST_FLAG(filter) = "Imu2d_tools.delta_correction"; + return RUN_ALL_TESTS(); +} + diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp index bb668f2b85062e100ad80dbd92ce29411dd19458..34cf4948419a04b2b8f0648fe75b020495509b70 100644 --- a/test/gtest_imu_mocap_fusion.cpp +++ b/test/gtest_imu_mocap_fusion.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu_mocap_fusion.cpp * @@ -92,7 +113,7 @@ class ImuMocapFusion_Test : public testing::Test solver_->getSolverOptions().max_num_iterations = 500; // initial guess VectorComposite x_origin("POV", {w_p_wb, w_q_b.coeffs(), w_v_wb}); - FrameBasePtr KF1 = problem_->setPriorInitialGuess(x_origin, 0, 0.0005); // if mocap used + FrameBasePtr KF1 = problem_->setPriorInitialGuess(x_origin, 0); // if mocap used // pose sensor and proc (to get extrinsics in the prb) auto intr_sp = std::make_shared<ParamsSensorPose>(); @@ -279,4 +300,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_imu_static_init.cpp b/test/gtest_imu_static_init.cpp new file mode 100644 index 0000000000000000000000000000000000000000..e0c769b4a30a4d1dc122be4ebe88622634186e1a --- /dev/null +++ b/test/gtest_imu_static_init.cpp @@ -0,0 +1,554 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu_static_init.cpp + * + * Created on: Sept 2021 + * Author: igeer + */ +#include <fstream> + +#include <core/ceres_wrapper/solver_ceres.h> +#include <core/utils/utils_gtest.h> +#include "imu/internal/config.h" + +#include "imu/factor/factor_imu.h" +#include "imu/math/imu_tools.h" +#include "imu/sensor/sensor_imu.h" +#include "core/capture/capture_void.h" +#include <core/factor/factor_relative_pose_3d.h> + +using namespace Eigen; +using namespace wolf; + +/** + * SET TO TRUE TO PRODUCE CSV FILE + * SET TO FALSE TO STOP PRODUCING CSV FILE + */ +#define WRITE_CSV_FILE true + +class ProcessorImuStaticInitTest : public testing::Test +{ + + public: //These can be accessed in fixtures + wolf::ProblemPtr problem_ptr_; + wolf::SensorBasePtr sensor_ptr_; + wolf::ProcessorMotionPtr processor_motion_; + wolf::TimeStamp t; + wolf::TimeStamp t0; + double dt; + Vector6d data; + Matrix6d data_cov; + FrameBasePtr KF0_; + FrameBasePtr first_frame_; + FrameBasePtr last_frame_; + SolverCeresPtr solver_; + + //a new of this will be created for each new test + void SetUp() override + { + WOLF_INFO("Doing setup..."); + using namespace Eigen; + using std::shared_ptr; + using std::make_shared; + using std::static_pointer_cast; + using namespace wolf::Constants; + + std::string wolf_root = _WOLF_IMU_ROOT_DIR; + + // Wolf problem + problem_ptr_ = Problem::create("POV", 3); + Vector7d extrinsics = (Vector7d() << 0,0,0,0,0,0,1).finished(); + sensor_ptr_ = problem_ptr_->installSensor("SensorImu", "Main Imu", extrinsics, wolf_root + "/test/yaml/sensor_imu_static_init.yaml"); + ProcessorBasePtr processor_ptr = problem_ptr_->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/imu_static_init.yaml"); + processor_motion_ = std::static_pointer_cast<ProcessorMotion>(processor_ptr); + + // Time and data variables + dt = 0.1; + t0.set(0); + t = t0; + data = Vector6d::Random(); + data_cov = Matrix6d::Identity(); + last_frame_ = nullptr; + first_frame_ = nullptr; + + // Create the solver + solver_ = make_shared<SolverCeres>(problem_ptr_); + + // Set the origin + VectorComposite x0c("POV", {Vector3d::Zero(), (Vector4d() << 0,0,0,1).finished(), Vector3d::Zero()}); + WOLF_INFO("x0c is: \n", x0c); + //KF0_ = problem_ptr_->setPriorFix(x0c, t0); + + Vector4d q_init; q_init << 0,0,0,1; + VectorComposite x_origin("POV", {Vector3d::Zero(), q_init, Vector3d::Zero()}); + VectorComposite std_origin("POV", {0.001*Vector3d::Ones(), 0.001*Vector3d::Ones(), 0.001*Vector3d::Ones()}); + KF0_ = problem_ptr_->setPriorFactor(x_origin, std_origin, 0); + + } + + void TestStatic(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol) + { + //Data + data = body_magnitudes; + data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity(); + data += bias_groundtruth; + + //Set bias initial guess + sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess); + processor_motion_->setOrigin(KF0_); + +#if WRITE_CSV_FILE + std::fstream file_est; + file_est.open("./est-"+test_name+".csv", std::fstream::out); + // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n"; + std::string header_est; + if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n"; + if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n"; + if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n"; + if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n"; + file_est << header_est; +#endif + + + int n_frames = 0; + for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){ + WOLF_INFO("\n------------------------------------------------------------------------"); + + //Create and process capture + auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front()); + C->process(); + + auto state = problem_ptr_->getState(); + auto bias_est = sensor_ptr_->getIntrinsic()->getState(); + auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + +#if WRITE_CSV_FILE + // pre-solve print to CSV + if(testing_var == 3){ + file_est << std::fixed << t << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'].norm() << ";" + << bias_est.head(3).norm() << ";" + << bias_est.tail(3).norm() << ";" + << bias_preint.head(3).norm() << ";" + << bias_preint.tail(3).norm() << "\n"; + } + else + { + file_est << std::fixed << t << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'](testing_var) << ";" + << bias_est(testing_var) << ";" + << bias_est(testing_var+3) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(testing_var+3) << "\n"; + + } +#endif + + // new frame + if (last_frame_ != processor_motion_->getOrigin()->getFrame()) + { + n_frames++; + last_frame_ = processor_motion_->getOrigin()->getFrame(); + + // impose static + last_frame_->getP()->setState(KF0_->getP()->getState()); + last_frame_->getO()->setState(KF0_->getO()->getState()); + last_frame_->getV()->setZero(); + + //Fix frame + last_frame_->fix(); + + //Solve + if(n_frames > 0) + { + std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO("Solver Report: ", report); + + state = problem_ptr_->getState(); + bias_est = sensor_ptr_->getIntrinsic()->getState(); + bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + + WOLF_INFO("The current problem is:"); + problem_ptr_->print(4); + +#if WRITE_CSV_FILE + // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result + if(testing_var == 3){ + file_est << std::fixed << t+dt/2 << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'].norm() << ";" + << bias_est.head(3).norm() << ";" + << bias_est.tail(3).norm() << ";" + << bias_preint.head(3).norm() << ";" + << bias_preint.tail(3).norm() << "\n"; + } + else + { + file_est << std::fixed << t+dt/2 << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'](testing_var) << ";" + << bias_est(testing_var) << ";" + << bias_est(testing_var+3) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(testing_var+3) << "\n"; + + } +#endif + } + + } + + + + WOLF_INFO("Number of frames is ", n_frames); + WOLF_INFO("The state is: ", state); + WOLF_INFO("The estimated bias is: ", bias_est.transpose()); + WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose()); + if(small_tol) + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6); + } + } + else + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3); + } + } + } + +#if WRITE_CSV_FILE + file_est.close(); +#endif + + } + + void TestStaticZeroOdom(const Vector6d& body_magnitudes, const Vector6d& bias_groundtruth, const Vector6d& bias_initial_guess, const string& test_name, int testing_var, bool small_tol) + { + //Data + data = body_magnitudes; + data.head(3) -= Quaterniond(Vector4d(KF0_->getO()->getState())).conjugate() * wolf::gravity(); + data += bias_groundtruth; + + //Set bias initial guess + sensor_ptr_->getIntrinsic(t0)->setState(bias_initial_guess); + processor_motion_->setOrigin(KF0_); + +#if WRITE_CSV_FILE + std::fstream file_est; + file_est.open("./estzeroodom-"+test_name+".csv", std::fstream::out); + // std::string header_est = "t,px,py,pz,qx,qy,qz,qw,vx,vy,vz,bax_est,bax_preint\n"; + std::string header_est; + if(testing_var == 0) header_est = "t;px;vx;ox;bax_est;bgx_est;bax_preint;bgx_preint\n"; + if(testing_var == 1) header_est = "t;py;vy;oy;bay_est;bgy_est;bay_preint;bgy_preint\n"; + if(testing_var == 2) header_est = "t;pz;vz;oz:baz_est;bgz_est;baz_preint;bgz_preint\n"; + if(testing_var == 3) header_est = "t;pnorm;vnorm;onorm;banorm_est;bgnorm_est;banorm_preint;bgnorm_preint\n"; + file_est << header_est; +#endif + + + int n_frames = 0; + for(t = t0; t <= t0 + 9.9 + dt/2; t+=dt){ + WOLF_INFO("\n------------------------------------------------------------------------"); + + //Create and process capture + auto C = std::make_shared<CaptureImu>(t, sensor_ptr_, data, data_cov, KF0_->getCaptureList().front()); + C->process(); + + auto state = problem_ptr_->getState(); + auto bias_est = sensor_ptr_->getIntrinsic()->getState(); + auto bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + +#if WRITE_CSV_FILE + // pre-solve print to CSV + if(testing_var == 3){ + file_est << std::fixed << t << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'].norm() << ";" + << bias_est.head(3).norm() << ";" + << bias_est.tail(3).norm() << ";" + << bias_preint.head(3).norm() << ";" + << bias_preint.tail(3).norm() << "\n"; + } + else + { + file_est << std::fixed << t << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'](testing_var) << ";" + << bias_est(testing_var) << ";" + << bias_est(testing_var+3) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(testing_var+3) << "\n"; + + } +#endif + + // new frame + if (last_frame_ != processor_motion_->getOrigin()->getFrame()) + { + n_frames++; + last_frame_ = processor_motion_->getOrigin()->getFrame(); + + // impose zero odometry + processor_motion_->setOdometry(sensor_ptr_->getProblem()->stateZero(processor_motion_->getStateStructure())); + + // add zero displacement and rotation capture & feature & factor with all previous frames + assert(sensor_ptr_->getProblem()); + for (auto frm_pair = sensor_ptr_->getProblem()->getTrajectory()->begin(); + frm_pair != sensor_ptr_->getProblem()->getTrajectory()->end(); + frm_pair++) + { + if (frm_pair->second == last_frame_) + break; + auto capture_zero = CaptureBase::emplace<CaptureVoid>(last_frame_, last_frame_->getTimeStamp(), nullptr); + + auto feature_zero = FeatureBase::emplace<FeatureBase>(capture_zero, + "FeatureZeroOdom", + Vector7d::Zero(), + Eigen::MatrixXd::Identity(6,6) * 0.01); + + FactorBase::emplace<FactorRelativePose3d>(feature_zero, + feature_zero, + frm_pair->second, + nullptr, + false, + TOP_MOTION); + + } + + // impose static + last_frame_->getV()->setZero(); + + //Fix frame + last_frame_->getV()->fix(); + + //Solve + if(n_frames > 0) + { + std::string report = solver_->solve(SolverManager::ReportVerbosity::FULL); + //WOLF_INFO("Solver Report: ", report); + + state = problem_ptr_->getState(); + bias_est = sensor_ptr_->getIntrinsic()->getState(); + bias_preint = processor_motion_->getLast()->getCalibrationPreint(); + + WOLF_INFO("The current problem is:"); + problem_ptr_->print(4); + +#if WRITE_CSV_FILE + // post-solve print to CSV with time-stamp shifted by dt/2 to separate from pre-solve result + if(testing_var == 3){ + file_est << std::fixed << t+dt/2 << ";" + << state['P'].norm() << ";" + << state['V'].norm() << ";" + << state['O'].norm() << ";" + << bias_est.head(3).norm() << ";" + << bias_est.tail(3).norm() << ";" + << bias_preint.head(3).norm() << ";" + << bias_preint.tail(3).norm() << "\n"; + } + else + { + file_est << std::fixed << t+dt/2 << ";" + << state['P'](testing_var) << ";" + << state['V'](testing_var) << ";" + << state['O'](testing_var) << ";" + << bias_est(testing_var) << ";" + << bias_est(testing_var+3) << ";" + << bias_preint(testing_var) << ";" + << bias_preint(testing_var+3) << "\n"; + + } +#endif + } + + } + + + + WOLF_INFO("Number of frames is ", n_frames); + WOLF_INFO("The state is: ", state); + WOLF_INFO("The estimated bias is: ", bias_est.transpose()); + WOLF_INFO("The preintegrated bias is: ", bias_preint.transpose()); + if(small_tol) + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-6); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-6); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-6); + } + } + else + { + if(n_frames == 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + } + else if (n_frames > 2) + { + EXPECT_MATRIX_APPROX(state.vector("POV"), KF0_->getState().vector("POV"), 1e-3); + EXPECT_MATRIX_APPROX(bias_est, bias_groundtruth, 1e-3); + EXPECT_MATRIX_APPROX(bias_preint, bias_groundtruth, 1e-3); + } + } + } + +#if WRITE_CSV_FILE + file_est.close(); +#endif + + } +}; + + + +TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); + Vector6d bias_initial_guess = Vector6d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); + Vector6d bias_initial_guess = Vector6d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_aX_initial_guess_zero_zeroodom) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); + Vector6d bias_initial_guess = Vector6d::Zero(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_aX_initial_guess_zero", 0, true); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_aX_zeroodom) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = (Vector6d() << 0.42, 0, 0, 0, 0, 0).finished(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_aX", 0, true); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_gX_initial_guess_zero_zeroodom) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); + Vector6d bias_initial_guess = Vector6d::Zero(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_gX_initial_guess_zero", 0, false); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_gX_zeroodom) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = (Vector6d() << 0, 0, 0, 0.01, 0, 0).finished(); + + TestStaticZeroOdom(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_gX", 0, false); +} + + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_a_random) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = Vector6d::Random()*100; + bias_initial_guess.tail(3) = Vector3d::Zero(); + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_a_random", 3, true); +} + +TEST_F(ProcessorImuStaticInitTest, static_bias_zero_initial_guess_random) +{ + Vector6d body_magnitudes = Vector6d::Zero(); + Vector6d bias_groundtruth = Vector6d::Zero(); + Vector6d bias_initial_guess = Vector6d::Random()*0.01; + + TestStatic(body_magnitudes, bias_groundtruth, bias_initial_guess, "static_bias_zero_initial_guess_random", 3, false); +} + + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/gtest_imu_tools.cpp b/test/gtest_imu_tools.cpp index c2e8306ce5b7d0c6cacafc1ac30f8be0474cf0e9..4053b15f912ef7e5cb4b39181e7a5b81395d0c6a 100644 --- a/test/gtest_imu_tools.cpp +++ b/test/gtest_imu_tools.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu_tools.cpp * diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp index 4099cf133f000dff71b7eb890c22a9c25c529605..6255fa38517be1218fcd6e39647c18e2151974c6 100644 --- a/test/gtest_processor_imu.cpp +++ b/test/gtest_processor_imu.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu.cpp * @@ -66,7 +87,7 @@ class ProcessorImut : public testing::Test x0.resize(10); // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.) - cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero()); + cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero().eval()); } void TearDown() override @@ -144,7 +165,7 @@ TEST(ProcessorImu, voteForKeyFrame) TimeStamp t(0); VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0, s0, t, 0.01); + problem->setPriorFactor(x0, s0, t); //data variable and covariance matrix // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions @@ -154,7 +175,7 @@ TEST(ProcessorImu, voteForKeyFrame) data_cov(0,0) = 0.5; // Create the captureImu to store the Imu data arriving from (sensor / callback / file / etc.) - std::shared_ptr<wolf::CaptureImu> cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero()); + std::shared_ptr<wolf::CaptureImu> cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero().eval()); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->process(); @@ -212,7 +233,7 @@ TEST_F(ProcessorImut, acc_x) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -243,7 +264,7 @@ TEST_F(ProcessorImut, acc_y) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -285,7 +306,7 @@ TEST_F(ProcessorImut, acc_z) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -327,7 +348,7 @@ TEST_F(ProcessorImut, check_Covariance) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -353,7 +374,7 @@ TEST_F(ProcessorImut, gyro_x) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -410,7 +431,7 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx) // init things x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -468,7 +489,7 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -524,7 +545,7 @@ TEST_F(ProcessorImut, gyro_z) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -567,7 +588,7 @@ TEST_F(ProcessorImut, gyro_xyz) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -659,7 +680,7 @@ TEST_F(ProcessorImut, gyro_z_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -703,7 +724,7 @@ TEST_F(ProcessorImut, gyro_x_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -752,7 +773,7 @@ TEST_F(ProcessorImut, gyro_xy_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -801,7 +822,7 @@ TEST_F(ProcessorImut, gyro_y_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -849,7 +870,7 @@ TEST_F(ProcessorImut, gyro_xyz_ConstVelocity) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -943,7 +964,7 @@ TEST_F(ProcessorImut, gyro_x_acc_x) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -997,7 +1018,7 @@ TEST_F(ProcessorImut, gyro_y_acc_y) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -1051,7 +1072,7 @@ TEST_F(ProcessorImut, gyro_z_acc_z) MatrixXd P0(9,9); P0.setIdentity(); x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t, 0.01); + problem->setPriorFactor(x0c, s0c, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); diff --git a/test/gtest_processor_imu2d.cpp b/test/gtest_processor_imu2d.cpp new file mode 100644 index 0000000000000000000000000000000000000000..87c3619d3fbe37425bfc62bb6c0e2b17cfc40d9c --- /dev/null +++ b/test/gtest_processor_imu2d.cpp @@ -0,0 +1,264 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu2d.cpp + * + * Created on: Nov 24, 2020 + * \author: igeer + */ + +#include "imu/internal/config.h" +#include "imu/capture/capture_imu.h" +#include "imu/processor/processor_imu2d.h" +#include "imu/sensor/sensor_imu2d.h" + +// #include "core/common/wolf.h" + +#include <core/utils/utils_gtest.h> +#include "core/utils/logging.h" + +#include "core/math/rotations.h" +#include <cmath> +#include <iostream> + +using namespace Eigen; +using namespace wolf; + +class ProcessorImu2dTest : public testing::Test +{ + + public: //These can be accessed in fixtures + wolf::ProblemPtr problem; + wolf::SensorBasePtr sensor_ptr; + wolf::ProcessorMotionPtr processor_motion; + wolf::TimeStamp t; + wolf::TimeStamp t0; + double mti_clock, tmp; + double dt; + Vector6d data; + Matrix6d data_cov; + VectorComposite x0c; // initial state composite + VectorComposite s0c; // initial sigmas composite + std::shared_ptr<wolf::CaptureImu> C; + + //a new of this will be created for each new test + void SetUp() override + { + using namespace Eigen; + using std::shared_ptr; + using std::make_shared; + using std::static_pointer_cast; + using namespace wolf::Constants; + + std::string wolf_root = _WOLF_IMU_ROOT_DIR; + + // Wolf problem + problem = Problem::create("POV", 2); + Vector3d extrinsics = (Vector3d() << 0,0, 0).finished(); + sensor_ptr = problem->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu2d.yaml"); + ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu2d.yaml"); + processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr); + + // Time and data variables + dt = 0.01; + t0.set(0); + t = t0; + data = Vector6d::Zero(); + data_cov = Matrix6d::Identity(); + + // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.) + C = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector3d::Zero()); + } + +}; + +TEST(ProcessorImu2d_constructors, ALL) +{ + //constructor with ProcessorImu2dParamsPtr argument only + ParamsProcessorImu2dPtr param_ptr = std::make_shared<ParamsProcessorImu2d>(); + param_ptr->max_time_span = 2.0; + param_ptr->max_buff_length = 20000; + param_ptr->dist_traveled = 2.0; + param_ptr->angle_turned = 2.0; + + ProcessorImu2dPtr prc1 = std::make_shared<ProcessorImu2d>(param_ptr); + ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span); + ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length); + ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled); + ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned); + + //Factory constructor with pointers + std::string wolf_root = _WOLF_IMU_ROOT_DIR; + ProblemPtr problem = Problem::create("POV", 2); + Vector3d extrinsics = (Vector3d()<<1,0, 0).finished(); + std::cout << "creating sensor_ptr" << std::endl; + SensorBasePtr sensor_ptr = problem->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu2d.yaml"); + std::cout << "created sensor_ptr" << std::endl; + ParamsProcessorImu2dPtr params_default = std::make_shared<ParamsProcessorImu2d>(); + ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", sensor_ptr, params_default); + ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(), params_default->max_time_span); + ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length); + ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(), params_default->dist_traveled); + ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getAngleTurned(), params_default->angle_turned); + + //Factory constructor with yaml + processor_ptr = problem->installProcessor("ProcessorImu2d", "Sec Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu2d.yaml"); + ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(), 2.0); + ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), 20000); + ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(), 2.0); + ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getAngleTurned(), 0.2); +} + +TEST_F(ProcessorImu2dTest, Prior) +{ + // Set the origin + x0c['P'] = Vector2d(1,2); + x0c['O'] = Vector1d(0); + x0c['V'] = Vector2d(4,5); + auto KF0 = problem->setPriorFix(x0c, t0); + processor_motion->setOrigin(KF0); + //WOLF_DEBUG("x0 =", x0c); + //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t)->getState("POV")); +} + +TEST_F(ProcessorImu2dTest, MRUA) +{ + data << 1, 0, 0, 0, 0, 0; + data_cov *= 1e-3; + // Set the origin + x0c['P'] = Vector2d(1,2); + x0c['O'] = Vector1d(0); + x0c['V'] = Vector2d(4,5); + auto KF0 = problem->setPriorFix(x0c, t0); + processor_motion->setOrigin(KF0); + + //WOLF_DEBUG("Current State: ", problem->getState()); + for(t = t0+dt; t <= t0 + 1.0 + dt/2; t+=dt){ + C->setTimeStamp(t); + C->setData(data); + C->setDataCovariance(data_cov); + C->process(); + //WOLF_DEBUG("Current State: ", problem->getState()['P'].transpose()); + //WOLF_DEBUG((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose()); + ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2), problem->getState()['P'], 1e-9); + } +} + +TEST_F(ProcessorImu2dTest, parabola) +{ + double v0 = 10; + double a = 1; + + Vector2d pos; + // Set the origin + x0c['P'] = Vector2d(0, 0); + x0c['O'] = Vector1d(0); + x0c['V'] = Vector2d(v0, 0); + + data_cov *= 1e-3; + auto KF0 = problem->setPriorFix(x0c, t0); + processor_motion->setOrigin(KF0); + + for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ + C->setTimeStamp(t); + data << 0, a, 0, 0, 0, 0; + C->setData(data); + C->setDataCovariance(data_cov); + C->process(); + //WOLF_DEBUG("Current State: ", problem->getState()); + pos << v0*(t-t0), + 0.5*a*std::pow(t-t0, 2); + //WOLF_DEBUG("Calculated State: ", pos.transpose()); + EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9); + EXPECT_MATRIX_APPROX(Vector2d(v0, a*(t-t0)), problem->getState()['V'], 1e-9); + } +} + +TEST_F(ProcessorImu2dTest, parabola_deluxe) +{ + Vector2d v0(13, 56); + Vector2d a(1, 2); + + Vector2d pos; + // Set the origin + x0c['P'] = Vector2d(0, 0); + x0c['O'] = Vector1d(0); + x0c['V'] = v0; + + data_cov *= 1e-3; + auto KF0 = problem->setPriorFix(x0c, t0); + processor_motion->setOrigin(KF0); + + for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ + C->setTimeStamp(t); + data << a(0), a(1), 0, 0, 0, 0; + C->setData(data); + C->setDataCovariance(data_cov); + C->process(); + //WOLF_DEBUG("Current State: ", problem->getState()); + pos = v0*(t-t0) + 0.5*a*std::pow(t-t0, 2); + //WOLF_DEBUG("Calculated State: ", pos.transpose()); + EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9); + EXPECT_MATRIX_APPROX(v0 + a*(t-t0), problem->getState()['V'], 1e-9); + } +} + +TEST_F(ProcessorImu2dTest, Circular_move) +{ + double pi = 3.14159265358993; + double r = 1; + double w = 1; + double alpha = pi/4.; + Vector2d pos; + // Set the origin + x0c['P'] = Vector2d(r, 0); + pos = x0c['P']; + x0c['O'] = Vector1d(alpha); + x0c['V'] = Vector2d(0, w*r); + + data_cov *= 1e-3; + //dt = 0.0001; + auto KF0 = problem->setPriorFix(x0c, t0); + processor_motion->setOrigin(KF0); + + WOLF_DEBUG("Current State: ", problem->getState()); + for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ + C->setTimeStamp(t); + data << -std::cos(alpha)*w*w*r, std::sin(alpha)*w*w*r, 0, 0, 0, w; + C->setData(data); + C->setDataCovariance(data_cov); + C->process(); + WOLF_DEBUG("Current State: ", problem->getState()); + pos << r*std::cos( w*(t-t0) ), + r*std::sin( w*(t-t0) ); + WOLF_DEBUG("Calculated State: ", pos.transpose()); + EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + //::testing::GTEST_FLAG(filter) = "ProcessorImu2dt.check_Covariance"; + return RUN_ALL_TESTS(); +} + diff --git a/test/gtest_processor_imu_jacobians.cpp b/test/gtest_processor_imu_jacobians.cpp index bc4ff1533c993e8be1b530433527a2e2cb7783d2..929b92569f363af49e92a39a28428f23df500e10 100644 --- a/test/gtest_processor_imu_jacobians.cpp +++ b/test/gtest_processor_imu_jacobians.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_imu_preintegration_jacobians.cpp * diff --git a/test/gtest_processor_motion_intrinsics_update.cpp b/test/gtest_processor_motion_intrinsics_update.cpp index 4dbac962133c7d8da9f76e152a09e0749c1f2e71..93ba8561b5b4fea55e814cc9d3b68501ae2439ac 100644 --- a/test/gtest_processor_motion_intrinsics_update.cpp +++ b/test/gtest_processor_motion_intrinsics_update.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 <fstream> #include "imu/internal/config.h" @@ -77,7 +98,7 @@ class ProcessorImuTest : public testing::Test params_->time_tolerance = 0.0025; processor_ = std::static_pointer_cast<ProcessorImu>(problem_->installProcessor("ProcessorImu", "processor imu", sensor_, params_)); - KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0, 0.01); + KF0_ = problem_->setPriorFactor(x_origin, std_origin, 0); Vector6d bias; bias << 0.42,0,0, 0,0,0; // Vector6d bias; bias << 0.0,0,0, 0.42,0,0; @@ -155,7 +176,7 @@ TEST_F(ProcessorImuTest, test1) * SET TO TRUE TO PRODUCE CSV FILE * SET TO FALSE TO STOP PRODUCING CSV FILE */ -#define WRITE_CSV_FILE false +#define WRITE_CSV_FILE true TEST_F(ProcessorImuTest, getState) { diff --git a/test/gtest_sensor_compass.cpp b/test/gtest_sensor_compass.cpp index 8b404d2ad88ea63c0d6a6beac32f2a7b31377299..0200128bad102e5b4c50015a4428050bd957d5a7 100644 --- a/test/gtest_sensor_compass.cpp +++ b/test/gtest_sensor_compass.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 "imu/internal/config.h" diff --git a/test/processor_imu2d_UnitTester.cpp b/test/processor_imu2d_UnitTester.cpp new file mode 100644 index 0000000000000000000000000000000000000000..3412389aa7c251d37aa1f98afe3891e9b9b7374e --- /dev/null +++ b/test/processor_imu2d_UnitTester.cpp @@ -0,0 +1,34 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- +#include "processor_imu2d_UnitTester.h" + +namespace wolf { + +ProcessorImu2d_UnitTester::ProcessorImu2d_UnitTester() : + ProcessorImu2d(std::make_shared<ParamsProcessorImu2d>()), + Dq_out_(nullptr) +{} + +ProcessorImu2d_UnitTester::~ProcessorImu2d_UnitTester(){} + +} // namespace wolf + diff --git a/test/processor_imu2d_UnitTester.h b/test/processor_imu2d_UnitTester.h new file mode 100644 index 0000000000000000000000000000000000000000..8dd65887a91947890e6f83aa48c797f5c39cd522 --- /dev/null +++ b/test/processor_imu2d_UnitTester.h @@ -0,0 +1,400 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU2D_UNITTESTER_H +#define PROCESSOR_IMU2D_UNITTESTER_H + +// Wolf +#include "imu/processor/processor_imu2d.h" +#include "core/processor/processor_motion.h" + +namespace wolf { + struct Imu_jac_bias{ //struct used for checking jacobians by finite difference + + Imu_jac_bias(Eigen::Matrix<Eigen::VectorXd,6,1> _Deltas_noisy_vect, + Eigen::VectorXd _Delta0 , + Eigen::Matrix3d _dDp_dab, + Eigen::Matrix3d _dDv_dab, + Eigen::Matrix3d _dDp_dwb, + Eigen::Matrix3d _dDv_dwb, + Eigen::Matrix3d _dDq_dwb) : + Deltas_noisy_vect_(_Deltas_noisy_vect), + Delta0_(_Delta0) , + dDp_dab_(_dDp_dab), + dDv_dab_(_dDv_dab), + dDp_dwb_(_dDp_dwb), + dDv_dwb_(_dDv_dwb), + dDq_dwb_(_dDq_dwb) + { + // + } + + Imu_jac_bias(){ + + for (int i=0; i<6; i++){ + Deltas_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1); + } + + Delta0_ = Eigen::VectorXd::Zero(1,1); + dDp_dab_ = Eigen::Matrix3d::Zero(); + dDv_dab_ = Eigen::Matrix3d::Zero(); + dDp_dwb_ = Eigen::Matrix3d::Zero(); + dDv_dwb_ = Eigen::Matrix3d::Zero(); + dDq_dwb_ = Eigen::Matrix3d::Zero(); + } + + Imu_jac_bias(Imu_jac_bias const & toCopy){ + + Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_; + Delta0_ = toCopy.Delta0_; + dDp_dab_ = toCopy.dDp_dab_; + dDv_dab_ = toCopy.dDv_dab_; + dDp_dwb_ = toCopy.dDp_dwb_; + dDv_dwb_ = toCopy.dDv_dwb_; + dDq_dwb_ = toCopy.dDq_dwb_; + } + + public: + /*The following vectors will contain all the matrices and deltas needed to compute the finite differences. + place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data + place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data + */ + Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect_; + Eigen::VectorXd Delta0_; + Eigen::Matrix3d dDp_dab_; + Eigen::Matrix3d dDv_dab_; + Eigen::Matrix3d dDp_dwb_; + Eigen::Matrix3d dDv_dwb_; + Eigen::Matrix3d dDq_dwb_; + + public: + void copyfrom(Imu_jac_bias const& right){ + + Deltas_noisy_vect_ = right.Deltas_noisy_vect_; + Delta0_ = right.Delta0_; + dDp_dab_ = right.dDp_dab_; + dDv_dab_ = right.dDv_dab_; + dDp_dwb_ = right.dDp_dwb_; + dDv_dwb_ = right.dDv_dwb_; + dDq_dwb_ = right.dDq_dwb_; + } + }; + + struct Imu_jac_deltas{ + + Imu_jac_deltas(Eigen::VectorXd _Delta0, + Eigen::VectorXd _delta0, + Eigen::Matrix<Eigen::VectorXd,9,1> _Delta_noisy_vect, + Eigen::Matrix<Eigen::VectorXd,9,1> _delta_noisy_vect, + Eigen::MatrixXd _jacobian_delta_preint, + Eigen::MatrixXd _jacobian_delta ) : + Delta0_(_Delta0), + delta0_(_delta0), + Delta_noisy_vect_(_Delta_noisy_vect), + delta_noisy_vect_(_delta_noisy_vect), + jacobian_delta_preint_(_jacobian_delta_preint), + jacobian_delta_(_jacobian_delta) + { + // + } + + Imu_jac_deltas(){ + for (int i=0; i<9; i++){ + Delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1); + delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1); + } + + Delta0_ = Eigen::VectorXd::Zero(1,1); + delta0_ = Eigen::VectorXd::Zero(1,1); + jacobian_delta_preint_ = Eigen::MatrixXd::Zero(9,9); + jacobian_delta_ = Eigen::MatrixXd::Zero(9,9); + } + + Imu_jac_deltas(Imu_jac_deltas const & toCopy){ + + Delta_noisy_vect_ = toCopy.Delta_noisy_vect_; + delta_noisy_vect_ = toCopy.delta_noisy_vect_; + + Delta0_ = toCopy.Delta0_; + delta0_ = toCopy.delta0_; + jacobian_delta_preint_ = toCopy.jacobian_delta_preint_; + jacobian_delta_ = toCopy.jacobian_delta_; + } + + public: + /*The following vectors will contain all the matrices and deltas needed to compute the finite differences. + Elements at place 0 are those not affected by the bias noise that we add (Delta_noise, delta_noise -> dPx, dpx, dVx, dvx,..., dOz,doz). + Delta_noisy_vect_ delta_noisy_vect_ + 0: + 0, 0: + 0 + 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2: +dpy, 3: +dpz + 4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz + 7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 9: +dvy, +: +dvz + */ + Eigen::VectorXd Delta0_; //this will contain the Delta not affected by noise + Eigen::VectorXd delta0_; //this will contain the delta not affected by noise + Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect_; //this will contain the Deltas affected by noises + Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect_; //this will contain the deltas affected by noises + Eigen::MatrixXd jacobian_delta_preint_; + Eigen::MatrixXd jacobian_delta_; + + public: + void copyfrom(Imu_jac_deltas const& right){ + + Delta_noisy_vect_ = right.Delta_noisy_vect_; + delta_noisy_vect_ = right.delta_noisy_vect_; + Delta0_ = right.Delta0_; + delta0_ = right.delta0_; + jacobian_delta_preint_ = right.jacobian_delta_preint_; + jacobian_delta_ = right.jacobian_delta_; + } + }; + + class ProcessorImu2d_UnitTester : public ProcessorImu2d{ + + public: + + ProcessorImu2d_UnitTester(); + ~ProcessorImu2d_UnitTester() override; + + //Functions to test jacobians with finite difference method + + /* params : + _data : input data vector (size 6 : ax,ay,az,wx,wy,wz) + _dt : time interval between 2 Imu measurements + da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one. + */ + Imu_jac_bias finite_diff_ab(const double _dt, + Eigen::Vector6d& _data, + const double& da_b, + const Eigen::Matrix<double,10,1>& _delta_preint0); + + /* params : + _data : input data vector (size 6 : ax,ay,az,wx,wy,wz) + _dt : time interval between 2 Imu measurements + _Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix())) + _delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix())) + */ + Imu_jac_deltas finite_diff_noise(const double& _dt, + Eigen::Vector6d& _data, + const Eigen::Matrix<double,9,1>& _Delta_noise, + const Eigen::Matrix<double,9,1>& _delta_noise, + const Eigen::Matrix<double,10,1>& _Delta0); + + public: + static ProcessorBasePtr create(const std::string& _unique_name, + const ParamsProcessorBasePtr _params, + const SensorBasePtr = nullptr); + + public: + // Maps quat, to be used as temporary + Eigen::Map<Eigen::Quaterniond> Dq_out_; + + }; + +} + +///////////////////////////////////////////////////////// +// IMPLEMENTATION. Put your implementation includes here +///////////////////////////////////////////////////////// + +// Wolf +#include "core/state_block/state_block.h" +#include "core/math/rotations.h" + +namespace wolf{ + + //Functions to test jacobians with finite difference method +inline Imu_jac_bias ProcessorImu2d_UnitTester::finite_diff_ab(const double _dt, + Eigen::Vector6d& _data, + const double& da_b, + const Eigen::Matrix<double,10,1>& _delta_preint0) +{ + //TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything + ///Define all the needed variables + Eigen::VectorXd Delta0; + Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect; + Eigen::Vector6d data0; + data0 = _data; + + Eigen::MatrixXd data_cov; + Eigen::MatrixXd jacobian_delta_preint; + Eigen::MatrixXd jacobian_delta; + Eigen::VectorXd delta_preint_plus_delta0; + data_cov.resize(6,6); + jacobian_delta_preint.resize(9,9); + jacobian_delta.resize(9,9); + delta_preint_plus_delta0.resize(10); + + //set all variables to 0 + data_cov = Eigen::MatrixXd::Zero(6,6); + jacobian_delta_preint = Eigen::MatrixXd::Zero(9,9); + jacobian_delta = Eigen::MatrixXd::Zero(9,9); + delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV + + Vector6d bias = Vector6d::Zero(); + + /*The following vectors will contain all the matrices and deltas needed to compute the finite differences. + place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data + place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data + */ + + Eigen::Matrix3d dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb; + + //Deltas without use of da_b + computeCurrentDelta(_data, data_cov, bias, _dt,delta_,delta_cov_,jacobian_delta_calib_); + deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta); + MatrixXd jacobian_bias = jacobian_delta * jacobian_delta_calib_; + Delta0 = delta_preint_plus_delta0; //this is the first preintegrated delta, not affected by any added bias noise + dDp_dab = jacobian_bias.block(0,0,3,3); + dDp_dwb = jacobian_bias.block(0,3,3,3); + dDq_dwb = jacobian_bias.block(3,3,3,3); + dDv_dab = jacobian_bias.block(6,0,3,3); + dDv_dwb = jacobian_bias.block(6,3,3,3); + + + // propagate bias noise + for(int i=0; i<6; i++){ + //need to reset stuff here + delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV + data_cov = Eigen::MatrixXd::Zero(6,6); + + // add da_b + _data = data0; + _data(i) = _data(i) - da_b; //- because a = a_m − a_b + a_n, in out case, a = a_m − a_b - da_b + a_n + //data2delta + computeCurrentDelta(_data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_); + deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta); + Deltas_noisy_vect(i) = delta_preint_plus_delta0; //preintegrated deltas affected by added bias noise + } + + Imu_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb); + return bias_jacobians; +} + +inline Imu_jac_deltas ProcessorImu2d_UnitTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0) +{ + //we do not propagate any noise from data vector + Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise + Eigen::VectorXd delta0; //will contain the delta not affected by added noise + // delta_ that /will contain the delta affected by added noise is declared in processor_motion.h + Eigen::VectorXd delta_preint_plus_delta; + delta0.resize(10); + delta_preint_plus_delta.resize(10); + delta_preint_plus_delta << 0,0,0 ,0,0,0,1 ,0,0,0; + + Eigen::MatrixXd jacobian_delta_preint; //will be used as input for deltaPlusDelta + Eigen::MatrixXd jacobian_delta; //will be used as input for deltaPlusDelta + jacobian_delta_preint.resize(9,9); + jacobian_delta.resize(9,9); + jacobian_delta_preint.setIdentity(9,9); + jacobian_delta.setIdentity(9,9); + Eigen::MatrixXd jacobian_delta_preint0; //will contain the jacobian we want to check + Eigen::MatrixXd jacobian_delta0; //will contain the jacobian we want to check + jacobian_delta_preint0.resize(9,9); + jacobian_delta0.resize(9,9); + jacobian_delta_preint0.setIdentity(9,9); + jacobian_delta0.setIdentity(9,9); + + Eigen::MatrixXd data_cov; //will be used filled with Zeros as input for data2delta + data_cov.resize(6,6); + data_cov = Eigen::MatrixXd::Zero(6,6); + + Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect; //this will contain the Deltas affected by noises + Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect; //this will contain the deltas affected by noises + + Vector6d bias = Vector6d::Zero(); + + computeCurrentDelta(_data, data_cov, bias,_dt,delta_,delta_cov_,jacobian_delta_calib_); //Affects dp_out, dv_out and dq_out + delta0 = delta_; //save the delta that is not affected by noise + deltaPlusDelta(_Delta0, delta0, _dt, delta_preint_plus_delta, jacobian_delta_preint, jacobian_delta); + jacobian_delta_preint0 = jacobian_delta_preint; + jacobian_delta0 = jacobian_delta; + + //We compute all the jacobians wrt deltas and noisy deltas + for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space + { + //PQV formulation + //Add perturbation in position + delta_ = delta0; + delta_(i) = delta_(i) + _delta_noise(i); //noise has been added + delta_noisy_vect(i) = delta_; + + //Add perturbation in velocity + /* + delta_ is size 10 (P Q V), _delta_noise is size 9 (P O V) + */ + delta_ = delta0; + delta_(i+7) = delta_(i+7) + _delta_noise(i+6); //noise has been added + delta_noisy_vect(i+6) = delta_; + } + + for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions + { + //PQV formulation + //fist we need to reset some stuff + Eigen::Vector3d dtheta = Eigen::Vector3d::Zero(); + + delta_ = delta0; + new (&Dq_out_) Map<Quaterniond>(delta_.data() + 3); //not sure that we need this + dtheta(i) += _delta_noise(i+3); //introduce perturbation + Dq_out_ = Dq_out_ * v2q(dtheta); + delta_noisy_vect(i+3) = delta_; + } + + //We compute all the jacobians wrt Deltas and noisy Deltas + for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space + { + //PQV formulation + //Add perturbation in position + Delta_ = _Delta0; + Delta_(i) = Delta_(i) + _Delta_noise(i); //noise has been added + Delta_noisy_vect(i) = Delta_; + + //Add perturbation in velocity + /* + Delta_ is size 10 (P Q V), _Delta_noise is size 9 (P O V) + */ + Delta_ = _Delta0; + Delta_(i+7) = Delta_(i+7) + _Delta_noise(i+6); //noise has been added + Delta_noisy_vect(i+6) = Delta_; + } + + for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions + { + //fist we need to reset some stuff + Eigen::Vector3d dtheta = Eigen::Vector3d::Zero(); + + Delta_ = _Delta0; + new (&Dq_out_) Map<Quaterniond>(Delta_.data() + 3); + dtheta(i) += _Delta_noise(i+3); //introduce perturbation + Dq_out_ = Dq_out_ * v2q(dtheta); + Delta_noisy_vect(i+3) = Delta_; + } + + Imu_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0); + return jac_deltas; + +} + +} // namespace wolf + +#endif // PROCESSOR_Imu_UNITTESTER_H diff --git a/test/processor_imu_UnitTester.cpp b/test/processor_imu_UnitTester.cpp index 71b68efe982fa4c635b1bbf4f068a2101e07fc70..8b6f10f272a0923c67b709ba3676a4b21c95719b 100644 --- a/test/processor_imu_UnitTester.cpp +++ b/test/processor_imu_UnitTester.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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 "processor_imu_UnitTester.h" namespace wolf { diff --git a/test/processor_imu_UnitTester.h b/test/processor_imu_UnitTester.h index f9990707cb56f5ec6d1357683aa749503343ca1b..398b69bc28c4be496b3d742750621b4b5e18ea90 100644 --- a/test/processor_imu_UnitTester.h +++ b/test/processor_imu_UnitTester.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have 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_IMU_UNITTESTER_H #define PROCESSOR_IMU_UNITTESTER_H @@ -108,16 +129,16 @@ namespace wolf { jacobian_delta_ = Eigen::MatrixXd::Zero(9,9); } - Imu_jac_deltas(Imu_jac_deltas const & toCopy){ - - Delta_noisy_vect_ = toCopy.Delta_noisy_vect_; - delta_noisy_vect_ = toCopy.delta_noisy_vect_; - - Delta0_ = toCopy.Delta0_; - delta0_ = toCopy.delta0_; - jacobian_delta_preint_ = toCopy.jacobian_delta_preint_; - jacobian_delta_ = toCopy.jacobian_delta_; - } +// Imu_jac_deltas(Imu_jac_deltas const & toCopy){ +// +// Delta_noisy_vect_ = toCopy.Delta_noisy_vect_; +// delta_noisy_vect_ = toCopy.delta_noisy_vect_; +// +// Delta0_ = toCopy.Delta0_; +// delta0_ = toCopy.delta0_; +// jacobian_delta_preint_ = toCopy.jacobian_delta_preint_; +// jacobian_delta_ = toCopy.jacobian_delta_; +// } public: /*The following vectors will contain all the matrices and deltas needed to compute the finite differences. diff --git a/test/yaml/imu2d_static_init.yaml b/test/yaml/imu2d_static_init.yaml new file mode 100644 index 0000000000000000000000000000000000000000..4978c8e2863908df72a336da1225667411d3022a --- /dev/null +++ b/test/yaml/imu2d_static_init.yaml @@ -0,0 +1,12 @@ +type: "ProcessorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +time_tolerance: 0.0025 # Time tolerance for joining KFs +unmeasured_perturbation_std: 0.0001 + +keyframe_vote: + voting_active: true + voting_aux_active: false + max_time_span: 0.9999 # seconds + max_buff_length: 1000000000 # motion deltas + dist_traveled: 100000000000 # meters + angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg) diff --git a/test/yaml/imu_static_init.yaml b/test/yaml/imu_static_init.yaml new file mode 100644 index 0000000000000000000000000000000000000000..46393f30b8f24bf645bfb1272764e6c8915e5dc2 --- /dev/null +++ b/test/yaml/imu_static_init.yaml @@ -0,0 +1,12 @@ +type: "ProcessorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +time_tolerance: 0.0025 # Time tolerance for joining KFs +unmeasured_perturbation_std: 0.0001 + +keyframe_vote: + voting_active: true + voting_aux_active: false + max_time_span: 0.9999 # seconds + max_buff_length: 1000000000 # motion deltas + dist_traveled: 100000000000 # meters + angle_turned: 10000000000 # radians (1 rad approx 57 deg, approx 60 deg) diff --git a/test/yaml/sensor_imu.yaml b/test/yaml/sensor_imu.yaml new file mode 100644 index 0000000000000000000000000000000000000000..3c78a00d35c785fe73381d8f6ce99a705d27ce77 --- /dev/null +++ b/test/yaml/sensor_imu.yaml @@ -0,0 +1,9 @@ +type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +motion_variances: + a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2 + w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec + ab_initial_stdev: 0.800 # m/s2 - initial bias + wb_initial_stdev: 0.350 # rad/sec - initial bias + ab_rate_stdev: 0.1 # m/s2/sqrt(s) + wb_rate_stdev: 0.0400 # rad/s/sqrt(s) diff --git a/test/yaml/sensor_imu2d_static_init.yaml b/test/yaml/sensor_imu2d_static_init.yaml new file mode 100644 index 0000000000000000000000000000000000000000..ce810d4f5f397a7bcb8647e086c026b125fbc936 --- /dev/null +++ b/test/yaml/sensor_imu2d_static_init.yaml @@ -0,0 +1,9 @@ +type: "SensorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +motion_variances: + a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2 + w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec + ab_initial_stdev: 0.800 # m/s2 - initial bias + wb_initial_stdev: 0.350 # rad/sec - initial bias + ab_rate_stdev: 0.1 # m/s2/sqrt(s) + wb_rate_stdev: 0.0400 # rad/s/sqrt(s) diff --git a/test/yaml/sensor_imu_static_init.yaml b/test/yaml/sensor_imu_static_init.yaml new file mode 100644 index 0000000000000000000000000000000000000000..700b8d6762b91b38e96391f6032b2c7c4221eabc --- /dev/null +++ b/test/yaml/sensor_imu_static_init.yaml @@ -0,0 +1,9 @@ +type: "SensorImu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. + +motion_variances: + a_noise: 0.001 # standard deviation of Acceleration noise (same for all the axis) in m/s2 + w_noise: 0.001 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec + ab_initial_stdev: 0.001 # m/s2 - initial bias + wb_initial_stdev: 0.001 # rad/sec - initial bias + ab_rate_stdev: 0.001 # m/s2/sqrt(s) + wb_rate_stdev: 0.001 # rad/s/sqrt(s)